function [s,extra] = stepinfo(y,varargin)
%STEPINFO  Computes step response characteristics.
%
%   S = STEPINFO(Y,T,YFINAL,YINIT) takes the step response data (T,Y), an
%   initial value YINIT, and a steady-state value YFINAL, and returns a
%   structure S containing the following performance indicators:
%     * RiseTime: rise time
%     * TransientTime: transient time
%     * SettlingTime: settling time
%     * SettlingMin: min value of Y once the response has risen
%     * SettlingMax: max value of Y once the response has risen
%     * Overshoot: percentage overshoot (relative to YFINAL)
%     * Undershoot: percentage undershoot
%     * Peak: peak value of deviation |Y-YINIT|
%     * PeakTime: time at which this peak value is reached.
%
%   For SISO responses, T and Y are vectors with the same length NS.
%   For systems with NU inputs and NY outputs, you can specify Y as
%   an NS-by-NY-by-NU array (see STEP) and YFINAL and YINIT as
%   NY-by-NU arrays. STEPINFO then returns a NY-by-NU structure array S
%   of performance metrics for each I/O pair.
%
%   When omitted, T defaults to 1:NS, YFINAL defaults to the last sample
%   value of Y, and YINIT defaults to zero. Set YINIT to a nonzero value
%   when the Y data has an initial offset (Y is nonzero prior to the step).
%
%   S = STEPINFO(SYS) computes the step response characteristics for
%   the dynamic system SYS. The rise time, settling time, and peak time
%   are all expressed in the time units of SYS (see "TimeUnit" property).
%
%   S = STEPINFO(SYS,YFINAL) explicitly specifies the steady-state value 
%   YFINAL.
%
%   S = STEPINFO(...,'SettlingTimeThreshold',ST) lets you specify the
%   threshold ST used in the settling time calculation.  The response
%   has settled when the error |y(t) - YFINAL| becomes smaller than a
%   fraction ST of |YINIT - YFINAL|. The default value is ST=0.02 (2%).
%
%   S = STEPINFO(...,'RiseTimeLimits',RT) lets you specify the lower
%   and upper thresholds used in the rise time calculation.  By default,
%   the rise time is the time the response takes to rise from 10% to 90%
%   of the steady-state value (RT=[0.1 0.9]).  Note that RT(2) is also
%   used to calculate SettlingMin and SettlingMax.
%
%   Example:
%      sys = rss(5);
%      s = stepinfo(sys,'RiseTimeLimits',[0.05,0.95])
%
%   See also STEP, LSIMINFO, DYNAMICSYSTEM.

%   Author(s): P. Gahinet
%   Copyright 1986-2021 The MathWorks, Inc.
if nargin==0
   error(message('Controllib:general:OneOrMoreInputsRequired','stepinfo','stepinfo'))
else
   % Parse input list and perform consistency checks
   try
      [y,t,yfinal,yinit,SettlingTimeThreshold,RiseTimeLims,Ts] = ...
         utRespInfoCheck(y,varargin{:});
   catch E
      throw(E)
   end
end

% Loop over each I/O pair
sio = size(yfinal);
s = struct(...
   'RiseTime',cell(sio),...
   'TransientTime',cell(sio),...
   'SettlingTime',[],...
   'SettlingMin',[],...
   'SettlingMax',[],...
   'Overshoot',[],...
   'Undershoot',[],...
   'Peak',[],...
   'PeakTime',[]);
extra = struct(...
   'RiseTimeLow',cell(sio),...
   'RiseTimeHigh',[]);
for ct=1:prod(sio)
   % Remove offset YINIT to obtain offset-invariant results
   [s(ct),extra(ct)] = LocalGetInfo(s(ct),extra(ct),...
      y(:,ct),t,yinit(ct),yfinal(ct),SettlingTimeThreshold,RiseTimeLims,Ts);
end

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

function [s,xt] = LocalGetInfo(s,xt,y,t,yi,yf,SettlingTimeThreshold,RiseTimeLims,Ts)
% Computes step response metrics for SISO response
% Note: Y(1) differs from YI when feedthrough is present.

