function [y,t,focus,x,ysd] = timeresp(D,RespType,t,Config)
%TIMERESP  Time response of single LTI model.
%
%   [Y,T,FOCUS,X] = TIMERESP(D,RESPTYPE,T,CONFIG) computes the step, impulse, 
%   or initial response based on the value of RESPTYPE. The input T can be
%      * [] for infinite-horizon simulations
%      * a scalar specifying the final time Tf 
%      * a 1x2 vector T=[T0 Tf] specifying the start time and final time
%      * a vector of time samples.
%   CONFIG contains the setup parameters for each response types (e.g.,
%   Delay and Amplitude for step).
%   
%   When T=[], FOCUS is set the preferred simulation horizon. Otherwise FOCUS 
%   is equal to T(END) unless the final time has to be reduced to prevent 
%   overflow (for unstable systems only).

%	 Author: P. Gahinet
%   Copyright 1986-2023 The MathWorks, Inc.
INITIAL = strcmp(RespType,'initial');
[ny,nu] = iosize(D);
isCT = (D.Ts==0);
ysd = []; % default handling for model containing no covariance info
x = [];

% Compatibility checks
if INITIAL
   D = getsubsys(D,':',[]); % eliminate input channels
   nu = 1;
else
   Config = validate(Config,nu);
   if ischar(Config.InputOffset)
      if isa(D,'ltipack.ssdata')
         Config.InputOffset = D.u0;
      else
         Config.InputOffset = zeros(nu,1);
      end
   end
end
[D,xinit,ComputeX] = checkCompatibility(D,RespType,Config.InitialState,[],nargout>3);

% Interpret T (start, stop, step size)
[t,t0,tf,dt] = ltipack.util.getTimeInfo(t,D.Ts);

% Quick exit for systems w/o inputs (step/impulse only)
if nu==0
   if isempty(tf)  % infinite horizon
      focus = [];  tf = 1;
   else
      focus = [t0,tf];
   end
   if isempty(t)
      t = [t0;tf];
   end
   Ns = length(t);
   y = zeros(Ns,ny,0);
   if ComputeX
      x = zeros(Ns,order(D),0); % only compute order when state space
   end
   return
end

% Populate SimInfo struct
if isCT
   % Adjustable step size
   MaxSample = 1e5;
else
   % Fixed step size, unknown horizon
   MaxSample = 1e6;
end
SimInfo = struct(...
   'nsim',nu,...  % number of simulations needed
   'Delay',0,...  % step or impulse delay relative to start time
   'xinit',xinit,...  % initial state
   'uinit',[],...     % input value prior to start (input offset)
   'yinit',[],...     % output value prior to start (zero when unknown or unsteady)
   'du',[],...        % step or impulse amplitude
   'FinalValue',[],...
   'MaxSample',MaxSample,... % max number of time steps in simulation
   'DivThreshold',1e100,...  % divergence threshold (|y|>DivThreshold -> diverged)
   'ComputeX',ComputeX);  % whether state vector is needed
if isempty(tf)
   % Steady-state value 
   % Note: Response may converge to difference final value if there are pole/zero
   %       cancellations at s=0 or z=1
   SimInfo.FinalValue = getFinalValue(D,RespType,Config);
end
if ~INITIAL
   SimInfo = getSignalParam(Config,SimInfo);
end
   
% Simulate response
if isCT && hasDelayDynamics(D,'pole')
   % Simulate with DDAE solver when there are delay dynamics
   if SimInfo.Delay>0
      error(message('Control:analysis:timeresp2'))
   end
   if isempty(tf)
      % Infinite horizon
      [y,t,tFocus,x] = ddaeresp(D,[],[],SimInfo);
      % Set focus
      switch tFocus
         case 0 % marginally stable
            tFocus = 0.1*t(end-1);
         case -1 % divergent
            tFocus = localDivergentFocusVS(t,abs(y),getMaxDelay(D));  % divergent case
      end
      focus = [t0 , tFocus];
   else
      [y,t,~,x] = ddaeresp(D,t,tf,SimInfo);
      focus = [t0,tf];
   end
