function [xinit,winit,yinit,ypast] = resolveIC(D,xinit,uinit)
% Resolve initial condition for STEP or IMPULSE.
%
%   u(t) is assumed constant and equal to UINIT (input offset) prior to start.
%
%   XINIT=[] is resolved to a steady initial condition (XINIT,UINIT,WINIT)
%   satisfying
%      dx/dt = dx0 + A (XINIT-x0) + B1 (UINIT-u0) + B2 (WINIT-w0) = 0
%      WINIT = z0 + C2 (XINIT-x0) + D21 (UINIT-u0) + D22 (WINIT-w0) .
%   The second equation says that the internal signals z(t) are steady with
%   value winit, which is required to condense the z(t<t0) history into a
%   single value winit. The value 
%      YINIT = y0 + C1 (XINIT-x0) + D11 (UINIT-u0) + D12 (WINIT-w0) 
%   is the output at time t0, and this value is steady for t<t0 so YPAST=YINIT.
%
%   When XINIT=x(t0) is specified, the model is required to have no state  
%   delays and be of the form
%      dx = dx0 + A (x-x0) + sum Bj (u(t-tj)-u0)
%      y = y0 + C (x-x0) + sum Dj (u(t-tj)-u0)
%   (see checkCompatibility). An equivalent model is obtained by dropping
%   the term C2*(x-x0) from z: 
%      z = z0 + D21 (u-u0) + D22 (w-w0) .
%   When u(t)=UINIT for t<t0, we have z(t)=w(t)=WINIT given by
%      WINIT = z0 + D21 (UINIT-u0) + D22 (WINIT-w0)  .
%   The output at time t0 is 
%      YINIT = y0 + C1 (XINIT-x0) + D11 (UINIT-u0) + D12 (WINIT-w0) 
%   This value is not steady for t<t0 except for entries yinit(i) that do 
%   not depend on x due to C1(i,:)=0. YPAST coincides with YINIT for such 
%   entries and is set to zero elsewhere (unknown or unsteady past value).

%   Copyright 1986-2022 The MathWorks, Inc.
if isempty(xinit)
   % Compute steady-state IC for input offset uinit
   opspec = ltioptions.findop;
   opspec.u = uinit;
   [op,SINGULAR] = findop(D,opspec);
   if SINGULAR
      error(message('Control:analysis:step2'))
   end
   xinit = op.x;
   winit = op.w;
   yinit = op.y;
   ypast = yinit;
else
   % Specified xinit, NO state delays
   [~,~,~,c1,~,d11,d12,d21,d22,~] = getBlockData(D);
   nu = numel(uinit);
   ny = size(d11,1);
   nfd = size(d22,1);
   u0 = D.u0(1:nu,:);
   w0 = D.u0(nu+1:end,:);
   y0 = D.y0(1:ny,:);
   z0 = D.y0(ny+1:end,:);
   rhs = full(z0-w0+d21*(uinit-u0));
   if nfd>0 && any(rhs)
      % Note: D22 nilpotent in this context
      winit = w0 + matlab.internal.math.nowarn.mldivide(eye(nfd,'like',d22)-d22,rhs);
   else
      winit = w0;
   end
   % Note:
   % * yi(t) is steady for t<t0 iff. it does not depend on x
   % * In ypast, zero is used as "don't know" to avoid holes in plots
   yinit = y0+c1*(xinit-D.x0)+d11*(uinit-u0)+d12*(winit-w0);
   ypast = yinit;
   ypast(any(c1,2),:) = 0;
end
