function [y,t,focus,x,ysd,p] = timeresp_(sys,RespType,t,pSpec,Config)
% LTV/LPV simulation with STEP/IMPULSE/INITIAL.

%   Copyright 2022 The MathWorks, Inc.
[ny,nu] = size(sys);
nx = sys.Nx_;
nfd = sys.Nfd_;
Ts = abs(sys.Ts);
ysd = [];
NEEDX = (nargout>3);
INITIAL = strcmp(RespType,'initial');
LPV = isLPV(sys);
np = nparam(sys);

% Compatibility checks
if INITIAL
   nsim = 1;
else
   Config = validate(Config,nu);
   nsim = nu;
end
xinit = Config.InitialState; % [], value, or 'x0'
sys = checkCompatibility(sys,RespType,xinit,pSpec);

% Interpret T (start, stop, step size)
[t,t0,tf,dt] = ltipack.util.getTimeInfo(t,Ts);
focus = [t0,tf];
if isempty(t)
   if Ts>0
      t = (t0:dt:tf).';
   elseif isnumeric(pSpec) && ~isempty(pSpec)
      t = linspace(t0,tf,size(pSpec,1)).';
   else
      t = linspace(t0,tf,1001).';
   end
end
Ns = numel(t);

% Quick exit for systems w/o inputs (step/impulse only)
if nsim==0
   y = zeros(Ns,ny,0);  x = zeros(Ns,nx,0);
   if LPV
      p = zeros(Ns,np,0);
   else
      p = [];
   end
   return
end

% Initial condition.
% Note: Default IC.w=0 is arbitrary but consistent with LSIM
IC = struct('x',xinit,'u',zeros(nu,1),'y',zeros(ny,1),'w',zeros(nfd,1),'p',[]);

% Check parameters
if LPV
   if isnumeric(pSpec)
      if np==0 && isempty(pSpec)
         pSpec = zeros(0,Ns);
      else
         pSpec = pSpec.';  % put samples along column dimension
      end
   end
end

% Simulate
DF = sys.DataFunction_;
if Ts>0
   ODEFUN = @ltvpack.DSIM;
   tk0 = round(t0/Ts);  % t0 or k0
else
   if isnumeric(pSpec)
      ODEFUN = @ltvpack.TRBDF2;
   else
      ODEFUN = @ltvpack.TRBDF2x;
   end
   tk0 = t0;
end
OPTS = struct('RELINPUT',false,'DIRAC',[],'NEEDX',NEEDX); % solver options

if INITIAL
   % Initial response (free response with u(t)=u0(t) and x(t0)=x0)
   OPTS.RELINPUT = true;
   ut = zeros(nu,Ns);
   if LPV
      % Resolve PINIT
      if isnumeric(pSpec)
         pinit = pSpec(:,1);
      else
         % PINIT required for p=F(t,x,u)
         pinit = Config.InitialParameter;
         if np>0 && isempty(pinit)
            error(message('Control:analysis:rfinputs30'))
         elseif ~isequal(size(pinit),[np 1])
            error(message('Control:analysis:rfinputs31',np))
         end
      end
      IC.p = pinit;
   end
   [y,x,p,WARN] = ODEFUN(DF,ut,t,pSpec,IC,OPTS);