else
   % Compute discrete model for fixed-step integration
   % Note: 
   %   * TF may be adjusted for unstable systems
   %   * trespSetUp may increase length of SimInfo.xinit
   [DS,dt,tf,SimInfoS] = trespSetUp(D,RespType,dt,tf,SimInfo);
   if isempty(tf)
      % Infinite horizon simulation with on-the-fly settling time detection
      % Note: Last point is always (t,y)=(Inf,y(Inf))
      [y,Nfocus,x] = tresp(DS,RespType,[],SimInfoS);
      if Nfocus==0 && isCT
         % Response has neither settled nor diverged. Retry with larger step size
         [DS,~,~,SimInfoS] = trespSetUp(D,RespType,50*dt,tf,SimInfo);
         [y1,Nfocus,x1] = tresp(DS,RespType,[],SimInfoS);
         if Nfocus~=0
            % Use second simulation if converged or diverged
            y = y1;  x = x1;  dt = 50*dt;
         end
      end
      Nf = size(y,1)-1;   % watch for extra point (t,y)=(Inf,y(Inf))

      % Adjust focus in nonconvergent cases
      switch Nfocus
         case 0
            % Marginally stable
            Nfocus = round(0.9*Nf);
         case -1
            % Divergent
            Nfocus = localDivergentFocusFS(abs(y),getMaxDelay(DS));
      end

      % Set time grid and focus
      t = t0 + dt*[0:Nf-1,1e10*Nf].';  % last entry is proxy for t=Inf
      if isstatic(D)
         focus = []; % no focus for static gain (e.g., step(tf(1),tf(100,[1 100]))
      else
         focus = [t0,t0+Nfocus*dt];
      end
   else
      % Finite-horizon simulation
      Nf = ceil((1-1e3*eps)*(tf-t0)/dt);  % watch for adding extra sample when TF/DT = NF + o(eps)
      t = t0 + dt*(0:Nf).';
      [y,~,x] = tresp(DS,RespType,numel(t),SimInfoS);
      focus = [t0,tf];
   end
end

if ComputeX && isCT && strcmp(RespType,'impulse')
   % For IMPULSE, set x(1,:,:) to x(0)+ for consistency with y1=y(0)+
   % and to enforce y = c*x
   % Note: No internal delays since impulse does not support it.
   Delays = D.Delay.Input+SimInfo.Delay;
   for j=1:nu
      tau = Delays(j)/dt;  % discrete delay
      nd = round(tau);     % rounded in sample periods
      if abs(nd-tau)<1e3*eps
         x(nd+1,:,j) = x(nd+1,:,j) + SimInfo.du(j) * D.b(:,j).';
      end
   end
end


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


function Nfocus = localDivergentFocusFS(y,MaxDelay)
% Adjusts focus of divergent simulations (fixed step case). Goal is to
% avoid plots looking like _|. Require that |y|>a*ymax for some t<(1-a)*tf
% where a<0.1
a = 0.05;
Nfocus = size(y,1);
Nmin = max(50,2*MaxDelay);
while Nfocus>2*Nmin && ...
      any(max(y(1:round((1-a)*Nfocus),:)) < a*max(y(1:Nfocus-1,:)))
   Nfocus = round(Nfocus/2);
end

function tFocus = localDivergentFocusVS(t,y,MaxDelay)
% Adjusts focus of divergent simulations (variable step case)
a = 0.05;
tFocus = t(end-1);
tmin = max(t(min(50,end)),2*MaxDelay);
while tFocus>2*tmin && ...
      any(max(y(t<(1-a)*tFocus,:)) < a*max(y(t<tFocus,:)))
   tFocus = t(find(t>tFocus/2,1));
end