%% [Aout,Bout,Cout,Dout,Ts,To,Ti,nDim,l,r,q,Aaa,Aobs,Cobs,Acon,Bcon]=scbsym(Ain,Bin,Cin,Din,'v'*)
%
% Computes the Special Coordinate Basis (SCB) for the LTI system given by the
% state-space description [Ain, Bin, Cin, Din] in symbolic form.
% Details on the SCB can be found in the book
% Chen, B. M. et. al.: Linear Systems Theory, A Structural Decomposition Approach. Birkh�user, Boston, 2004.
% 
% output arguments:
%   [Aout, Bout, Cout, Dout]    state-space representation in SCB form
%   [Ts, To, Ti]                state, output, and input transformation matrices
%   nDim=[na, nb, nc, nd]       number of states corresponding the finite zero structure, left invertibility structure, 
%                               right invertibility structure, and infinite zero structure, respectively
%   [l, r, q]                   indices of left invertibility structure, right invertibility structure, 
%                               and infinite zero structure, respectively
%   Aaa                         system matrix of zero dynamics
%   [Cobs, Aobs]                observability matrix pair 
%   [Acon, Bcon]                controllability matrix pair
%
% input arguments:
%   [Ain, Bin, Cin, Din]        symbolic state-space representation
%   'v'*                        activation of verbose output; default=off
% 
%  inputs marked with * are optional
%
%  Copyright by Oliver Janda 2017
% BSD License
% https://uk.mathworks.com/matlabcentral/fileexchange/67287-special-coordinate-basis-scb-of-lti-system

function [Aout,Bout,Cout,Dout,Ts,To,Ti,nDim,l,r,q,Aaa,Aobs,Cobs,Acon,Bcon]=scbsym(Ain,Bin,Cin,Din,varargin)


%% check input data

assert(isa(Ain,'sym') && isa(Bin,'sym') && isa(Cin,'sym') && isa(Din,'sym'),'Input matrices must be symbolic!')

p=size(Cin,1);
m=size(Bin,2);
n=size(Ain,1);

assert(size(Bin,1)==size(Cin,2) && size(Ain,1)==size(Ain,2) && size(Ain,1)==size(Bin,1) && size(Din,1)==size(Cin,1)...
                 && size(Din,2)==size(Bin,2),'Matrix dimensions are inconsistent!')
assert(rank([Cin Din])==p,'[C D] must be of full rank!')
assert(rank([Bin; Din])==m,'[B; D] must be of full rank!')

if nargin>4 && strcmp(varargin{1},'v')
    verbose=1;
else
    verbose=0;
end

           
%% handling of direct feedthrough

