% function [v,t,flgout,reig_min] = csord(a,epp,flgord,flgjw,flgeig)
%
%  This function produces an ordered complex schur matrix, T,
%  where
%              v' * a * v  = t = |t11  t12|
%                                | 0   t22|.
%  and V'V = eye().
%  The MATLAB function SCHUR is called, which results in an 
%  unordered Schur form matrix. A subroutine called CGIVENS
%  is used to form a complex Givens rotation matrix which
%  orders the T matrix in a user-defined manner. A series
%  of input flags can be set:
%
%  EPP           user supplied zero tolerance (default EPP=1e-15)
%  FLGORD = 0    order eigenvalues in ascending real part (default)
%         = 1    partial real part ordering, <0 , =0, >0
%  FLGJW  = 0    no exit condition on eigenvalue location (default)
%         = 1    exit if abs(real(eigenvalue_i)) < EPP
%  FLGEIG = 0    no exit condition on half-plane eigenvalue 
%                distribution (default)
%         = 1    exit if 
%                length(real(eigenvalue)>0) ~= length(real(eigenvalue)<0)
%
%
%  The output flag FLGOUT is nominally 0. If there are jw-axis
%  eigenvalues, FLGOUT is set to 1. If there are an unequal
%  number of positive and negative eigenvalues, FLGOUT is set
%  to 2. If both conditions occur, FLGOUT = 3. The minimum real part
%  of the eigenvalues is output in REIG_MIN.
%
%  See also: RIC_SCHR, SCHUR, and RSF2CSF.

%  The RIC_SCHR routine uses CSORD to solve stabilizing
%  solutions to matrix Ricatti equations. In that case, the
%  a matrix has a special structure, and there are failure modes
%  which can be flagged, thus avoiding extra, unnecessary
%  computations. 
%  Wes Wang made change to limit the matrix to be a square matrix.

function [v,t,flgout,reig_min] = csord(a,epp,flgord,flgjw,flgeig)

 if nargin < 1
   disp(['usage: [v,t,flgout] = csord(a,epp,flgord,flgjw,flgeig)'])
   return
 elseif nargin == 1
   epp = 1e-15;
   flgord = 0;
   flgjw = 0;
   flgeig = 0;
  elseif nargin == 2
   flgord = 0;
   flgjw = 0;
   flgeig = 0;
  elseif nargin == 3
   flgjw = 0;
   flgeig = 0;
  elseif nargin == 4
   flgeig = 0;
  end
   
 [nc,nr] = size(a);
%%%%%%%%%% begine the change made by Wes Wang %%%%%%%%
 if nc ~= nr,
   disp('Matrix must be square.')
   return
 end;
%%%%%%%%%% end the change made by Wes Wang %%%%%%%%%%%
 flgout = 0;
%
% complex schur decomposition 
%
 [v,t] = schur(a);
 [v,t] = rsf2csf(v,t); % change into complex Schur form if it isn't
 [ntc,ntr] = size(t); 
%
% check if there are any jw-axis eigenvalues in the matrix (used
% in HINFSYN and HINFFI routines)
%
reig_min = min(abs(real(diag(t))));
if min(abs(real(diag(t)))) < epp
  flgout = 1;                        % there is a jw axis eigenvalue
  if flgjw == 1
    t = [];
    v = [];
    return
  end
end
neg_eig = length(find(real(diag(t))<0));
pos_eig = length(find(real(diag(t))>0));
if neg_eig ~= ntc/2 | pos_eig ~= ntc/2
  if flgout == 0
    flgout = 2;
  else
    flgout = 3;
  end
  if flgeig == 1
    t = [];
    v = [];
    return
  end
end
% order evals
 if flgord == 1                      % partial ordering -0+
   if flgjw == 1                     % Riccati solution
     for i = 1 : ntc*ntc/4
       for k = 1 : (ntc-1)
         if (real(t(k,k))>0.) & (real(t(k+1,k+1))<0.)
           [c,s] = cgivens( t(k,k+1), t(k,k)-t(k+1,k+1) );
           t(:,k:k+1) = t(:,k:k+1)*[c s;-s' c];
           t(k:k+1,:) = [c s;-s' c]'*t(k:k+1,:);
	   v(:,k:k+1) = v(:,k:k+1)*[c s;-s' c];
         end
       end
       mxneg = max(find(real(diag(t))<0));
       mnpos = min(find(real(diag(t))>0));
%
%    check if the ordering of the Schur matrix is finished
%
       if (mxneg <  mnpos)
         break               % done partial ordering
       end
     end
   else
     for i = 1 : ntc*ntc/4
       for k = 1 : (ntc-1)
         if (real(t(k,k))>=0.) & (real(t(k+1,k+1))<=0.)
           [c,s] = cgivens( t(k,k+1), t(k,k)-t(k+1,k+1) );
           t(:,k:k+1) = t(:,k:k+1)*[c s;-s' c];
           t(k:k+1,:) = [c s;-s' c]'*t(k:k+1,:);
	   v(:,k:k+1) = v(:,k:k+1)*[c s;-s' c];
         end
       end
       mxneg = max(find(real(diag(t))<0));
       mxzer = max(find(real(diag(t))==0));
       mnzer = min(find(real(diag(t))==0));
       mnpos = min(find(real(diag(t))>0));
%
%    check if the ordering of the Schur matrix is finished
%
       if (mxneg < mnzer) & (mxzer < mnpos)
         break               % done partial ordering
       end
     end
   end %if flgjw
 else                         %  full ordering
   for i = 1 : ntc*ntc/4
     for k = 1 : (ntc-1)
       if (real(t(k,k)) > real(t(k+1,k+1)))
         [c,s] = cgivens( t(k,k+1), t(k,k)-t(k+1,k+1) );
         t(:,k:k+1) = t(:,k:k+1)*[c s;-s' c];
         t(k:k+1,:) = [c s;-s' c]'*t(k:k+1,:);
	        v(:,k:k+1) = v(:,k:k+1)*[c s;-s' c];
       end
     end
     if min(diff(real(diag(t)))) >= 0
       break               % done full ordering
     end
   end
 end
%
% Copyright MUSYN INC 1991,  All Rights Reserved
