NewR=balancetrunc(A,B,C,D,orders)

function NewR=balancetrunc(A,B,C,D,orders)
% Balance Truncation algorithm to obtain balance truncated reduced order model 
% of any order for a given system in state space
%
%NewR=BalanceTrunc(A,B,C,D,orders)
%Where A,B,C,D are the system matrices of the state space representation
%and orders is the order of the reduced order model.
%
%NewR=BalanceTrun(sys,orders)
%In this format, the state space model sys is reduced to a 'orders'-order
%model.
%
%It may also be noted that the function can also be used as
%BalanceTrunc(A,B,C,D) OR BalanceTrunc(sys), in which case all feasible
%reduced order realizations are presented as a cell array of state space
%objects.

if (nargin < 4)
   if (nargin < 3)
      if (nargin ==2)
         orders=B;
      end
      if isobject(A)
      D=A.d;C=A.c;B=A.b;A=A.a;
      else
          error('State Space Object is expected as input')
      end
   else
      error('Not enough number of inputs')
   end
end

if exist('orders','var')
    if isreal(orders)
        if (fix(orders)~=orders)
            error('Orders should be a real integer');
        end
    else
        error('Orders should be a real integer');
    end
end


sys1=ss(A,B,C,D);
[sys2]=canon(sys1,'modal');
A1=sys2.a;B1=sys2.b;C1=sys2.c;D=sys2.d;
[dummy,I]=sort(diag(A1));
A1=A1(I,I);
B1=B1(I,:);
C1=C1(:,I);
I=find(diag(A1) > 0);
Ax22=[];Bx2=[];
if ~isempty(I)
I=I(1);
Ax11=A1(1:I-1,1:I-1);
Ax12=A1(1:I-1,I:length(A1));
Ax22=A1(I:length(A1),I:length(A1));
Bx1=B1(1:I-1,:);
Bx2=B1(I:length(A1),:);
Cx1=C1(:,1:I-1);
Cx2=C1(:,I:length(A1));
ips=size(B1,2);
A=Ax11;
B=[Bx1 Ax12];
C=Cx1;
D=[D Cx2];
else
    ips=size(B1,2);
end
if (~isempty(Ax22) && exist('orders','var'))
   orders=orders-size(Ax22,1);
end

if (size(A,1) ~= size(B,1)) || (size(A,1) ~= size(A,2)) || (size(C,2) ~= size(A,2))...
        || any(size(D) ~= [size(C,1) size(B,2)]) || (max([length(size(A)) length(size(B)) length(size(C)) length(size(D))]) > 2)...
        || any([~isreal(A) ~isreal(B) ~isreal(C) ~isreal(D)])
   error(' Dimension Mismatch');
end

CGram=lyap(A,B*B');
OGram=lyap(A',C'*C);
[Uc,Sc,dummy]=svd(CGram);
[Uo,So,dummy]=svd(OGram);
H = So^(.5)*Uo'*Uc*Sc^(.5);
[Uh,Sh,dummy]=svd(H);
T = Uo*So^(-.5)*Uh*Sh^(.5);
Ab=inv(T)*A*T;
Bb=inv(T)*B;
Cb=C*T;
n=length(Ab);

if (exist('orders','var'))
   if (orders <= length(A))
      nos=orders;
   else
      error('Requested Approximation greater than system order')
   end
else
      nos=1:length(A)-1;
end

if (max(nos) <= 0)
   error('Reduction Unrealizable')
end


for i=nos
   A11=Ab(1:i,1:i);
   A12=Ab(1:i,(i+1):n);
   A21=Ab((i+1):n,1:i);
   A22=Ab((i+1):n,(i+1):n);
   B1=Bb(1:i,:);
   B2=Bb((i+1):n,:);
   C1=Cb(:,1:i);
   C2=Cb(:,(i+1):n);
   NewR{i}.a=A11-A12*inv(A22)*A21;
   NewR{i}.b=B1-A12*inv(A22)*B2;
   NewR{i}.c=C1-C2*inv(A22)*A21;
   NewR{i}.d=D-C2*inv(A22)*B2;
   P=NewR{i};
   Pb1=P.b(:,1:ips);
   Pb2=P.b(:,ips+1:size(P.b,2));
   P.a=[P.a Pb2;zeros([size(Ax22,1) size(P.a,2)]) Ax22];
   P.b=[Pb1;Bx2];
   Pd1=P.d(:,1:ips);
   Pd2=P.d(:,ips+1:size(P.d,2));
   P.c=[P.c Pd2];
   P.d=Pd1;
   
   if length(nos)>1
   NewR{i}=ss(P.a,P.b,P.c,P.d);
   clear P
   else
       NewR=ss(P.a,P.b,P.c,P.d);
       clear P
       return
   end
end

return