function [y,t,Tfocus,x] = ddaeresp(D,t,Tf,SimInfo)
% Step response of continuous-time state-space models with state delays.

%	 Author: P. Gahinet
%   Copyright 1986-2021 The MathWorks, Inc.
[ny,nu] = iosize(D);
ComputeX = SimInfo.ComputeX;
CplxFlag = (~isreal(D));
nx = size(D.a,1);
xch = cell(nu,1);
XMap = (1:nx)';
x = [];

% Resolve sim horizon
if isempty(Tf)
   if isempty(t)
      Tf = inf;
   else
      % user-defined time grid T
      Tf = t(end);
   end
end
   
% Extract s-minimal realization for each input channel
Dsim = ltipack.ssdata.array([nu 1]);
SimInfo.XMap = cell(nu,1);
for j=1:nu
   [Dsim(j),xkeep] = getsubsys(D,':',j,'min');
   % XMap keeps track of where the remaining states are in
   % the original state vector
   SimInfo.XMap{j} = XMap(xkeep);
   % Handle complex data
   if CplxFlag
      Dsim(j) = getsubsys(cplx2real(Dsim(j)),':',1);  % U is real
   end
end

% Extract and package data for simulating each input channel
[Data,DiscSet] = localBuildSolverData(Dsim,Tf,(1+CplxFlag)*nx);

% Integrate DDAE
% Sol: nu-by-1 struct array of time histories for each input channel
Info = struct(...
   'FinalValue',SimInfo.FinalValue,... 
   'IC',[],...
   'MaxSample',SimInfo.MaxSample,...
   'DivThreshold',SimInfo.DivThreshold);
if CplxFlag
   % Note: Do not modify DC value in output
   Info.FinalValue = [real(Info.FinalValue);imag(Info.FinalValue)];
end
atol = 1e-8;  rtol = 1e-3;
[Sol,Tfocus,yf] = ddaeresp(Data,Info,atol,rtol);

% Project simulations onto common time grid
Dout = Dsim(1).Delay.Output;
if ComputeX
   [y,t,xch] = mergeresp(Sol,Dout,DiscSet,t,Tfocus);
   for ct=1:length(xch)
      % Note: Account for scaling in localBuildSolverData
      xch{ct} = lrscale(xch{ct}.',[],Data(ct).s);
   end
else
   [y,t] = mergeresp(Sol,Dout,DiscSet,t,Tfocus);
end

% Repackage data in complex case
if CplxFlag
   y = complex(y(:,1:ny,:),y(:,ny+1:2*ny,:));
   if ComputeX
      for j=1:nu
         xj = xch{j};
         nxj = size(xj,2)/2;
         xch{j} = complex(xj(:,1:nxj),xj(:,nxj+1:2*nxj));
      end
   end
   yf = complex(yf(1:ny,:),yf(ny+1:2*ny,:));
end

% Return states of original model
if ComputeX
   x = zeros(numel(t),nx,nu);
   for j=1:nu
      XMap = SimInfo.XMap{j};
      x(:,XMap,j) = xch{j}(:,1:numel(XMap));
   end
end

% Add t=Inf point to (t,y) to avoid recomputing response when x limits change
if isinf(Tf)
   t = [t ; 1e10*t(end)];
   y = cat(1,y,reshape(yf,[1 ny nu]));
   % Note: Don't extend x, will be pruned anyway
end

% Account for offsets (STEP and IMPULSE only).
% Note: Valid only when starting from steady-state condition at t=t0
[y,x] = localApplyOffsets(y,x,D,SimInfo);

%---------------- Local Functions ----------------------

function [Data,DiscSet] = localBuildSolverData(Dsim,Tf,nxf)
% Constructs data structure for DDAERESP solver
%   * Tf: final time
%   * nxf: full order of MIMO model
hw = ctrlMsgUtils.SuspendWarnings; %#ok<NASGU>
nu = length(Dsim);
% Coefficients for two-point Radau IIA implicit Runge-Kutta method.
x11 = 5/12; x12 = -1/12; x21 = 0.75; x22 = 0.25;

Data = struct('nx',cell(nu,1),'ulag',[],'zlags',[],...
   'D',[],'ABCD',[],'H',[],'U',[],'s',[]);
jfull = 0;
for j=1:nu
   % Extract and package data for j-th channel
   Dj = Dsim(j);
   [a,~,b2,~,c2,~,~,d21,d22] = getBlockData(Dj);
   nx = size(a,1);
   % Scale state vector for numerical stability
   if nx<nxf || jfull==0
      % Scale A matrix of zero-order Pade appx (good approximation of DC dynamics)
      sm = a+b2*((eye(size(d22))-d22)\c2);
      if ~allfinite(sm)
         sm = a;
      end
      s = mscale(sm,'noperm','safebal');
   else
      % Reuse previously computed scaling for full order
      s = Data(jfull).s;
   end
   a = lrscale(a,1./s,s);
   Data(j).ABCD = [a lrscale(Dj.b,1./s,[]) ; lrscale(Dj.c,[],s) Dj.d];   
   Data(j).nx = nx;   
   Data(j).s = s;
   % Compute Hessenberg form of kron(x,a)
   if nx<nxf || jfull==0
      [Data(j).U,Data(j).H] = hess([x11*a x12*a;x21*a x22*a]);
      if nx==nxf
         jfull = j;
      end
   else
      Data(j).U = Data(jfull).U;  
      Data(j).H = Data(jfull).H;
   end
   % Construct vector of discontinuity points for x'(t)
   zlags = Dj.Delay.Internal;
   ulag = Dj.Delay.Input;
   if isempty(zlags)
      Data(j).D = [ulag Tf];
   else
      lags = sort(zlags);
      lags(diff(lags)==0,:) = [];
      if norm(d21,inf)==0 || norm(d22,inf)==0
         maxlevel = 1;
      else
         maxlevel = 4;
      end
      Data(j).D = propagate(ulag,lags,Tf,maxlevel);
   end
   Data(j).zlags = zlags;
   Data(j).ulag = ulag;
end
   
% All discontinuity points
DiscSet = sort(cat(2,Data.D));
DiscSet = DiscSet([true,diff(DiscSet)>0]);


%-------------------------
function [y,x] = localApplyOffsets(y,x,D,SimInfo)
% Applies offsets and amplitude factors.

%   Copyright 2020 The MathWorks, Inc.
[~,nu] = iosize(D);
uinit = SimInfo.uinit;  % input offset
du = SimInfo.du;
if ~isempty(SimInfo.xinit)
   % Offsets can be applied a posteriori only when initial condition is steady 
   error(message('Control:analysis:lsim4'))
end
% Resolve initial condition and output offset
[xinit,~,yinit] = resolveIC(D,[],uinit);
for j=1:nu
   y(:,:,j) = yinit' + du(j) * y(:,:,j);
   if SimInfo.ComputeX
      x(:,:,j) = xinit' + du(j) * x(:,:,j);
   end
end