%   [c2, p, seq, fc2, fseq, Zrhp, Prhp] = cwcc2(G, ztol, pst, zst)
%
%   The ``Compute-Worst-Case-c2'' cwcc2-function maximize the
%   c2 value by considering all possible factorizations of 
%   RHP-poles in Bz(s) and all RHP-poles.
%
%   Inputs:  G    - System containing RHP-zeros and poles.
%            ztol - Upper value on zeros which are treated as 
%                   RHP-zeros. Typical 1e12, default value 
%                   1e12.
%            pst  - SST argument passed to PDFST.
%            zst  - SST argument passed to ZDSVD.
%
%   Outputs: c2   - Worst case c2 value.
%            p    - RHP-pole for which c2 occure.
%            seq  - Describes the sequence of factorization for c2.
%            fc2  - All possible values of c2. Matrix containing
%                   dim(Prhp)-rows and fact{dim(Zrhp)} columns.
%                   Element fc2(i,j) is given for RHP-pole Prhp(i)
%                   with factorization described by fseq(j).
%            fseq - Describes the sequence of factorization for the
%                   columns in fc2.
%            Zrhp - RHP-zeros.
%            Prhp - RHP-poles.
%
%   Written by: Kjetil Havre, 30/11-1995, e-mail: kjetil@ife.no
%
%   See also:  ZDSVD, PDFST ALLBZP and CWCC1.
%
%   Reference: Havre K. and S. Skogestad, 1995, ``Effect of RHP Zeros and
%              Poles on Performance in Multivariable Systems''.
%
function [c2, p, seq, fc2, fseq, Zrhp, Prhp] = cwcc2(G, ztol, pst, zst)

   c2 = []; p = []; seq = []; fc2 = []; fseq = []; Zrhp = []; Prhp = [];
   if nargin < 4
      zst = 1e-14;
   end
   if nargin < 3
      pst = 1e-12;
   end
   if nargin < 2
      ztol = 1e12;
   end
   if nargin < 1
      disp('Usage: [c2, p, seq, fc2, fseq] = cwcc2(G, ztol, pst, zst)');
      return
   end

   [mt,ny,nu,nx] = minfo(G);

   if strcmp(mt, 'syst')==0
      disp( 'System matrix G is required, usage: [c2, p, seq, fc2, fseq] = cwcc2(G, ztol, pst, zst)' );
      return
   end

   Z = szeros(G);                   % Zeros.
   P = spoles(G);                   % Poles.
   IPrhp = find(P>0);               % Finds indices for poles in RHP 
   IZrhp = find(real(Z)>0&abs(Z)<ztol);  % Finds indices for zeros in RHP
   Prhp  = P(IPrhp);                % Store RHP-poles.
   Zrhp  = Z(IZrhp);                % Store RHP-zeros.
   np = max(size(IPrhp));           % Number of RHP-poles.
   nz = max(size(IZrhp));           % Number of RHP-zeros.

   if np == 0
      disp('Be happy: No RHP-poles.');
      return;
   end
   if nz == 0
      disp('Be happy: No RHP-zeros.');
      return;
   end
   
   [UP, YP, erp] = pdfst(G, Prhp, pst);
   if erp 
      if erp > 0
         disp('Error: In pole-direction calculation PDFST.')
         disp('       No direction with gain larger than PST')
         disp('       No direction with gain larger than PST')	 
         disp('       Decrease PST argument.')
	 return
      else
         disp('Warning: Two directions are possible pole directions.')
         disp('         May try to increase the PST argument.')
      end
   end
   
   [UZ, YZ, erz] = zdsvd(G, Zrhp, zst);
   if erz
      if erz > 0
         disp('Error: In zero-direction calculation zdsvd.')
         disp('       No direction with gain smaller than ZST.')
         disp('       Increace ZST argument.')
         return;
      else
         disp('Warning: Two directions are possible pole directions.')      
         disp('         May try to decrease the ZST argument.')
      end
   end

   for j=1:np
      [Bzps fseq]  = allbzp(YZ,Zrhp,Prhp(j));
      for i=1:fact(nz)
         Bzpyp = Bzps(:,ny*(i-1)+1:ny*i)*YP(:,j);
         fc2(j,i) = norm( Bzpyp );
      end
   end

   c2         = max(max(fc2));
   [Ic2, Jc2] = find(fc2==c2);
   seq        = fseq(Jc2);
   p          = Prhp(Ic2);

end