if any(any(Din~=0))
    U2=null(Din')';
    U1=[];
    count=1;
    while size(U1,1)<p-size(U2,1)
        Drow=Din(:,count)';
        if rank([U1; Drow])>rank(U1)
            U1=[U1; Drow];
        end
        count=count+1;
    end
    U=[U1; U2];
    
    Vinv1=U1*Din;
    Vinv2=null(Vinv1)';
    Vinv=[Vinv1; Vinv2];
    V=inv(Vinv);
    
    ToHat=inv(U);
    TiHat=V;
    
    % DhatTest=ToHat\Din*TiHat;
else
    ToHat=sym(eye(p));
    TiHat=sym(eye(m));    
end

m0=uint8(rank(Din));

Ahat=Ain;
Bhat=Bin*TiHat;
Chat=ToHat\Cin;
% Dhat=sym([eye(m0) zeros(m0,m-m0); zeros(p-m0,m)]);

B0=Bhat(:,1:m0);
B1=Bhat(:,m0+1:end);
C0=Chat(1:m0,:);
C1=Chat(m0+1:end,:);
A1=Ahat-B0*C0;

m1=size(B1,2);
p1=size(C1,1);


%% call of appropriate subfunction

if m1>0 && p1>0
    [Ts1,To1,Ti1,l,r,q,nDim,Lid]=scbspsym(A1,B1,C1);
elseif p1>0
    [Ts1,To1,l,nDim]=osdsym(A1,C1);
    Ti1=sym(eye(m1));
    r=[];
    q=[];
    Lid=[];
elseif m1>0
    [Ts1,Ti1,r,nDim]=csdsym(A1,B1); 
    To1=sym(eye(p1));
    l=[];
    q=[];
    Lid=[];
else
    Ts1=sym(eye(n));
    To1=[];
    Ti1=[];
    nDim=[n 0 0 0];
    l=[];
    r=[];
    q=[];
    Lid=[];
end

%% assembling overall coordinate transformations

Ts=simplify(Ts1);
To=simplify(ToHat*blkdiag(eye(m0),To1));
Ti=simplify(TiHat*blkdiag(eye(m0),Ti1));

Aout=simplify(Ts\Ain*Ts);
Bout=simplify(Ts\Bin*Ti);
Cout=simplify(To\Cin*Ts);
Dout=simplify(To\Din*Ti);

%% reduced controllability and observability matrices

na=nDim(1);
nb=nDim(2);
nc=nDim(3);
% nd=nDim(4);

md=length(q);
% mc=m-m0-md;

A1SCB=Ts1\A1*Ts1;
% B1SCB=Ts1\B1*Ti1;
% C1SCB=To1\C1*Ts1;

Aaa=simplify(A1SCB(1:na,1:na));
BcEca=A1SCB(na+nb+1:na+nb+nc,1:na);
Acc=A1SCB(na+nb+1:na+nb+nc,na+nb+1:na+nb+nc);
Aobs=simplify([Aaa zeros(na,nc); BcEca Acc]);

B0out=Bout(:,1:m0);
C0out=Cout(1:m0,:);

C0a=C0out(:,1:na);
C0c=C0out(:,na+nb+1:na+nb+nc);
Eda=sym(zeros(md,na));
Edc=sym(zeros(md,nc));
for count=1:md
   Eda(count,:)=A1SCB(na+nb+nc+sum(q(1:count)),1:na);
   Edc(count,:)=A1SCB(na+nb+nc+sum(q(1:count)),na+nb+1:na+nb+nc);
end
Cobs=simplify([C0a C0c; Eda Edc]);

LabCb=A1SCB(1:na,na+1:na+nb);
Abb=A1SCB(na+1:na+nb,na+1:na+nb);
Acon=simplify([Aaa LabCb; zeros(nb,na) Abb]);

B0a=B0out(1:na,:);
B0b=B0out(na+1:na+nb,:);
Lad=sym(zeros(na,md));
Lbd=sym(zeros(nb,md));
for count=1:md
   Lad(:,count)=A1SCB(1:na,na+nb+nc+1+sum(q(1:count-1)));
   Lbd(:,count)=A1SCB(na+1:na+nb,na+nb+nc+1+sum(q(1:count-1)));
end
Bcon=simplify([B0a Lad; B0b Lbd]);

%% computation of relative degrees

relDegSCB=zeros(1,p);

B0d=B0out(na+nb+nc+1:end,:);
relDegYd=q;
for count1=1:md
  LidBool=Lid{count1}~=0;
  for count2=1:size(LidBool,1)-1
     for count3=1:count1-1
        if LidBool(count2,count3) && count2+q(count3)<relDegYd(count1)
           relDegYd(count1)=count2+q(count3);
        end
     end
  end
  if ~isempty(B0d)
      B0i=B0d(sum(q(1:count1-1))+1:sum(q(1:count1)),:);
      B0iBool=B0i~=0;
      for count2=1:size(B0iBool,1)
          for count3=1:size(B0iBool,2)
              if B0iBool(count2,count3) && count2<relDegYd(count1)
                  relDegYd(count1)=count2;
              end
          end
      end
  end
end
relDegSCB(m0+1:m0+md)=relDegYd;

B0b=B0out(na+1:na+nb,:);
pb=p-m0-md;
relDegYb=inf(1,pb);
for count1=1:pb
  if md>0
      LidBool=Lbd(sum(l(1:count1-1))+1:sum(l(1:count1)),:)~=0;
      for count2=1:size(LidBool,1)
          for count3=1:size(LidBool,2)
              if LidBool(count2,count3) && count2+relDegYd(count3)<relDegYb(count1)
                  relDegYb(count1)=count2+relDegYd(count3);
              end
          end
      end
  end
  if ~isempty(B0b)
      B0i=B0b(sum(l(1:count1-1))+1:sum(l(1:count1)),:);
      B0iBool=B0i~=0;
      for count2=1:size(B0iBool,1)
          for count3=1:size(B0iBool,2)
              if B0iBool(count2,count3) && count2<relDegYb(count1)
                  relDegYb(count1)=count2;
              end
          end
      end
  end
end
relDegSCB(m0+md+1:end)=relDegYb;

relDeg=zeros(1,p);
for count=1:p
   relDeg(count)=min(relDegSCB(To(count,:)~=0));
end

%% verbose output

if verbose
    disp(' ')
    disp(' ')
    disp('The provided system has the following structural properties:')
    disp('============================================================')
    
    disp(' ')
    
    disp('Finite zero structure:')
    disp('----------------------')
    if nDim(1)>0
        disp('The finite zero structure corresponds to the eigenvalue structure of the matrix')
        Aaa
    else
        disp('The system has no invariant zeros.')
    end
    
    disp(' ')
    
    disp('Observability:')
    disp('--------------')
    if ~isempty(Aobs)
       disp('Observability of the system corresponds to the observability of the matrix pair')
       Aobs
       Cobs
    else
        disp('The system is structurally observable.')
    end
    
    disp(' ')
    
    disp('Controllability:')
    disp('----------------')
    if ~isempty(Acon)
        disp('Controllability of the system corresponds to the controllability of the matrix pair')
        Acon
        Bcon
    else
        disp('The system is structurally controllable.')
    end
    
    disp(' ')
    
    disp('Left invertibility structure:')
    disp('-----------------------------')
    if ~isempty(l)
       disp(['- The left invertibility structure is given by the set {' num2str(l) '}.' ])
       disp(['  Thus, the transformed system has ' num2str(length(l)) ' integrator chains without direct output of lengths ' num2str(l) '.'])
       disp('- The system is not right invertible.')
    else
       disp('The left invertibility structure is empty, i.e. the system is right invertible.')
    end
    
    disp(' ')
    
    disp('Right invertibility structure:')
    disp('------------------------------')
    if ~isempty(r)
       disp(['- The right invertibility structure is given by the set {' num2str(r) '}.' ])
       disp(['  Thus, the transformed system has ' num2str(length(r)) ' integrator chains without direct input of lengths ' num2str(r) '.'])
       disp('- The system is not left invertible.')
    else
       disp('The right invertibility structure is empty, i.e. the system is left invertible.')
    end
    
    disp(' ')
    
    disp('Infinite zero structure:')
    disp('------------------------')
    if ~isempty(q)
       disp(['- The infinite zero structure is given by the set {' num2str(q) '}.' ])
       disp(['  Thus, the system has ' num2str(length(q)) ' infinite zeros of orders ' num2str(q) '.'])
    else
       disp('- The system has no infinite zeros of order greater than zero.')
    end
    if m0>0
       disp(['The system has ' num2str(m0) ' infinite zeros of order zero.'])
    else
       disp('The system has no infinite zeros of order zero.')
    end
    
    disp(' ')
    disp('Normal rank:')
    disp('------------')
    disp(['The normal rank of the system is ' num2str(m0+md) '.'])
    
    disp(' ')
    
    disp('Relative degrees:')
    disp('-----------------')
    disp(['The relative degrees of the outputs of the original system are ' num2str(relDeg) '.'])
    
    disp(' ')
end

end

%% subfunctions

% subfunction SCB for strictly proper systems
function [Ts,To,Ti,l,r,q,nDim,Lid]=scbspsym(A,B,C)

%% SCB step 1 - initializiation

n=size(A,1);
p=size(C,1);
m=size(B,2);

Zi=cell(p,1);
for count=1:p
    Zi{count}=C(count,:);
end

f=ones(p,1);

Z=C;

Zd=sym([]);
Wd=sym([]);
Zb=sym([]);

v=0;
w=0;

%% SCB step 2 - repetitive differentiation of system output

while any(f)
    for count1=1:p
        if f(count1)==1
            alphai=size(Zi{count1},1);
            Cialphai=Zi{count1}(end,:);
            if rank([Wd; Cialphai*B])>rank(Wd)  % case 1
                f(count1)=0;
                Zd=[Zd; Zi{count1}];
                Wd=[Wd; Cialphai*B];
                v=v+1;
                q(v)=alphai;
            else                                % case 2
                Acoeff=[];
                for count2=1:v
                    Acoeff=[Acoeff (Zd(sum(q(1:count2)),:)*B)'];
                end
                bcoeff=(Cialphai*B)';
                if v>0
                    betai=Acoeff\bcoeff;
                end
                
                Chati=sym(zeros(alphai+1,n));
                for count2=1:alphai
                    for count3=0:v
                        if count3==0
                            Chati(count2,:)=Zi{count1}(count2,:);
                        else
                            if q(count3)-alphai+count2>0
                                Chati(count2,:)=Chati(count2,:)-betai(count3)*Zd(sum(q(1:count3-1))+q(count3)-alphai+count2,:);
                            end
                        end
                    end
                end
                Chati(end,:)=Chati(end-1,:)*A;
                
                if rank([Z; Chati(end,:)])>rank(Z)  % subcase 2.1
                    Z=[Z; Chati(end,:)];
                    Zi{count1}=Chati;
                else                                % subcase 2.2
                    f(count1)=0;
                    Zi{count1}=Chati(1:end-1,:);
                    Zb=[Zb; Zi{count1}];
                    w=w+1;
                    l(w)=alphai;
                end  
            end
        end
    end
end

%% SCB step 3 - interim transformations

md=v;
% mc=m-md;
pb=w;
nd=sum(q);
if exist('l','var')
    nb=sum(l);
else
    nb=0;
end
n0=n-nd-nb;

Zbd=[Zb; Zd];


Z0=null(Zbd)';
% if test
% Z0=sym([0 -1  0  0  0  0  0  0  0  0  0  0  0;
%     0  1  0  0  0  0  0  0  0  1  0  0  0
%     0  0  0  0  0  0  1  0  0  0  0  0  0
%     0  0  1  0  0  0  0  1  0  1  0  0  0
%     0  1  0  0  0  0  0  0  1  1  0  0  0]);
% end

S=[Z0; Zbd];

Wc=null(Wd)';
% if test
% Wc=sym([0  0  0  1;
%     0  1  1  0]);
% end

W=[Wd; Wc];

M=sym(zeros(p));
for count1=1:v
    Acoeff=C(:,1:p)';                       % take at least p columns
    count2=p+1;
    while rank(Acoeff)<p                    % add additional columns until Acoeff is of full rank
        Acoeff=[Acoeff; C(:,count2)'];
        count2=count2+1;
    end
    bcoeff=Zd(sum(q(1:count1-1))+1,:)';
    bcoeff=bcoeff(1:length(Acoeff));
   
    M(count1,:)=(Acoeff\bcoeff)';
end

for count1=1:w
    Acoeff=C(:,1:p)';                       % take at least p columns
    count2=p+1;
    while rank(Acoeff)<p                    % add additional columns until Acoeff is of full rank
        Acoeff=[Acoeff; C(:,count2)'];
        count2=count2+1;
    end
    bcoeff=Zb(sum(l(1:count1-1))+1,:)';
    bcoeff=bcoeff(1:length(Acoeff));
   
    M(v+count1,:)=(Acoeff\bcoeff)';
end

A1tilde=S*A/S;
B1tilde=S*B/W;
C1tilde=M*C/S;

%% SCB step 4 - determination of xd

% A00=A1tilde(1:n0,1:n0);

B0c=B1tilde(1:n0,md+1:end);
B0d=B1tilde(1:n0,1:md);

alphabb=sym(zeros(pb,nb));
for count=1:pb
    alphabb(count,:)=A1tilde(n0+sum(l(1:count-1))+l(count),n0+1:n0+nb);
end

alphabd=sym(zeros(pb,nd));
for count=1:pb
    alphabd(count,:)=A1tilde(n0+sum(l(1:count-1))+l(count),n0+nb+1:end);
end

A1tildedd=A1tilde(n0+nb+1:end,n0+nb+1:end);
Lid=cell(md,1);
for count1=1:md
    Lid{count1}=zeros(q(count1),md);
    for count2=1:q(count1)-1
        for count3=1:count1-1
            Lid{count1}(count2,count3)=A1tildedd(sum(q(1:count1-1))+count2,sum(q(1:count3)));
        end
    end
end

%% SCB step 5 - determination of xb

Sb=sym(eye(n));
for count1=1:pb
    for count2=1:l(count1)
        for count3=1:pb
            for count4=l(count1)+count2-2*(count2-1):min(l(count1)+1,l(count3))
                if count4<=l(count3)
                    Sb(n0+sum(l(1:count1-1))+count2,n0+sum(l(1:count3-1))+count4-l(count1)+count2-1)=-alphabb(count1,sum(l(1:count3-1))+count4);
                end
            end
        end   
        for count3=1:md
            for count4=l(count1)+count2-2*(count2-1):min(l(count1)+1,q(count3))
                if count4<=q(count3)
                    Sb(n0+sum(l(1:count1-1))+count2,n0+nb+sum(q(1:count3-1))+count4-l(count1)+count2-1)=-alphabd(count1,sum(q(1:count3-1))+count4);
                end
            end
        end
    end
end

Mb=sym(eye(p));
for count1=1:pb
    for count2=1:pb
        if l(count1)+1<=l(count2)
            Mb(p-pb+count1,p-pb+count2)=-alphabb(count1,sum(l(1:count2-1))+l(count1)+1);
        end
    end
    for count2=1:md
        if l(count1)+1<=q(count2)
            Mb(p-pb+count1,count2)=-alphabd(count1,sum(q(1:count2-1))+l(count1)+1);
        end
    end
end

A2tilde=Sb*A1tilde/Sb;
B2tilde=Sb*B1tilde;
C2tilde=Mb*C1tilde/Sb;

% A0b=A2tilde(1:n0,n0+1:n0+nb);
% A0d=A2tilde(1:n0,n0+nb+1:end);

Ad0=sym(zeros(md,n0));
Adb=sym(zeros(md,nb));
Add=sym(zeros(md,nd));
for count=1:md
    Ad0(count,:)=A2tilde(n0+nb+sum(q(1:count)),1:n0);
    Adb(count,:)=A2tilde(n0+nb+sum(q(1:count)),n0+1:n0+nb);
    Add(count,:)=A2tilde(n0+nb+sum(q(1:count)),n0+nb+1:end);
end

%% SCB step 6 - elimination of ud

% A00bar=A00+B0d*Ad0;
% A0bbar=A0b+B0d*Adb;
% A0dbar=A0d+B0d*Add;

%% SCB step 7 - elimination of xd_dot

T01=sym(zeros(n0,nb+nd));
for count1=1:n0
   for count2=1:md
       T01(count1,nb+sum(q(1:count2-1))+q(count2))=-B0d(count1,count2);
   end
end

S01=[eye(n0) T01; zeros(nb+nd,n0) eye(nb+nd)];

A3tilde=S01*A2tilde/S01;
B3tilde=S01*B2tilde;
C3tilde=C2tilde/S01;

%% SCB step 8 - elimination of all but first elements of xdij and xbij

if n0>0
    mainCount=0;
    
    if exist('q','var') && exist('l','var')
        maxql=max([q l]);
    elseif exist('q','var')
        maxql=max(q);
        l=[];
    elseif exist('l','var')
        maxql=max(l);
        q=[];
    else
        maxql=0;
    end
    
    Axtilde=A3tilde;            % Atildex with x=4,5,...
    Bxtilde=B3tilde;
    Cxtilde=C3tilde;
    
    while mainCount<=maxql-2
        T0x(:,:)=sym(zeros(n0,nb+nd));    % T0x with x=2,3,...
        for count=1:pb
            if l(count)-mainCount-1>0
                T0x(:,sum(l(1:count-1))+l(count)-mainCount-1)=-Axtilde(1:n0,n0+sum(l(1:count-1))+l(count)-mainCount);
            end
        end
        for count=1:md
            if q(count)-mainCount-1>0
                T0x(:,sum(l)+sum(q(1:count-1))+q(count)-mainCount-1)=-Axtilde(1:n0,n0+sum(l)+sum(q(1:count-1))+q(count)-mainCount);
            end
        end
        S0x(:,:,mainCount+1)=[eye(n0) T0x; zeros(nb+nd,n0) eye(nb+nd)];     % S0x with x=2,3,...
        Axtilde=S0x(:,:,mainCount+1)*Axtilde/S0x(:,:,mainCount+1);
        Bxtilde=S0x(:,:,mainCount+1)*Bxtilde;
        Cxtilde=Cxtilde/S0x(:,:,mainCount+1);
        mainCount=mainCount+1;
    end
end

%% SCB step 9 - separation of xa and xc

if n0>0
    A00bar=Axtilde(1:n0,1:n0);
    
    if ~isempty(B0c)
        [T0s,T0i,r]=csdsym(A00bar,B0c);
    else
        T0s=sym(eye(n0));
        T0i=[];
        r=[];
    end
    na=n0-sum(r);
    
%     Aaacc=T0s\A00bar*T0s;
%     B0ac=T0s\B0c*T0i;
    
%     if test
%         T0s=sym([1  0  0  0  0;
%             0 -1  0  0  1
%             0 -2  1  1  1
%             2 -5  1  1  3
%             0 -3  0  1  2]);
% %         Aaacc=T0s\A00bar*T0s;
% %         B0ac=T0s\B0c*T0i;
%     end
else
    T0s=[];
    T0i=[];
    r=[];
    na=0;
end

TiCSD=[eye(md) zeros(md,m-md); zeros(m-md,md) T0i];

TsCSD=[T0s zeros(n0,n-n0); zeros(n-n0,n0) eye(n-n0)];

nc=n-na-nb-nd;

TsPerm=zeros(n);
TsPerm(1:na,1:na)=eye(na);
TsPerm(n0+1:n0+nb,na+1:na+nb)=eye(nb);
TsPerm(na+1:na+nc,na+nb+1:na+nb+nc)=eye(nc);
TsPerm(na+nb+nc+1:n,na+nb+nc+1:n)=eye(nd);

%% SCB step 10 - finishing touch

Ti=W\TiCSD;
To=inv(Mb*M);
TsInv=S01*Sb*S;
if exist('S0x','var')
    for count=1:size(S0x,3)
        TsInv=S0x(:,:,count)*TsInv; 
    end
end
Ts=TsInv\TsCSD*TsPerm;

nDim=[na nb nc nd];

if ~exist('l','var')
    l=[];
end

if ~exist('q','var')
    q=[];
end

end

% subfunction OSD for unforced systems
function [Ts,To,l,nDim]=osdsym(A,C)

%% OSD step 1 - initialization

p=size(C,1);
n=size(A,1);
Zi=cell(p,1);
for count=1:p
    Zi{count}=C(count,:);
end

f=ones(p,1);

Z=C;
Ztilde=sym([]);

w=0;

%% OSD step 2 - repetitive differentiation of system output

count2=0;
while any(f)
    for count1=1:p
        if f(count1)==1
            alphai=size(Zi{count1},1);
            Cialphai=Zi{count1}(end,:);
            if rank([Z; Cialphai*A])>rank(Z)     % case 1
                Z=[Z; Cialphai*A];
                Zi{count1}=[Zi{count1}; Cialphai*A];
            else                                % case 2
                f(count1)=0;
                Ztilde=[Ztilde; Zi{count1}];
                w=w+1;
                count2=count2+1;
                l(count2)=alphai; 
            end
        end
    end
end

%% OSD step 3 - preliminary transformation

n0=n-sum(l);

S0=null(Ztilde)';
S=[S0; Ztilde];

Atilde=S*A/S;
% Ctilde=C/S;

for count=1:p
    alpha(count,:)=Atilde(n0+sum(l(1:count)),n0+1:end);
end

%% OSD step 4 - further simplification, part 1

W=sym(eye(n));
for count0=1:p
    for count1=1:l(count0)
        for count2=1:p
            for count3=l(count0)+(count1-2*(count1-1)):min(l(count0)+1,l(count2))
                if count3<=l(count2)
                    W(n0+sum(l(1:count0-1))+count1,n0+sum(l(1:count2-1))+count3-l(count0)+count1-1)=-alpha(count0,sum(l(1:count2-1))+count3);
                end
            end
        end
    end
end

Cdes=sym(zeros(p,n));
for count=1:p
   Cdes(count,n0+sum(l(1:count-1))+1)=1; 
end

Wo=sym(zeros(p));
K=C/S/W;
for count1=1:p
    Acoeff=K(:,1:p)';                       % take at least p columns
    count2=p+1;
    while rank(Acoeff)<p                    % add additional columns until Acoeff is of full rank
        Acoeff=[Acoeff; K(:,count2)'];
        count2=count2+1;
    end
    bcoeff=Cdes(count1,1:length(Acoeff))';
    Wo(count1,:)=(Acoeff\bcoeff)';
end

A2tilde=W*Atilde/W;

%% OSD step 5 - further simplification, part 2

mainCount=0;
Axtilde=A2tilde;            % Atildex with x=3,4,...

while mainCount<=max(l)-2
    Mx(:,:,mainCount+1)=sym(eye(n));              % Mx with x=1,2,...
    for count=1:p
        if n0>0 && l(count)-mainCount-1>0
            Mx(1:n0,n0+sum(l(1:count-1))+l(count)-mainCount-1,mainCount+1)=-Axtilde(1:n0,n0+sum(l(1:count-1))+l(count)-mainCount);
        end
    end
    Axtilde=Mx(:,:,mainCount+1)*Axtilde/Mx(:,:,mainCount+1);
    mainCount=mainCount+1;
end

%% OSD step 6 - finishing touch

TsInvTemp=W*S;
if exist('Mx','var')
   for count=1:size(Mx,3)
      TsInvTemp=Mx(:,:,count)*TsInvTemp ;
   end
end
Ts=inv(TsInvTemp);

To=inv(Wo);

na=n-sum(l);
nb=sum(l);
nc=0;
nd=0;
nDim=[na nb nc nd];

end

% subfunction CSD for unsensed systems
function [Ts,Ti,r,nDim]=csdsym(A,B)

n=size(A,1);

[TsOSD,ToOSD,r]=osdsym(A',B');
na=n-sum(r);
tt90=eye(na);          % code from linsyskit function csd
for count=1:length(r)
    t1=na+sum(r(1:count-1));
    t2=na+sum(r(1:count));
    tt90(t1+1:t2,t1+1:t2)=rot90(eye(r(count)));
end
% invT0s=tt90'*TsOSD';
Ts=TsOSD'\tt90;

Ti=inv(ToOSD');

% if test
%    Ts=sym([1  0  0  0  0;
%         0 -1  0  0  1
%         0 -2  1  1  1
%         2 -5  1  1  3
%         0 -3  0  1  2]); 
% end

nb=0;
nc=n-na;
nd=0;
nDim=[na nb nc nd];

end



