function y = DSIM(M,C,K,B,F,G,D,id,u,t,x0,w0)
% Discrete-time simulation of second-order models.
% 
%   The discrete-time equations are 
%       M q[k+2] + C q[k+1] + K q[k] = B [u[k] ; w[k]]
%       y[k] = F1 q[k] + G1 q[k+1] + D1 [u[k] ; w[k]]
%       z[k] = F2 q[k] + G2 q[k+1] + D2 [u[k] ; w[k]]
%       w[k] = z[k-ID]
%   where ID is the vector of integer-valued internal delays (ID>0)

%  Copyright 2020 The MathWorks, Inc.
Ns = numel(t);
nq = size(K,1);
nfd = numel(id);
if isempty(x0)
   q0 = zeros(nq,1); q1 = q0;
else
   q0 = x0(1:nq,:); q1 = x0(nq+1:2*nq);
end
u0 = u(:,1);
EXPLICIT = isempty(M);

if EXPLICIT || (all(any(M,1)) && all(any(M,2)))
   % M=I or M has no zero rows/columns
   if ~EXPLICIT
      SOLVER = ltipack.spssdata.linearSolver(M);
   end
   
   if nfd==0
      % No internal delays
      y = zeros(size(D,1),Ns);
      qk = q0;  qkp1 = q1;
      uk = u0;
      y(:,1) = F*qk+G*qkp1+D*uk;
      for k=2:Ns
         if EXPLICIT
            aux = B*uk-C*qkp1-K*qk;
         else
            aux = SOLVER(B*uk-C*qkp1-K*qk);  % q[k+2]
         end
         qk = qkp1;  qkp1 = aux;  uk = u(:,k);
         y(:,k) = F*qk+G*qkp1+D*uk;
      end
   else
      nfd = numel(id);
      ny = size(D,1)-nfd;
      F1 = F(1:ny,:);  G1 = G(1:ny,:);  D1 = D(1:ny,:);
      F2 = F(ny+1:ny+nfd,:);  G2 = G(ny+1:ny+nfd,:);  D2 = D(ny+1:ny+nfd,:);
      % Circulant delay buffers ZBUF keeps track of z[k-m] for m=1:IDMAX.
      idmax = max(id);  % max delay
      zbuf = repmat(w0,[1 idmax]);
      % Step forward
      ioffset = (1:nfd)';
      y = zeros(ny,Ns);
      qk = q0;  qkp1 = q1;
      uwk = [u0;w0];
      y(:,1) = F1*qk+G1*qkp1+D1*uwk;
      zbuf(:,1) = F2*qk+G2*qkp1+D2*uwk;
      for k=2:Ns
         if EXPLICIT
            aux = B*uwk-C*qkp1-K*qk;
         else
            aux = SOLVER(B*uwk-C*qkp1-K*qk);  % q[k+2]
         end
         qk = qkp1;  qkp1 = aux;
         % Read w[k] from Z buffer
         ix = ioffset + mod(k-id-1,idmax) * nfd;
         uwk = [u(:,k) ; zbuf(ix)];
         % Compute and store z[k]
         jz = mod(k-1,idmax)+1;
         zbuf(:,jz) = F2*qk+G2*qkp1+D2*uwk;
         y(:,k) = F1*qk+G1*qkp1+D1*uwk;
      end
   end      

