classdef (Hidden = true) spssdata < ltipack.AbstractSS
   % Class definition for @spssdata (sparse state-space model data)
   
   %   Author(s): P. Gahinet
   %   Copyright 2020 The MathWorks, Inc.
   properties
      StateInfo
      SolverConfig
   end
   
   methods
      function D = spssdata(a,b,c,d,e,Ts)
         % Constructs @spssdata instance
         if nargin==6
            nx = min(size(a));
            ny = max(size(c,1),size(d,1));
            nu = max(size(b,2),size(d,2));
            D.a = sparse(a);
            D.b = sparse(b);
            D.c = sparse(c);
            D.d = sparse(d);
            if isempty(e)
               D.e = [];
            else
               D.e = sparse(e);
            end
            D.dx0 = zeros(nx,1);  D.x0 = zeros(nx,1);
            D.u0 = zeros(nu,1);  D.y0 = zeros(ny,1);
            D.Ts = Ts;
            D.Delay = ltipack.utDelayStruct(ny,nu,true);
            % Type=1: states and dofs
            % Type=2: interface variables
            % Type=3: signals
            if nx>0
               D.StateInfo = struct('Type',1,'Name',"",'Size',nx);
            end
         end
      end
      
      function e = getE(D)
         e = D.e;
         if isempty(e)
            e = speye(size(D.a));
         end
      end
      
      function [D,SingularFlag] = sparss(D)
         % Conversion to SPARSS
         SingularFlag = false;
      end
      
      function sys = utCreateLTI(D)
         % Creates LTI object from low-level @xxxdata representation
         sys = sparss.make(D);
      end
      
      function D = createGain(Dref,G)
         % Wraps static gain into @spssdata object
         [ny,nu] = size(G);
         D = ltipack.spssdata(sparse(0,0),sparse(0,nu),sparse(ny,0),G,[],Dref.Ts);
      end
      
      function boo = issparse(~)
         boo = true;
      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 = sparse(0,0);
         end
         if Ne==0
            se = [0 0];  D.e = [];
         end
         if any(sb==0)
            sb = [Nx Nu];  D.b = sparse(Nx,Nu);
         end
         if any(sc==0)
            sc = [Ny Nx];  D.c = sparse(Ny,Nx);
         end
         if any(sd==0) || isequal(D.d,sparse(0))
            % Also allow for scalar expansion when d=0
            % Note: syntax sparss([],[],[],0) returns a zero gain system
            sd = [Ny Nu];  D.d = sparse(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
         
         % StateInfo
         if Nx>0 && sum([D.StateInfo.Size])~=Nx
            if isscalar(D.StateInfo)
               D.StateInfo.Size = Nx;
            else
               % Reset
               D.StateInfo = struct('Type',1,'Name',"",'Size',Nx);
            end
         end
         
         % Checks for NaNs
         if ~allfinite(D.d)
            D = ltipack.spssdata(...
               sparse(0,0),sparse(0,Nu),sparse(Ny,0),sparse(nan(Ny,Nu)),[],D.Ts);
         end
      end
            
      function nx = order(D)
         % Computes order of SPARSS models.
         nx = size(D.a,1);
      end

      function [D,xkeep] = sminreal(D)
         [D,xkeep] = sminreal@ltipack.AbstractSS(D);
         D.StateInfo = ltipack.util.selectStateInfo(D.StateInfo,xkeep);
      end
      
      function D = xperm(D,perm)
         % Applies state permutation
         D = xperm@ltipack.AbstractSS(D,perm);
         if ~isscalar(D.StateInfo)
            D.StateInfo = struct('Type',1,'Name',"",'Size',size(D.a,1));
         end
      end
      
      function [D,perm] = xsort(D)
         % Sort and consolidates aggregate state info.
         [D.StateInfo,perm] = ltipack.util.sortStateInfo(D.StateInfo);
         if ~isempty(perm)
            D.a = D.a(perm,perm);
            D.b = D.b(perm,:);
            D.c = D.c(:,perm);
            if ~isempty(D.e)
               D.e = D.e(perm,perm);
            end
            perm = perm(:);
         end
      end
      
      function D = cplx2real(D)
         D = cplx2real@ltipack.AbstractSS(D);
         % Adjust state info
         Sr = D.StateInfo;
         Si = Sr;
         for ct=1:numel(Sr)
            if strlength(Sr(ct).Name)>0
               Sr(ct).Name = Sr(ct).Name + " (real)";
               Si(ct).Name = Si(ct).Name + " (imag)";
            end
         end
         D.StateInfo = [Sr;Si];
      end
      
      function [D,xinit] = checkCompatibility(D,PlotType,xinit,varargin)
         % Check compatibility of model with given plot type
         switch PlotType
            case {'step','impulse','lsim','initial'}
               nx = size(D.a,1);
               % Check initial condition
               if ischar(xinit)
                  xinit = D.x0;
               elseif numel(xinit)~=nx && any(xinit)
                  error(message('Control:analysis:timeresp1'))
               end
               % Plot-specific restriction
               switch PlotType
                  case {'step','impulse'}
                     if ~isempty(xinit)
                        [DP,DY] = hasDelayDynamics(D,'pole');
                        if (DP || DY)
                           % No state delays with unsteady IC
                           error(message('Control:analysis:step3'))
                        end
                     end
                  case 'initial'
                     if isempty(xinit) && nx>0
                        % Initial state required
                        error(message('Control:analysis:rfinputs04'))
                     end
               end
            case {'sector'}
               % System must be delay free in CT
               if D.Ts==0 && hasdelay(D)
                  error(message('Control:analysis:getSectorIndex9'))
               end
         end
      end
                  
      function D = diagrep(D,k)
         % Replicates model along the diagonal.
         D = diagrep@ltipack.AbstractSS(D,k);
         D.StateInfo = repmat(D.StateInfo,[k 1]);
      end
      
      function D = utGrowIO(D,ny,nu)
         % Grows I/O size of SPARSS model (used in SYS(I,J)=RHS)
         [ny0,nu0] = iosize(D);
         if ny>ny0 || nu>nu0
            nx = size(D.a,1);
            if nu>nu0
               % Resize B, D, and input delay
               [rs,cs] = size(D.d);
               D.b = [D.b(:,1:nu0) , sparse(nx,nu-nu0) , D.b(:,nu0+1:cs)];
               D.d = [D.d(:,1:nu0) , sparse(rs,nu-nu0) , D.d(:,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
               [rs,cs] = size(D.d);
               D.c = [D.c(1:ny0,:) ; sparse(ny-ny0,nx) ; D.c(ny0+1:rs,:)];
               D.d = [D.d(1:ny0,:) ; sparse(ny-ny0,cs) ; D.d(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
         end
      end
      
      function Dss = full(D)
         % Conversion to full-data state space.
         Dss = ltipack.ssdata(...
            full(D.a),full(D.b),full(D.c),full(D.d),full(D.e),D.Ts);
         Dss.dx0 = D.dx0;
         Dss.x0 = D.x0;
         Dss.u0 = D.u0;
         Dss.y0 = D.y0;
         Dss.Delay = D.Delay;
      end
      
      function OPTS = getSolverConfig(D)
         OPTS = D.SolverConfig;
         if isempty(OPTS)
            OPTS = ltioptions.sparss; % defaults
         end
      end
      
      % Unsupported operations
      function varargout = ss(varargin)
         error(message('Control:transformation:sparse2dense'))
      end
      function varargout = tf(varargin)
         error(message('Control:transformation:sparse2dense'))
      end
      function varargout = zpk(varargin)
         error(message('Control:transformation:sparse2dense'))
      end
      function varargout = pole(varargin)
         error(message('Control:analysis:NoSparseDynamics','sparss'))
      end
      function varargout = zero(varargin)
         error(message('Control:analysis:NoSparseDynamics','sparss'))
      end
      function varargout = sectorbnd(varargin)
         error(message('Control:analysis:NoSparseSupportFRD','sparss'))
      end
      function varargout = ifpofp(varargin)
         error(message('Control:analysis:NoSparseSupportFRD','sparss'))
      end
      function varargout = getSectorCrossover(varargin)
         error(message('Control:analysis:NoSparseSupportFRD','sparss'))
      end
      function varargout = bandwidth(varargin)
         error(message('Control:analysis:NoSparseSupportFRD','sparss'))
      end
      function wc = getGainCrossover(varargin)
         error(message('Control:analysis:NoSparseSupportFRD','sparss'))
      end
      function n = normh2(varargin)
         error(message('Control:analysis:norm4'))
      end      
      function varargout = norminf(varargin)
         error(message('Control:analysis:NoSparseSupportFRD','sparss'))
      end
      function varargout = allmargin(varargin)
         error(message('Control:analysis:NoSparseSupportFRD','sparss'))
      end

      
      %------------ Delay management ------------------
      
      function D = delay2z(Dref,Delays)
         % Returns model for diag(z^(-DELAYS))
         [a,b,c,d] = ltipack.util.realizeDelays(Delays);
         D = ltipack.spssdata(a,b,c,d,[],Dref.Ts);
      end
      
      function D = zeroInternalDelay(D)
         % Sets all internal delays to zero and models resulting feedback
         % loops as algebraic states.
         nfd = length(D.Delay.Internal);
         if nfd>0
            [a,b1,b2,c1,c2,d11,d12,d21,d22,e] = getBlockData(D);
            [ny,nu] = size(d11);
            D.a = [a b2;c2 d22-speye(nfd)];
            D.e = ltipack.util.blkdiag(e,sparse(nfd,nfd));
            D.b = [b1 ; d21];
            D.c = [c1 , d12];
            D.d = d11;
            w0 = D.u0(nu+1:nu+nfd,:);
            z0 = D.y0(ny+1:ny+nfd,:);
            D.dx0 = [D.dx0;z0-w0];
            D.x0 = [D.x0;w0];
            D.u0 = D.u0(1:nu,:);
            D.y0 = D.y0(1:ny,:);
            D.Delay.Internal = zeros(0,1);
         end
      end
      
   end
     
   
   methods(Static)
            
      function D = array(size)
         % Create a ssdata array of a given size
         D = ltipack.spssdata.newarray(size);
      end
      
      function D = default()
         % Fast construction of default 0x0 spssdata
         D = ltipack.spssdata;
         D.Ts = 0;
         D.Delay = ltipack.utDelayStruct(0,0,true);
      end
      
      function D = loadobj(S)
         % Load filter for @spssdata
         if isstruct(S)
            % R2024a: f,g removed
            D = ltipack.transferData(ltipack.spssdata,S);
         else
            D = S;
         end
         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 [x0,xp0] = impulseStateUpdate(A,E,f,b,x0,xp0)
         % Integrates the DAE
         %    E x'(t) = A x(t) + f + b * delta(t) ,  x(0-) = x0
         % through a Dirac impulse. Returns X0=x(0+) and the new state
         % derivative XP0=x'(0+). The DAE must be of index 0 or 1 
         % structurally.
         if isempty(E)
            x0 = x0 + b;
            xp0 = A*x0+f;
         else
            % For index-1 DAE
            %    E11 x1' = A11 x1 + A12 x2 + f1 + b1 * delta(t)
            %         0  = A21 x1 + A22 x2 + f2 + b2 * delta(t)
            % with E11,A22 nonsingular, the dx0 change in x0 satisfies
            %    E11 dx01 = b1 - A12 * (A22\b2)
            %    0 = A21 dx01 + A22 dx02
            % based on the ODE obtained by eliminating 
            %    x2 = -A22\(A21*x1 + f2 + b2*delta(t)).
            [p,q,~,~,cc,rr] = dmperm(E);
            if rr(1)==rr(2) && cc(4)==cc(5)
               % E(p,q) = E23 or [0 E23;0 0] with E23 full s-rank
               nz = cc(3)-cc(1);
               if nz>0
                  % E can be permuted to [E11 0;0 0] with full s-rank E11
                  nx = size(A,1);
                  i1 = p(1:nx-nz); i2 = p(nx-nz+1:nx);
                  j1 = q(nz+1:nx); j2 = q(1:nz);
                  A22 = A(i2,j2);
                  aux = matlab.internal.math.nowarn.mldivide(A22,b(i2,:));
                  SOLVER = ltipack.spssdata.linearSolver(E(i1,j1));
                  dx01 = SOLVER(b(i1,:)-A(i1,j2)*aux);
                  x0(j1) = x0(j1) + dx01;
                  x0(j2) = x0(j2) - matlab.internal.math.nowarn.mldivide(A22,A(i2,j1)*dx01);
                  xp0(j1) = SOLVER(A(i1,:)*x0+f(i1,:));  % E11\(A11*x01+A12*x02+f1)
               else
                  % E full rank
                  SOLVER = ltipack.spssdata.linearSolver(E);
                  x0 = x0 + SOLVER(b);
                  xp0 = SOLVER(A*x0+f);
               end
            else
               % Index > 1
               x0 = NaN;  xp0 = NaN; 
            end
            if ~(allfinite(x0) && allfinite(xp0))
               error(message('Control:analysis:ImpulseResponseIndexOne'))
            end
            % norm(E*xp0-(A*x0+f),'fro')/norm(E*xp0,'fro')
         end
         x0 = full(x0);  xp0 = full(xp0);
      end
      
      function SOLVER = linearSolver(A)
         % DAE solvers: Selects linear solver for Ax=b based on the 
         % structure of A.
         % Note: For single RHS, dense banded backsolves were found to be
         % slower than LU-based backsolves.
         n = size(A,1);
         [kl,ku] = bandwidth(A);
         nnzFullBand = n*(kl+ku+1)-kl*(kl+1)/2-ku*(ku+1)/2;
         if kl==0 && ku==0
            % A diagonal
            [~,~,dA] = find(A);
            SOLVER = @(b) dA(:).\b;  % dA(:) because FIND returns s=[] for sparse(0,0)
         elseif kl==0 && all(diag(A) ~= 0)
            % A upper triangular
            SOLVER = @(b) matlab.internal.decomposition.builtin.sparseTriangSolve(A,b,'upper',false);
         elseif ku==0 && all(diag(A) ~= 0)
            % A lower triangular
            SOLVER = @(b) matlab.internal.decomposition.builtin.sparseTriangSolve(A,b,'lower',false);
         else
            % Use tridiagonal solver when possible
            if kl==1 && ku==1 && isreal(A) && nnz(A)==nnzFullBand
               [LU,Success] = matlab.internal.decomposition.builtin.tridiagFactor(A);
               if Success
                  SOLVER = @(b) matlab.internal.decomposition.builtin.tridiagSolve(LU,full(b),false);
                  return
               end
            end
            % Otherwise default to LU solver with no iterative refinement
            LU = matlab.internal.decomposition.builtin.UMFPACKWrapper(A, 0.1, 0.001, true, 0);
            SOLVER = @(b) solve(LU,b,false);
         end
      end

   end
   
end