if isfinite(yf)
   % Converging response
   ns = length(t);

   % Peak response
   [s.Peak,ipeak] = max(abs(y-yi));
   s.PeakTime = t(ipeak);

   if yf==yi
      % Degenerate case YF=YI
      s.RiseTime = 0;
      xt.RiseTimeLow = 0;
      xt.RiseTimeHigh = 0;
      s.SettlingMin = min(y);
      s.SettlingMax = max(y);
      s.Overshoot = Inf;
      if all(y>=0)
         s.Undershoot = 0;
      else
         s.Undershoot = Inf;
      end
   else
      yrel = (y-yi)/(yf-yi);  % normalize to YI=0, YF=1

      % Get time TLOW of first crossing of y = RiseTimeLims(1)*(yf-yi)
      yLow = RiseTimeLims(1);
      iLow = find(yrel>yLow,1);
      if isempty(iLow)
         % Has not yet reached RiseTimeLims(1) level
         tLow = NaN;
      elseif Ts==0 && iLow>1
         % Interpolate for more accuracy
         tLow = t(iLow) + (t(iLow)-t(iLow-1))/(yrel(iLow)-yrel(iLow-1)) * (yLow-yrel(iLow));
      else
         % Discrete time or feedthrough
         tLow = t(iLow);
      end

      % Get time THIGH of first crossing of y = RiseTimeLims(2)*(yf-yi)
      yHigh = RiseTimeLims(2);
      iHigh = find(yrel>yHigh,1);
      if isempty(iHigh)
         % Has not yet reached RiseTimeLims(2) level
         tHigh = NaN;
         s.SettlingMin = NaN;
         s.SettlingMax = NaN;
      else
         if Ts==0 && iHigh>1
            % Interpolate for more accuracy
            tHigh = t(iHigh) + (t(iHigh)-t(iHigh-1))/(yrel(iHigh)-yrel(iHigh-1)) * (yHigh-yrel(iHigh));
         else
            % Discrete time or feedthrough
            tHigh = t(iHigh);
         end
         yRisen = y(iHigh:end);
         s.SettlingMin = min(yRisen);
         s.SettlingMax = max(yRisen);
      end
      
      % Rise time
      % Time for response to go from 10% to 90% of the way from YI to YF
      s.RiseTime = tHigh - tLow;
      xt.RiseTimeLow = tLow;
      xt.RiseTimeHigh = tHigh;

      % Overshoot and undershoot
      s.Overshoot = 100 * max(0,max(yrel-1));
      s.Undershoot = -100 * min(0,min(yrel));
   end
   
   % Transient Time
   % |Y-YF| < SettlingTimeThreshold * max(|Y-YF|)
   err = abs(y-yf);
   tol = SettlingTimeThreshold * max(err);
   iSettle = find(err>tol,1,'last');
   if isempty(iSettle)
      % Pure gain
      s.TransientTime = 0;
   elseif iSettle==ns
      % Has not settled
      s.TransientTime = NaN;
   elseif Ts==0
      % Interpolate for more accuracy
      ySettle = yf + sign(y(iSettle)-yf) * tol;
      s.TransientTime = t(iSettle) + ...
         (t(iSettle)-t(iSettle+1))/(y(iSettle)-y(iSettle+1)) * (ySettle-y(iSettle));
   else
      % Discrete time or feedthrough
      s.TransientTime = t(iSettle+1);
   end
   
   % Settling Time
   % |Y-YF| <= SettlingTimeThreshold * |YF-YI|
   tol = SettlingTimeThreshold * abs(yf-yi);
   iSettle = find(err>tol,1,'last');
   if isempty(iSettle)
      % Pure gain
      s.SettlingTime = 0;
   elseif iSettle==ns
      % Has not settled
      s.SettlingTime = NaN;
   elseif Ts==0
      % Interpolate for more accuracy
      ySettle = yf + sign(y(iSettle)-yf) * tol;
      s.SettlingTime = t(iSettle) + ...
         (t(iSettle)-t(iSettle+1))/(y(iSettle)-y(iSettle+1)) * (ySettle-y(iSettle));
   else
      % Discrete time or feedthrough
      s.SettlingTime = t(iSettle+1);
   end
      
else
   % Unstable response or unknown steady state
   s.RiseTime = NaN;
   s.TransientTime = NaN;
   s.SettlingTime = NaN;
   s.SettlingMin = NaN;
   s.SettlingMax = NaN;
   s.Overshoot = NaN;
   s.Undershoot = NaN;
   s.Peak = Inf;
   s.PeakTime = Inf;
   xt.RiseTimeLow = Inf;
   xt.RiseTimeHigh = Inf;
end
   