else
   % Structural DAE (M has zero rows/columns).
   % Try regularizing M by eliminating redundant part of q[k+1] and 
   % time-shifting equations with no q[k+2] term (zero rows of M). This
   % is equivalent to converting to SPARSS but does not increase complexity
   % because E = blkdiag(I,EE) with size(EE)=size(M)
   nuw = size(B,2);

   % Step1: Partition
   %   M = [M1 0],  G = [G1 0],  q = [q1;q2], C = [C1 C2], K = [K1 K2]
   % and rewrite the recursion as
   %   [I 0 0;0 C2 M1] x[k+1] = [0 0 I;-K1 -K2 -C1] x[k] + [0;B] u[k]
   % where x[k] is the reduced state: 
   %   x[k] = [q1[k] ; q2[k] ; q1[k+1]]
   boo = any(M,1) | any(G,1);
   j1 = find(boo);  j2 = find(~boo);  % M1 = M(:,j1)
   nq1 = numel(j1);
   E = blkdiag(speye(nq1),[C(:,j2) M(:,j1)]);
   A = [sparse(nq1,nq) speye(nq1) ; -K(:,[j1 j2]) -C(:,j1)];
   B = [sparse(nq1,nuw) ; B];
   Bp1 = sparse(nq+nq1,nuw);  
   C = [F(:,[j1 j2]) G(:,j1)];
   x0 = [q0([j1 j2],:) ; q1(j1)];
   
   % Step 2: Further partition
   %   [C2 M1] = [C12 M11;0 0]  % C22=0, M21=0
   % and regularize to
   %    [  I   0    0  ]          [   0    0    I  ]        [    0     ]
   %    [  0  C12  M11 ] x[k+1] = [ -K11 -K12 -C11 ] x[k] + [  B1 u[k] ]
   %    [  0  K22  C21 ]          [   0    0  -K21 ]        [ B2 u[k+1]]
   % to obtain a well-posed recursion. 
   i2 = find(~any(E,2));  % zero rows
   if ~isempty(i2)
      % Find consistent IC 
      x0 = localCIC(x0,A,E,A*x0+B*[u0;w0]);
      % Regularize E matrix
      E(i2,nq1+1:nq1+nq) = -A(i2,nq1+1:nq1+nq);
      A(i2,nq+1:nq+nq1) = A(i2,1:nq1);  A(i2,1:nq) = 0;
      Bp1(i2,:) = B(i2,:);  B(i2,:) = 0;
   end
   
   % Factor E
   SOLVER = ltipack.spssdata.linearSolver(E);
   
   % Simulate
   if nfd==0
      % INITIAL and STEP/IMPULSE/LSIM without internal delays
      y = zeros(size(D,1),Ns);
      xk = x0;
      uk = u0;
      y(:,1) = C*xk+D*uk;
      ukp1 = u(:,min(2,Ns));
      for k=2:Ns
         xk = SOLVER(A*xk+B*uk+Bp1*ukp1);
         uk = ukp1;  ukp1 = u(:,min(k+1,Ns));
         y(:,k) = C*xk+D*uk;
      end
   else
      ny = size(D,1)-nfd;
      C1 = C(1:ny,:);  D1 = D(1:ny,:);
      C2 = C(ny+1:ny+nfd,:);  D2 = D(ny+1:ny+nfd,:);
      % Circulant delay buffers ZBUF keeps track of z[k-m] for m=1:IDMAX.
      idmax = max(id);  % max delay
      zbuf = repmat(w0,[1 idmax]);
      % Step forward
      ioffset = (1:nfd)';
      y = zeros(ny,Ns);
      xk = x0;
      uwk = [u0;w0];
      y(:,1) = C1*xk+D1*uwk;
      zbuf(:,1) = C2*xk+D2*uwk;
      ix = ioffset + mod(1-id,idmax) * nfd;
      uwkp1 = [u(:,min(2,Ns)) ; zbuf(ix)];
      for k=2:Ns
         % x[k]
         xk = SOLVER(A*xk+B*uwk+Bp1*uwkp1);
         % [u[k];w[k]]
         uwk = uwkp1;
         % y[k]
         y(:,k) = C1*xk+D1*uwk;
         % z[k]
         jz = mod(k-1,idmax)+1;
         zbuf(:,jz) = C2*xk+D2*uwk;
         % Read w[k+1] from Z buffer
         ix = ioffset + mod(k-id,idmax) * nfd;
         uwkp1 = [u(:,min(k+1,Ns)) ; zbuf(ix)];
      end
   end
end

if ~(EXPLICIT || allfinite(y))
   error(message('Control:analysis:DiscreteDAE'))
end

%-------------------------------------------------------------
function x0 = localCIC(x0,A,E,rhs)
% Finds consistent initial values for algebraic variables. 
% If any of the conditions below is not satisfied, x0 is unchanged 
% on exit.

% Permute E(p,q) = [0 E12;0 0]
[p,q,~,~,cc,rr] = dmperm(E);
if rr(1)==rr(2) && cc(4)==cc(5) % E12 square
   % Solve A(i2,j1) dx0(j1) = -(A*x0+b)(i2)
   nx = size(A,1);
   nz = cc(3)-cc(1);
   i2 = p(nx-nz+1:nx);
   j1 = q(1:nz);  % zero columns in E
   if any(rhs(i2,:))
      dx0j1 = matlab.internal.math.nowarn.mldivide(A(i2,j1),rhs(i2));
      if allfinite(dx0j1)
         x0(j1) = x0(j1)-dx0j1;
      end
   end
end