else
   % STEP and IMPULSE
   IC.u = Config.InputOffset;
   du = Config.Amplitude;
   nDelay = Config.Delay/(t(2)-t(1)); % step/impulse delay in sampling periods
   OPTS.RELINPUT = (ischar(IC.u));
   X0IC = (ischar(IC.x));
   IMPLICIT_IC = (OPTS.RELINPUT || X0IC || isempty(IC.x));
   if nfd>0 && ~(OPTS.RELINPUT || isempty(IC.x))
      % Can't resolve W when starting from unsteady condition, unless
      % step/impulse is relative to uw0(t,p) in which case w is fully
      % specified.
      error(message('Control:analysis:LTV5'))
   end

   % Resolve implicit initial condition (done once for all sims)
   if IMPLICIT_IC
      % Resolve PINIT
      if LPV
         if isnumeric(pSpec)
            pinit = pSpec(:,1);
         else
            % PINIT must be specified when P(t) and U(t) or X(t0) are implicit
            pinit = Config.InitialParameter;
            if np>0 && isempty(pinit)
               error(message('Control:analysis:rfinputs27'))
            elseif numel(pinit)~=np
               error(message('Control:analysis:rfinputs31',np))
            end
         end
         [~,~,~,~,~,~,x0,uw0] = DF(tk0,pinit);
         IC.p = pinit;
      else
         [~,~,~,~,~,~,x0,uw0] = DF(tk0);
      end
      % Resolve XINIT and WINIT
      if X0IC
         % XINIT is x0(t0,p(t0))
         if isempty(x0)
            IC.x = zeros(nx,1);
         else 
            IC.x = x0;
         end
      end
      if OPTS.RELINPUT
         % Input offset is [u0;w0](t0,p(t0))
         if isempty(uw0)
            uw0 = zeros(nu+nfd,1);
         end
         IC.u = uw0(1:nu,:);
         IC.w = uw0(nu+1:nu+nfd,:);
         if isempty(IC.x)
            % Find steady state for u=u0(t0,p(t0)) and w=w0(t0,p(t0))
            [op,SINGULAR] = findop(sys,tk0,IC.p,u=IC.u,w=IC.w);
            if SINGULAR
               error(message('Control:analysis:step4'))
            end
            IC.x = op.x;  IC.y = op.y;
         end
      else
         % Input offset is explicitly specified
         if isempty(IC.x)
            % Find steady-state IC for constant input
            [op,SINGULAR] = findop(sys,tk0,IC.p,u=IC.u);
            if SINGULAR
               error(message('Control:analysis:step2'))
            end
            IC.x = op.x;  IC.w = op.w;  IC.y = op.y;
         end
      end
   end

   % Allocate
   y = zeros(Ns,ny,nsim);
   if NEEDX
      x = zeros(Ns,nx,nsim);
   else
      x = zeros(0,0,nsim);
   end
   p = zeros(Ns,np,nsim);
   WARN = false(Ns,nsim);

   % Simulate each channel
   for j=1:nsim
      % Input signal
      if OPTS.RELINPUT
         ut = zeros(nu,Ns); % relative to u0(t)
      else
         ut = repmat(IC.u,[1 Ns]);
      end
      if RespType(1)=='s'
         kx = round(nDelay)+1:Ns;   ut(j,kx) = ut(j,kx)+du(j);
      elseif Ts==0
         uP = zeros(nu,1);   uP(j) = du(j);
         OPTS.DIRAC = struct('j',j,'nDelay',nDelay,'u',uP);
      else
         kx = round(nDelay)+1;   ut(j,kx) = ut(j,kx)+du(j)/dt;
      end
      % Finalize IC
      if ~IMPLICIT_IC && LPV
         % Resolve PINIT (depends on u(t0) in p=F(t,x,u) case)
         if isnumeric(pSpec)
            pinit = pSpec(:,1);
         else
            try
               pinit = pSpec(tk0,IC.x,ut(:,1));
            catch
               error(message('Control:analysis:rfinputs25'))
            end
         end
         if ~isequal(size(pinit),[np 1])
            error(message('Control:analysis:rfinputs26',np))
         end
         IC.p = pinit;
      end
      % Call solver
      [y(:,:,j),x(:,:,j),p(:,:,j),WARN(:,j)] = ODEFUN(DF,ut,t,pSpec,IC,OPTS);
   end
   WARN = any(WARN,2);
end

% Warn when failed to solver nonlinear equations to full accuracy
if any(WARN)
   tWarn = t(WARN);
   s1 = sprintf('%.2e',tWarn(1));  s2 = sprintf('%.2e',tWarn(end));
   if Ts>0
      warning(message('Control:analysis:LTV4',s1,s2))
   else
      warning(message('Control:analysis:LTV3',s1,s2))
   end
end
