classdef (Hidden = true) mechdata < ltipack.ltidata
   % Class definition for @mechdata (sparse second-order model data)
   
   %   Author(s): P. Gahinet
   %   Copyright 2020 The MathWorks, Inc.
   
   % CT: State is x(t) = [q(t) ; q'(t)]
   %      M q'' + C q' + K q = B1 u + B2 w
   %      y = F1 q + G1 q' + D11 u + D12 w
   %      z = F2 q + G2 q' + D21 u + D22 w
   %      w(t) = z(t-tau)
   %
   % DT: Three-term recursion, state is x[k] = [q[k] ; q[k+1]]
   %      M q[k+2] + C q[k+1] + K q[k] = B1 u[k] + B2 w[k]
   %      y[k] = F1 q[k] + G1 q[k+1] + D11 u[k] + D12 w[k]
   %      z[k] = F2 q[k] + G2 q[k+1] + D21 u[k] + D22 w[k]
   %      w[k] = z(k-tau)   
   
   properties
      m  % mass
      c  % damping
      k  % stiffness
      b
      f  % multiplies q
      g  % multiplies q'
      d  % feedthrough
      StateInfo
      SolverConfig
   end
   
   methods
      
      function Data = mechdata(M,C,K,B,F,G,D,Ts)
         % Constructs @mechdata instance
         if nargin==8
            [ny,nu] = size(D);
            if isempty(M)
               Data.m = [];
            else
               Data.m = sparse(M);
            end
            Data.c = sparse(C);  Data.k = sparse(K);
            Data.b = sparse(B);  Data.f = sparse(F);  Data.g = sparse(G);
            Data.d = sparse(D);
            Data.Ts = Ts;
            Data.Delay = ltipack.utDelayStruct(ny,nu,true);
            nq = size(K,1);
            if nq>0
               Data.StateInfo = struct('Type',1,'Name',"",'Size',nq);
            end
         end
      end
      
      function [D,SingularFlag] = mechss(D)
         % Conversion to MECHSS
         SingularFlag = false;
      end
      
      function m = getM(D)
         m = D.m;
         if isempty(m)
            m = speye(size(D.k));
         end
      end
      
      function sys = utCreateLTI(D)
         % Creates LTI object from low-level @xxxdata representation
         sys = mechss.make(D);
      end
      
      function D = createGain(Dref,G)
         % Wraps static gain into @spssdata object
         [ny,nu] = size(G);
         D = ltipack.mechdata([],sparse(0,0),sparse(0,0),sparse(0,nu),...
            sparse(ny,0),sparse(ny,0),G,Dref.Ts);
      end
      
      function boo = issparse(~)
         boo = true;
      end
      
      function boo = isstatic(D)
         % Returns TRUE if model is a pure gain.
         if isempty(D.m)
            boo = isempty(D.k) && ~hasdelay(D);
         else
            boo = nnz(D.m)==0 && nnz(D.c)==0 && nnz(D.g)==0 && ~hasdelay(D);
         end
      end
      
      function boo = isreal(D)
         % Returns TRUE if model has real data.
         boo = isreal(D.m) && isreal(D.c) && isreal(D.k) && ...
            isreal(D.b) && isreal(D.f) && isreal(D.g) && isreal(D.d);
      end
      
      function D = checkData(D)
         % Checks that matrix data is consistent and NaN free.         
         % Get dimensions of M,D,K,B,C,F matrices
         sm = size(D.m);
         sc = size(D.c);
         sk = size(D.k);
         sb = size(D.b);
         sf = size(D.f);
         sg = size(D.g);
         sd = size(D.d);
         
         % Determine I/O size for matrix data
         Ny = max([sf(1) sg(1) sd(1)]);
         Nu = max(sb(2),sd(2));
         Nq = min(sk);
         
         % Handle [] shortcuts
         if any(sm==0)
            sm = [0 0];  D.m = []; % support M=[] for M=I
         end
         if any(sc==0)
            sc = [Nq Nq];  D.c = sparse(Nq,Nq);
         end
         if any(sb==0)
            sb = [Nq Nu];  D.b = sparse(Nq,Nu);
         end
         if any(sf==0)
            sf = [Ny Nq];  D.f = sparse(Ny,Nq);
         end
         if any(sg==0) || isequal(D.g,0)
            sg = [Ny Nq];  D.g = sparse(Ny,Nq);
         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 = sparse(Ny,Nu);
         end
         
         % Check compatibility of I/O and state dimensions
         if sk(1)~=sk(2) || sm(1)~=sm(2) || sc(1)~=sc(2)
            error(message('Control:ltiobject:mechss1'))
         elseif ~isequal(sk,sc) || (sm(1)>0 && ~isequal(sk,sm))
            error(message('Control:ltiobject:mechss2'))
         elseif Nq~=sb(1)
            error(message('Control:ltiobject:ssProperties1','K','B'))
         elseif Nq~=sf(2)
            error(message('Control:ltiobject:ssProperties2','K','F'))
         elseif Nq~=sg(2)
            error(message('Control:ltiobject:ssProperties2','K','G'))
         elseif sb(2)~=Nu || sd(2)~=Nu
            error(message('Control:ltiobject:ssProperties2','B','D'))
         elseif xor(sf(1)==Ny,sg(1)==Ny)
             error(message('Control:ltiobject:ssProperties1','F','G'))
         elseif xor(sf(1)==Ny,sd(1)==Ny)
             error(message('Control:ltiobject:ssProperties1','F','D'))
         end            
         
         % StateInfo
         if Nq>0 && sum([D.StateInfo.Size])~=Nq
            if isscalar(D.StateInfo)
               D.StateInfo.Size = Nq;
            else
               % Reset
               D.StateInfo = struct('Type',1,'Name',"",'Size',Nq);
            end
         end
         
         % Checks for NaNs
         if ~allfinite(D.d)
            K = sparse(0,0);
            D = ltipack.mechdata([],K,K,...
               sparse(0,Nu),sparse(Ny,0),sparse(Ny,0),nan(Ny,Nu),D.Ts);
         end
      end
      
      function D = scaleTime(D,sf)
         % Time vector scaling tnew = sf * told
         D = scaleTime@ltipack.ltidata(D,sf);
         if D.Ts==0
            % Reflect change in dx/dt
            D.m = sf^2 * D.m;
            D.c = sf * D.c;
         end
      end
      
      function D = uminus(D)
         % Computes -D.
         nfd = length(D.Delay.Internal);
         if nfd==0
            D.f = -D.f;
            D.g = -D.g;
            D.d = -D.d;
         else
            ny = size(D.d,1)-nfd;
            D.f(1:ny,:) = -D.f(1:ny,:);
            D.g(1:ny,:) = -D.g(1:ny,:);
            D.d(1:ny,:) = -D.d(1:ny,:);
         end
      end
      
      function D = conj(D)
         % Forms model with complex conjugate coefficients.
         D.m = conj(D.m);
         D.c = conj(D.c);
         D.k = conj(D.k);
         D.b = conj(D.b);
         D.f = conj(D.f);
         D.g = conj(D.g);
         D.d = conj(D.d);
      end
      
      function D = transpose(D)
         % Transposition of state-space models.
         if nnz(D.g)>0
            error(message('Control:transformation:Transpose5'))
         end
         D.m = D.m.';
         D.c = D.c.';
         D.k = D.k.';
         aux = D.b.';  D.b = D.f.';  D.f = aux;
         [ny,nq] = size(aux);
         D.g = sparse(ny,nq);
         D.d = D.d.';
         id = D.Delay.Input;
         D.Delay.Input = D.Delay.Output;
         D.Delay.Output = id;
      end

      function nx = order(D)
         % Computes order of MECHSS models.
         nq = size(D.k,1);
         if isempty(D.m)
            nx = 2*nq;
         else
            % See conversion to SPARSS for motivation
            nx = nq + sum(any(D.m,1) | any(D.g,1));
         end
      end
      
      function boo = isfinite(D)
         % Returns TRUE if the model data does not contain NaN's or Inf's.
         boo = allfinite(D.d);
      end
      
      function [ny,nu] = iosize(D)
         % Returns I/O size.
         [ny,nu] = size(D.d);  % NOTE: Data defines I/O size
         nfd = length(D.Delay.Internal);
         ny = ny-nfd;   nu = nu-nfd;
         if nargout<2
            ny = [ny nu];
         end
      end
      
      function D = downshift(D)
         % For scalar q, rewriting
         %   m q'' + c q' = b u , y = g q' + d u
         % as 
         %   m v' + c v = b u , y = g v + d u (v = q')
         % eliminates a pole/zero cancellation at s=0. DOWNSHIFT 
         % generalizes this process to q vectors.
         js = find(~(any(D.k,1) | any(D.f,1))); % zero columns of [K;F]
         if ~isempty(js)
            if isempty(D.m)
               D.m = speye(size(D.k));
            end
            D.k(:,js) = D.c(:,js);
            D.c(:,js) = D.m(:,js);
            D.m(:,js) = 0;
            D.f(:,js) = D.g(:,js);
            D.g(:,js) = 0;
         end
      end
      
      function boo = hasdelay(D)
         % Returns TRUE if model has delays.
         Delay = D.Delay;
         boo = any(Delay.Input) || any(Delay.Output) || any(Delay.Internal);
      end
      
      function boo = hasInternalDelay(D)
         % Returns T if model has internal delays.
         boo = any(D.Delay.Internal);
      end
      
      function D = checkDelay(D)
         % Checks validity and consistency of delay data.
         [Ny,Nu] = iosize(D);
         Ts = D.Ts;
         Delay = D.Delay;
         Delay.Input = ltipack.util.checkInputDelay(Delay.Input,Nu,Ts);
         Delay.Output = ltipack.util.checkOutputDelay(Delay.Output,Ny,Ts);
         Delay.Internal = ltipack.util.checkInternalDelay(Delay.Internal,Ts);
         D.Delay = Delay;
      end
      
      function [D,tau] = getInternalDelayModel(D)
         tau = D.Delay.Internal;
         nfd = numel(tau);
         D.Delay.Internal = zeros(0,1);
         D.Delay.Input = [D.Delay.Input ; zeros(nfd,1)];
         D.Delay.Output = [D.Delay.Output ; zeros(nfd,1)];
      end
      
      function D = setInternalDelayModel(D,tau)
         % Special LFT with pure delays (used by setDelayModel)
         nfd = numel(tau);
         [rs,cs] = iosize(D);
         nu = cs-nfd;  ny = rs-nfd;
         D.Delay.Internal = tau + D.Delay.Input(nu+1:cs,:) + D.Delay.Output(ny+1:rs,:);
         D.Delay.Input = D.Delay.Input(1:nu,:);
         D.Delay.Output = D.Delay.Output(1:ny,:);
      end
      
      function [D,xinit] = checkCompatibility(D,PlotType,xinit,varargin)
         % Check compatibility of model with given plot type
         switch PlotType
            case {'step','impulse','lsim','initial'}
               nq = size(D.k,1);
               % Check initial condition
               if ischar(xinit)
                  xinit = zeros(2*nq,1);  % x0(t)=0 for LTI
               elseif any(xinit) && ~any(numel(xinit)==[nq 2*nq])
                  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) && nq>0
                        % Initial state required
                        error(message('Control:analysis:rfinputs04'))
                     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
         end
      end
                  
      function [D,perm] = xsort(D)
         % Sort and consolidates aggregate state info S.
         [D.StateInfo,perm] = ltipack.util.sortStateInfo(D.StateInfo);
         if ~isempty(perm)
            D.k = D.k(perm,perm);
            if nnz(D.c)>0
               D.c = D.c(perm,perm);
            end
            if ~isempty(D.m) && nnz(D.m)>0
               D.m = D.m(perm,perm);
            end
            D.b = D.b(perm,:);
            D.f = D.f(:,perm);
            D.g = D.g(:,perm);
            perm = perm(:);
         end
      end
      
      function D = xperm(D,perm)
         % Applies state permutation
         nq = size(D.k,1);
         if numel(perm)~=nq
            error(message('Control:transformation:xperm',nq))
         end
         D.k = D.k(perm,perm);
         if nnz(D.c)>0
            D.c = D.c(perm,perm);
         end
         if ~isempty(D.m)
            D.m = D.m(perm,perm);
         end
         D.b = D.b(perm,:);
         D.f = D.f(:,perm);
         D.g = D.g(:,perm);
         if ~isscalar(D.StateInfo)
            D.StateInfo = struct('Type',1,'Name',"",'Size',size(D.k,1));
         end
      end
      
      function D = ioperm(D,yperm,uperm)
         % Applies I/O permutations
         D.Delay.Input = D.Delay.Input(uperm);
         D.Delay.Output = D.Delay.Output(yperm);
         nfd = length(D.Delay.Internal);
         if nfd>0
            [ny,nu] = size(D.d);
            yperm = [yperm(:) ; (length(yperm)+1:ny).'];
            uperm = [uperm(:) ; (length(uperm)+1:nu).'];
         end
         D.b = D.b(:,uperm);
         D.f = D.f(yperm,:);
         D.g = D.g(yperm,:);
         D.d = D.d(yperm,uperm);
      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
            nq = size(D.k,1);
            if nu>nu0
               % Resize B, D, and input delay
               [rs,cs] = size(D.d);
               D.b = [D.b(:,1:nu0) , sparse(nq,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;
            end
            if ny>ny0
               % Resize F, G, D, and output delay
               [rs,cs] = size(D.d);
               D.f = [D.f(1:ny0,:) ; sparse(ny-ny0,nq) ; D.f(ny0+1:rs,:)];
               D.g = [D.g(1:ny0,:) ; sparse(ny-ny0,nq) ; D.g(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;
            end
         end
      end
            
      function Dss = full(D)
         % Conversion to full-data state space.
         Dss = full(sparss(D));
      end
      
      function OPTS = getSolverConfig(D)
         OPTS = D.SolverConfig;
         if isempty(OPTS)
            OPTS = ltioptions.mechss;
         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 = pid(varargin)
         error(message('Control:transformation:sparse2dense'))
      end
      function varargout = pidstd(varargin)
         error(message('Control:transformation:sparse2dense'))
      end
      function varargout = pid2(varargin)
         error(message('Control:transformation:sparse2dense'))
      end
      function varargout = pidstd2(varargin)
         error(message('Control:transformation:sparse2dense'))
      end
      function varargout = pole(varargin)
         error(message('Control:analysis:NoSparseDynamics','mechss'))
      end
      function varargout = zero(varargin)
         error(message('Control:analysis:NoSparseDynamics','mechss'))
      end
      function varargout = sectorbnd(varargin)
         error(message('Control:analysis:NoSparseSupportFRD','mechss'))
      end
      function varargout = ifpofp(varargin)
         error(message('Control:analysis:NoSparseSupportFRD','mechss'))
      end
      function varargout = getSectorCrossover(varargin)
         error(message('Control:analysis:NoSparseSupportFRD','mechss'))
      end
      function varargout = bandwidth(varargin)
         error(message('Control:analysis:NoSparseSupportFRD','mechss'))
      end
      function wc = getGainCrossover(varargin)
         error(message('Control:analysis:NoSparseSupportFRD','mechss'))
      end
      function n = normh2(varargin)
         error(message('Control:analysis:norm4'))
      end
      function varargout = norminf(varargin)
         error(message('Control:analysis:NoSparseSupportFRD','mechss'))
      end
      function varargout = allmargin(varargin)
         error(message('Control:analysis:NoSparseSupportFRD','mechss'))
      end
      
      %-------------- Delay management
      
      function D = delay2z(Dref,Delays)
         % Returns model for diag(z^(-DELAYS))
         [a,b,c,d] = ltipack.util.realizeDelays(Delays); %#ok<*PROPLC>
         [ny,nx] = size(c);
         D = ltipack.mechdata(sparse(nx,nx),speye(nx),-a,b,c,sparse(ny,nx),d,Dref.Ts);
      end
      
      function D = zeroInternalDelay(D)
         % Returns model with all internal delays set to zero.
         nfd = length(D.Delay.Internal);
         if nfd>0
            [m,c,k,b1,b2,f1,f2,g1,g2,d11,d12,d21,d22] = getBlockData(D);
            nq = size(k,1);
            ny = size(d11,1);
            D.m = ltipack.util.blkdiag(m,sparse(nfd,nfd));
            D.c = [[c ; g2] sparse(nq+nfd,nfd)];
            D.k = [k -b2;f2 d22-speye(nfd)];
            D.b = [b1 ; -d21];
            D.f = [f1 , d12];
            D.g = [g1 , sparse(ny,nfd)];
            D.d = d11;
            D.Delay.Internal = zeros(0,1);
         end
      end

   end
      
   
   methods(Static)
      
      function D = array(size)
         % Create a mechdata array of a given size
         D = ltipack.mechdata.newarray(size);
      end
      
      function D = default()
         % Fast construction of default 0x0 ssdata
         D = ltipack.mechdata;
         D.Ts = 0;
         D.Delay = ltipack.utDelayStruct(0,0,true);
      end
      
      function [A,B,C,E,iNZ] = dss(M,C,K,B,F,G)
         % Descriptor form of second-order model.
         % State vector is [q ; q'(iNZ)] or [q[k] ; q[k+1](iNZ)] where iNZ selects
         % the nonzero columns of [M;G]
         nq = size(K,1);
         NoM = isequal(M,[]);
         if NoM
            % M=[] means M=I
            iNZ = (1:nq)';  iZ = zeros(0,1);
         else
            NZCOL = (any(M,1) | any(G,1))';
            iNZ = find(NZCOL);
            iZ = find(~NZCOL);
         end
         nqp = numel(iNZ);
         % E(:,[iNZ iZ nq+1:nq+nqp]) = [I 0 0;0 C(:,iZ) M(:,iNZ)]
         % A = [0 I_nqp;-K -C(:,iNZ)]
         nx = nq+nqp;
         if NoM
            E = [];
            A = [sparse(nq,nq) speye(nq) ; -K -C];
         else
            [iC,jC,sC] = find(C(:,iZ));  
            [iM,jM,sM] = find(M(:,iNZ));
            i = [(1:nqp)' ; iC+nqp ; iM+nqp];
            j = [iNZ ; iZ(jC,:) ; nq+jM];
            s = [ones(nqp,1) ; sC ; sM];
            E = sparse(i,j,s,nx,nx);
            A = [sparse(nqp,nq) speye(nqp) ; -K -C(:,iNZ)];
            if nqp==0 && isequal(E,speye(nx))
               E = [];
            end
         end
         % B = [0;B]
         [ib,jb,sb] = find(B);
         B = sparse(ib+nqp,jb,sb,nx,size(B,2));
         % C = [F G(:,iNZ)]
         C = [F G(:,iNZ)];
      end
      
   end
   
   % Protected methods (utilities)
   methods(Access=protected)
      
      function D = elimZeroDelay(D)
         % Folds zero internal delays into H(s) or H(z) (rational part)
         isZero = (D.Delay.Internal==0);
         idx = find(isZero);
         ndx = length(idx);
         if ndx>0
            % Close all LFT loops associated with zero internal delays
            % NOTE: This may increase the order or produce a NaN model.
            % Push non-appx delays to last row/columns of D.d
            [ny,nu] = iosize(D);
            dperm = [idx ; find(~isZero)];
            yperm = [(1:ny).' ; ny+dperm];
            uperm = [(1:nu).' ; nu+dperm];
            D.b = D.b(:,uperm);
            D.f = D.f(yperm,:);
            D.g = D.g(yperm,:);
            D.d = D.d(yperm,uperm);
            D.Delay.Internal(idx,:) = [];
            D.Delay.Input(nu+ndx,1) = 0;
            D.Delay.Output(ny+ndx,1) = 0;
            D = lft(D,createGain(D,eye(ndx)),nu+1:nu+ndx,ny+1:ny+ndx,1:ndx,1:ndx);
         end
      end
      
      function boo = utIsIOScaling(D)
         % True if D is of the form diag(dj*exp(-s*tauj))
         d = D.d; %#ok<*PROP>
         [ny,nu] = size(d);
         if isempty(D.k) && ny==nu && isempty(D.Delay.Internal)
            [i,j] = find(d);
            boo = isequal(i,j);
         else
            boo = false;
         end
      end
      
      function MaxDelay = getMaxDelay(D)
         % Estimates max I/O delay.
         MaxDelay = 0;
         for ct=1:length(D)
            Delay = D(ct).Delay;
            % Note: crude estimate only, does not take into
            % account structure of internal delay model.
            MaxDelay = max(MaxDelay,...
               max([0;Delay.Input]) + max([0;Delay.Output]) + sum(Delay.Internal));
         end
      end
                  
   end   
   
end

