classdef (Hidden = true) ssdata < ltipack.AbstractSS
   % Class definition for @ssdata (state-space data)
   
   %   Author(s): P. Gahinet
   %   Copyright 1986-2020 The MathWorks, Inc.
   properties
      StateName = strings(0,1);
      StatePath = strings(0,1); % block path for Simulink states
      StateUnit = strings(0,1);
   end
   
   methods
      function D = ssdata(a,b,c,d,e,Ts)
         % Constructs @ssdata instance
         if nargin==6
            D.a = a;  D.b = b;  D.c = c;  D.d = d;  D.e = e;
            D.Ts = Ts;
            nx = min(size(a));
            ny = max(size(c,1),size(d,1));
            nu = max(size(b,2),size(d,2));
            D.Delay = ltipack.utDelayStruct(ny,nu,true);
            D.dx0 = zeros(nx,1);  D.x0 = zeros(nx,1);
            D.u0 = zeros(nu,1);  D.y0 = zeros(ny,1);
         end
      end
      
      function [D,SingularFlag] = ss(D)
         % Converts to ss
         SingularFlag = false;
      end
      
      function sys = utCreateLTI(D)
         % Creates LTI object from low-level @xxxdata representation
         sys = ss.make(D);
      end
      
      function D = createGain(Dref,G)
         % Wraps static gain into @ssdata object
         [ny,nu] = size(G);
         D = ltipack.ssdata([],zeros(0,nu),zeros(ny,0),G,[],Dref.Ts);
      end
      
      function D = checkData(D)
         % Checks that state-space data is consistent and NaN free.
         % Get dimensions of A,B,C,E matrices
         sa = size(D.a);
         sb = size(D.b);
         sc = size(D.c);
         sd = size(D.d);
         se = size(D.e);
         
         % Determine I/O size for matrix data
         Ny = max(sc(1),sd(1));
         Nu = max(sb(2),sd(2));
         Nx = min(sa);
         Ne = min(se);
         
         % Handle [] shortcuts
         if Nx==0
            sa = [0 0];  D.a = [];
         end
         if Ne==0
            se = [0 0];  D.e = [];
         end
         if any(sb==0)
            sb = [Nx Nu];  D.b = zeros(Nx,Nu);
         end
         if any(sc==0)
            sc = [Ny Nx];  D.c = zeros(Ny,Nx);
         end
         if any(sd==0) || isequal(D.d,0)
            % Also allow for scalar expansion when d=0
            % Note: syntax ss([],[],[],0) returns a zero gain system
            sd = [Ny Nu];  D.d = zeros(Ny,Nu);
         end
         
         % Check compatibility of I/O and state dimensions
         if sa(1)~=sa(2) || se(1)~=se(2)
            error(message('Control:ltiobject:ssProperties4'))
         elseif Ne>0 && ~isequal(sa,se)
            error(message('Control:ltiobject:ssProperties5'))
         elseif Nx~=sb(1)
            error(message('Control:ltiobject:ssProperties1','A','B'))
         elseif Nx~=sc(2)
            error(message('Control:ltiobject:ssProperties2','A','C'))
         elseif sb(2)~=Nu || sd(2)~=Nu
            error(message('Control:ltiobject:ssProperties2','B','D'))
         elseif sc(1)~=Ny || sd(1)~=Ny
            error(message('Control:ltiobject:ssProperties1','C','D'))
         end

         % Offsets
         if ~any(D.dx0)
            D.dx0 = zeros(Nx,1);
         elseif numel(D.dx0)~=Nx
            error(message('Control:ltiobject:ssProperties7','dx'))
         end
         if ~any(D.x0)
            D.x0 = zeros(Nx,1);
         elseif numel(D.x0)~=Nx
            error(message('Control:ltiobject:ssProperties7','x'))
         end
         if ~any(D.u0)
            D.u0 = zeros(Nu,1);
         elseif numel(D.u0)~=Nu
            error(message('Control:ltiobject:ssProperties8'))
         end
         if ~any(D.y0)
            D.y0 = zeros(Ny,1);
         elseif numel(D.y0)~=Ny
            error(message('Control:ltiobject:ssProperties9'))
         end
         
         % StateName
         Nx = size(D.a,1);
         if ~any(numel(D.StateName)==[0 Nx])
            error(message('Control:ltiobject:ssProperties3','StateName'))
         end
         if ~any(numel(D.StatePath)==[0 Nx])
            error(message('Control:ltiobject:ssProperties3','StatePath'))
         end
         if ~any(numel(D.StateUnit)==[0 Nx])
            error(message('Control:ltiobject:ssProperties3','StateUnit'))
         end
         
         % Checks for NaNs
         if ~allfinite(D.d)
            D = createGain(D,nan(Ny,Nu));
         end
      end
      
      function [D,xkeep] = sminreal(D)
         [D,xkeep] = sminreal@ltipack.AbstractSS(D);
         if ~isempty(D.StateName)
            D.StateName = D.StateName(xkeep,:);
         end
         if ~isempty(D.StatePath)
            D.StatePath = D.StatePath(xkeep,:);
         end
         if ~isempty(D.StateUnit)
            D.StateUnit = D.StateUnit(xkeep,:);
         end
      end
      
      function D = xperm(D,perm)
         % Applies state permutation
         D = xperm@ltipack.AbstractSS(D,perm);
         if ~isempty(D.StateName)
            D.StateName = D.StateName(perm);
         end
         if ~isempty(D.StatePath)
            D.StatePath = D.StatePath(perm);
         end
         if ~isempty(D.StateUnit)
            D.StateUnit = D.StateUnit(perm);
         end
      end
      
      function D = diagrep(D,k)
         % Replicates model along the diagonal.
         D = diagrep@ltipack.AbstractSS(D,k);
         if ~isempty(D.StateName)
            D.StateName = repmat(D.StateName,[k 1]);
         end
         if ~isempty(D.StatePath)
            D.StatePath = repmat(D.StatePath,[k 1]);
         end
         if ~isempty(D.StateUnit)
            D.StateUnit = repmat(D.StateUnit,[k 1]);
         end
      end
      
      function D = cplx2real(D)
         D = cplx2real@ltipack.AbstractSS(D);
         % Clear state names/units
         D.StateName = strings(0,1);
         D.StateUnit = strings(0,1);
      end
      
      function SNU = getStateInfo(D,Prop)
         % Helper for get.StateName and get.StateUnit
         SNU = D.(Prop);
         if isempty(SNU)
            SNU = strings(size(D.a,1),1);
         end
      end
      
      function D1 = appendStateInfo(D1,D2,nx1,nx2)
         % Appends state names and units from D2 to those of D1
         if ~(isempty(D1.StateName) && isempty(D2.StateName))
            D1.StateName = [ltipack.fullstring(D1.StateName,nx1) ; ltipack.fullstring(D2.StateName,nx2)];
         end
         if ~(isempty(D1.StatePath) && isempty(D2.StatePath))
            D1.StatePath = [ltipack.fullstring(D1.StatePath,nx1) ; ltipack.fullstring(D2.StatePath,nx2)];
         end
         if ~(isempty(D1.StateUnit) && isempty(D2.StateUnit))
            D1.StateUnit = [ltipack.fullstring(D1.StateUnit,nx1) ; ltipack.fullstring(D2.StateUnit,nx2)];
         end
      end
      
      function Dout = sparss(D)
         % Conversion to spssdata.
         Dout = ltipack.spssdata(D.a,D.b,D.c,D.d,D.e,D.Ts);
         Dout.dx0 = D.dx0;
         Dout.x0 = D.x0;
         Dout.u0 = D.u0;
         Dout.y0 = D.y0;
         Dout.Delay = D.Delay;
      end
            
      function D = mechss(D)
         % Conversion to spssdata.
         D = mechss(sparss(D));
      end
      
      %-----------------------
      function GData = getPIDPlantData(GData)
         % Formats plant data for PID tuning (converts to either ZPK or FRD)
         if hasInternalDelay(GData)
            % convert to ZPKdata if possible
            hw = ctrlMsgUtils.SuspendWarnings; %#ok<NASGU>
            try
               % try convert to ZPKDATA
               GData = zpk(GData);
            catch %#ok<CTCH>
               % Extract linear I/O delays
               iod = GData.Delay.Input + GData.Delay.Output;
               GData.Delay.Input = 0;
               GData.Delay.Input = 0;
               % Generate frequency grid
               [z0,p0] = iodynamics(GData,'silent');
               w = freqgrid(GData,z0,p0,1);
               w = w(w>0);
               % Convert to FRDDATA
               GData = ltipack.frddata(fresp(GData,w),w,GData.Ts);
               GData.Delay.IO = iod;
            end
         else
            % convert to ZPKdata
            GData = zpk(GData);
         end
      end
      
      function [D,xinit,ComputeX] = checkCompatibility(D,PlotType,xinit,~,ComputeX)
         % Check compatibility of model with given plot type
         switch PlotType
            case {'step','impulse','lsim','initial'}
               % Required INFO fields: xinit,ComputeX
               isCT = (D.Ts==0);
               nx = size(D.a,1);  % initial order
               % 1) Remove zero internal delays (can create artificial DDAEs). Do it in
               % discrete time as well (g353999) and before ISPROPER to deal with singular loops
               % 2) Eliminate E matrix especially in discrete time (g560840, g346680)
               [isProper,D] = isproper(elimZeroDelay(D),true);
               if ~isProper
                  if hasInternalDelay(D)
                     error(message('Control:analysis:NotSupportedSimulationImproperDelayModel'))
                  else
                     error(message('Control:analysis:NotSupportedSimulationImproperSys'))
                  end
               elseif order(D)<nx && (ComputeX || (isnumeric(xinit) && any(xinit)))
                  % E was singular in original model and state vector changed
                  error(message('Control:analysis:NotSupportedSimulationSingularE'))
               end
               % Check initial condition
               if ischar(xinit)
                  xinit = D.x0;
               elseif numel(xinit)~=nx && ~isempty(xinit)
                  error(message('Control:analysis:timeresp1'))
               end
               % Plot-specific restriction
               [DP,DY] = hasDelayDynamics(D,'pole');
               switch PlotType
                  case 'step'
                     if (DP || DY) && ~isempty(xinit)
                        % No state delays with unsteady IC
                        error(message('Control:analysis:step3'))
                     end
                  case 'impulse'
                     if (isCT && DP)
                        % DDAERESP limitation: no support for CT IMPULSE
                        throw(ltipack.utNoDelaySupport('impulse',0,'internal'))
                     elseif (DP || DY) && ~isempty(xinit)
                        % No state delays with unsteady IC
                        error(message('Control:analysis:step3'))
                     end
                  case 'initial'
                     if isempty(xinit) && nx>0
                        % Initial state required
                        error(message('Control:analysis:rfinputs04'))
                     elseif isCT && DP && any(xinit)
                        % DDAERESP does not support nonzero initial state
                        error(message('Control:analysis:lsim4'))
                     end
                  case 'lsim'
                     if isCT && DP && any(xinit)
                        % DDAERESP does not support nonzero initial state
                        error(message('Control:analysis:lsim4'))
                     end
               end
            case {'sector'}
               % System must be proper and delay free in CT
               if D.Ts==0 && hasdelay(D)
                  error(message('Control:analysis:getSectorIndex9'))
               end
               [isProper,D] = isproper(elimZeroDelay(D),true);
               if ~isProper
                  error(message('Control:analysis:getSectorIndex10'))
               end
         end
      end
      
      %-----------------
      
      function D = utGrowIO(D,ny,nu)
         % Grows I/O size of SS model (used in SYS(I,J)=RHS)
         [ny0,nu0] = iosize(D);
         if ny>ny0 || nu>nu0
            nx = size(D.a,1);
            [rs,cs] = size(D.d);
            if nu>nu0
               % Resize B, D, and input delay
               D.b = [D.b(:,1:nu0) , zeros(nx,nu-nu0) , D.b(:,nu0+1:cs)];
               D.Delay.Input(nu0+1:nu,:) = 0;
               D.u0 = [D.u0(1:nu0,:) ; zeros(nu-nu0,1) ; D.u0(nu0+1:cs,:)];
            end
            if ny>ny0
               % Resize C, D, and output delay
               D.c = [D.c(1:ny0,:) ; zeros(ny-ny0,nx) ; D.c(ny0+1:rs,:)];
               D.Delay.Output(ny0+1:ny,:) = 0;
               D.y0 = [D.y0(1:ny0,:) ; zeros(ny-ny0,1) ; D.y0(ny0+1:rs,:)];
            end
            D.d = [D.d(1:ny0,1:nu0) zeros(ny0,nu-nu0) D.d(1:ny0,nu0+1:cs);...
               zeros(ny-ny0,nu+cs-nu0);...
               D.d(ny0+1:rs,1:nu0) zeros(rs-ny0,nu-nu0) D.d(ny0+1:rs,nu0+1:cs)];
         end
      end
      
      %------------ Delay management ------------------
      
      function D = delay2z(Dref,Delays)
         % Returns model for diag(z^(-DELAYS))
         [a,b,c,d] = ltipack.util.realizeDelays(Delays);
         D = ltipack.ssdata(a,b,c,d,[],Dref.Ts);
      end
      
      function D = zeroInternalDelay(D)
         % Returns model of the SAME order with all internal delays set to zero.
         % This operation may produce an ill-conditioned realization and should
         % be used only when order preservation is critical.
         nfd = length(D.Delay.Internal);
         if nfd>0
            [a,b1,b2,c1,c2,d11,d12,d21,d22,~] = getBlockData(D); %#ok<*PROP>
            nx = size(a,1);  [ny,nu] = size(d11);
            w0 = D.u0(nu+1:nu+nfd,:);
            z0 = D.y0(ny+1:ny+nfd,:);
            % Solve for w-w0:
            aux = matlab.internal.math.nowarn.mldivide(eye(nfd)-d22,[c2 d21 z0-w0]);
            if allfinite(aux)
               D.a = a+b2*aux(:,1:nx);
               D.b = b1+b2*aux(:,nx+1:nx+nu);
               D.dx0 = D.dx0+b2*aux(:,nx+nu+1);
               D.c = c1+d12*aux(:,1:nx);
               D.d = d11+d12*aux(:,nx+1:nx+nu);
               D.y0 = D.y0(1:ny,:)+d12*aux(:,nx+nu+1);
               D.u0 = D.u0(1:nu,:);
               D.Scaled = false;
               D.Delay.Internal = zeros(0,1);
            else
               error(message('Control:ltiobject:access3'))
            end
         end
      end
      
      %----------- LFT support ------------------------
      
      function D = appendGain(D,G)
         % Forms append(D,G) where G is a matrix
         nx = size(D.a,1);
         nfd = length(D.Delay.Internal);
         [ny,nu] = size(D.d);
         [nyG,nuG] = size(G);
         D.d = ltipack.util.blkdiag(D.d,G);
         D.b = [D.b , zeros(nx,nuG)];
         D.c = [D.c ; zeros(nyG,nx)];
         D.Delay.Input = [D.Delay.Input ; zeros(nuG,1)];
         D.Delay.Output = [D.Delay.Output ; zeros(nyG,1)];
         D.u0 = [D.u0 ; zeros(nuG,1)];
         D.y0 = [D.y0 ; zeros(nyG,1)];
         if nfd>0
            % Push delay channels to the bottom
            yperm = [1:ny-nfd , ny+1:ny+nyG , ny-nfd+1:ny];
            uperm = [1:nu-nfd , nu+1:nu+nuG , nu-nfd+1:nu];
            D.b = D.b(:,uperm);
            D.c = D.c(yperm,:);
            D.d = D.d(yperm,uperm);
            D.u0 = D.u0(uperm,:);
            D.y0 = D.y0(yperm,:);
         end
      end
      
      function [D,SingularFlag] = invLFT(D,nyu,~)
         % Computes Di such that inv(LFT(D,B)) = LFT(Di,B). The LFT model is
         % invertible iff H11(s) has full normal rank. NYU is the number of
         % external I/Os.
         
         % Error if inverse is not causal
         if any(D.Delay.Input(1:nyu)) || any(D.Delay.Output(1:nyu))
            error(message('Control:transformation:inv1'))
         end
         
         % Invert mapping from first NYU inputs to first NYU outputs
         a = D.a; b = D.b; c = D.c; d = D.d; e = D.e; %#ok<*PROPLC>
         [rs,cs] = size(d);
         nx = size(a,1);
         nfd = numel(D.Delay.Internal);
         u0 = D.u0;  y0 = D.y0;
         % Swap input and output offsets
         D.u0 = [y0(1:nyu,:);D.u0(nyu+1:end,:)];
         D.y0 = [u0(1:nyu,:);D.y0(nyu+1:end,:)];

         if nx==0 && nfd==0
            % Static LFT: inverse is static
            di = blkdiag(zeros(nyu),d(nyu+1:rs,nyu+1:cs)) - ...
               [eye(nyu);d(nyu+1:rs,1:nyu)] * ...
               matlab.internal.math.nowarn.mldivide(d(1:nyu,1:nyu),...
               [-eye(nyu) d(1:nyu,nyu+1:cs)]);
            if allfinite(di)
               SingularFlag = false;
               D.d = di;
            else
               SingularFlag = true;
            end
         else
            % Build DAE model of inverse LFT
            if isempty(e)
               e = eye(nx);
            end
            ei = blkdiag(e,zeros(nyu));
            ai = [a b(:,1:nyu);c(1:nyu,:) d(1:nyu,1:nyu)];
            p = dmperm((ai~=0) | (ei~=0));
            if any(p==0)
               % Pencil Ai-s*Ei is structurally singular (LFT model has
               % deficient normal rank)
               SingularFlag = true;
            else
               % Augment state vector with zeta=u
               SingularFlag = false;
               D.a = ai;  D.e = ei;
               D.b = [zeros(nx,nyu) b(:,nyu+1:cs); -eye(nyu) d(1:nyu,nyu+1:cs)];
               D.c = [zeros(nyu,nx) eye(nyu);c(nyu+1:rs,:) d(nyu+1:rs,1:nyu)];
               D.d = blkdiag(zeros(nyu),d(nyu+1:rs,nyu+1:cs));
               D.x0 = [D.x0 ; u0(1:nyu,:)];
               D.dx0 = [D.dx0 ; zeros(nyu,1)];
               D.Scaled = false;
               if nargin>2
                  % inv(sys,'min'): Try eliminating extra states
                  [D.a,br,D.c,dr,D.e] = elimAX(D.a,[D.b D.dx0],D.c,[D.d D.y0],D.e,nx+1:nx+nyu);
                  D.b = br(:,1:end-1);  D.dx0 = br(:,end);
                  D.d = dr(:,1:end-1);  D.y0 = dr(:,end);
                  D.x0 = D.x0(1:nx,:);
                  if ~isfinite(D)
                     error(message('Control:transformation:inv3'))
                  end
               else
                  % Account for extra states in state info
                  if ~isempty(D.StateName)
                     D.StateName = [D.StateName ; strings(nyu,1)];
                  end
                  if ~isempty(D.StatePath)
                     D.StatePath = [D.StatePath ; strings(nyu,1)];
                  end
                  if ~isempty(D.StateUnit)
                     D.StateUnit = [D.StateUnit ; strings(nyu,1)];
                  end
               end
            end
         end
      end
      
      function D = applyOffset(D,S)
         % Compute D-S where S is a ny-by-nu matrix
         [ny,nu] = size(S);
         D.d(1:ny,1:nu) = D.d(1:ny,1:nu)-S;
      end
      
      function D = applyTransform(D,R)
         % Compute LFT(R,D)
         [ny,nu] = iosize(D);
         D = lft(D.createGain(R),D,nu+1:nu+ny,ny+1:ny+nu,1:nu,1:ny);
      end
      
   end
   
   
   %% RCTB methods
   methods
      
      function w = sigmaGrid(D,Focus)
         % Generates suitable frequency grid for RCTB MIMO analysis functions.
         % Assumption: D is free of internal delays
         %             FOCUS is either empty or
         hw = ctrlMsgUtils.SuspendWarnings; %#ok<NASGU>
         Ts = abs(D.Ts);
         if isempty(Focus)
            Focus = [0,Inf];
         elseif iscell(Focus)
            Focus = [Focus{:}];
         end
         % Generate grid from poles and transmission zeros
         OPTS = struct('Focus',Focus,'Negative',~isreal(D),'Decades',false);
         w = freqgrid({tzero(D)},{pole(D)},Ts,4,OPTS);
         % Trim grid in auto-focus mode
         wneg = localTrimGrid(-flip(w(w<0,:),1),Focus,Ts);
         wpos = localTrimGrid(w(w>0,:),Focus,Ts);
         w = [-flip(wneg,1) ; wpos];
      end
      
   end
   
   
   methods(Static)
            
      D = rssnb(n,p,m,Ts,wmax)
      varargout = timegrid(a,b,c,x0,varargin)
      [a,b,c] = cancel01(a0,b0,c0,d,Ts,varargin)
      
      function D = array(size)
         % Create a ssdata array of a given size
         D = ltipack.ssdata.newarray(size);
      end
      
      function D = default()
         % Fast construction of default 0x0 ssdata
         D = ltipack.ssdata;
         D.dx0 = zeros(0,1);   D.x0 = zeros(0,1);
         D.u0 = zeros(0,1);   D.y0 = zeros(0,1);
         D.Ts = 0;
         D.Delay = ltipack.utDelayStruct(0,0,true);
      end
      
      function D = loadobj(S)
         % Load filter for @ssdata
         if isstruct(S)
            % R2013b: Tags removed
            % R2024a: f,g removed
            D = ltipack.transferData(ltipack.ssdata,S);
         else
            D = S;
         end
         if isfield(D.Delay,'IO')
            % Pre-R2009b: Delay structure had IO field for all model types
            D.Delay = rmfield(D.Delay,'IO');
         end
         if all(strcmp(D.StateName,''))
            % R2010a and beyond: store blank state names as []
            D.StateName = strings(0,1);
         else
            % R2017b: [] replaced by 0x1 string array
            D.StateName = string(D.StateName(:));
         end
         D.StateUnit = string(D.StateUnit(:));
         if isequal(D.dx0,[])
            % R2024a: Offset support
            nx = size(D.a,1);
            [ny,nu] = size(D.d);
            D.dx0 = zeros(nx,1);  D.x0 = zeros(nx,1);
            D.u0 = zeros(nu,1);  D.y0 = zeros(ny,1);
         end
      end
            
      function G = getThiranAppx(fdelay,Ts,delayOK)
         % Returns Thiran filter for fractional delay approximation.
         n = numel(fdelay);
         a = []; b = []; c = []; d = [];
         for ct = 1:n
            if rem(fdelay(ct),1)>0
               [ax,bx,cx,dx] = thirancoef(fdelay(ct),1);
            else
               ax = []; bx=zeros(0,1); cx=zeros(1,0); dx =1;
            end
            a = blkdiag(a,ax);
            b = blkdiag(b,bx);
            c = blkdiag(c,cx);
            d = blkdiag(d,dx);
         end
         if delayOK % DelayModeling = 'delay'
            % Realize dynamics as internal delays
            [ny,nu] = size(d); nx = size(a,1);
            G = ltipack.ssdata([],zeros(0,nu+nx),zeros(ny+nx,0),[d c;b a],[],Ts);
            G.Delay = ltipack.utDelayStruct(ny,nu,true);
            G.Delay.Internal = ones(nx,1);
         else
            % Realize dynamics as states
            G = ltipack.ssdata(a,b,c,d,[],Ts);
         end
      end
      
   end

   
end


function w = localTrimGrid(w,Focus,Ts)
% Trim grid of positive frequencies
AutoFocus = [Focus(1)==0 , Focus(2)==Inf];
if any(AutoFocus)
   % Remove sparse leading/trailing portions of the grid
   idx = find(diff(log10(w))<1.01);
   if numel(idx)>2
      if all(AutoFocus)
         w = w(idx(1):idx(end)+1);
      elseif AutoFocus(1)
         w = w(idx(1):end);
      else
         w = w(1:idx(end)+1);
      end
   end
end
% Clip grid to just before Nyquist frequency in discrete time
if Ts>0
   nf = 0.99999*pi/Ts;
   if numel(w)>1 && w(1)<nf && w(end)>nf
      w = [w(w<nf) ; nf];
   end
end
end
