%   function [c1, z, seq, fc1, fseq] = cwcc1(G, ztol, pst, zst)
%
%   The ``Compute-Worst-Case-c1'' cwcc1-function maximize the
%   c1 value by considering all possible factorizations of 
%   RHP-poles in Bp(s) and all RHP-zeros.
%
%   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: c1   - Worst case c1 value.
%            z    - RHP-zero for which c1 occure.
%            seq  - Describes the sequence of factorization for c1.
%            fc1  - All possible values of c1. Matrix containing
%                   dim(Zrhp)-rows and fact{dim(Prhp)} columns.
%                   Element fc1(i,j) is given for RHP-zero Zrhp(i)
%                   with factorization described by fseq(j).
%            fseq - Describes the sequence of factorization for the
%                   columns in fc1.
%            Zrhp - RHP-zeros.
%            Prhp - RHP-poles.
%
%   Written by: Kjetil Havre, 30/11-1995, e-mail: kjetil@ife.no
%
%   See also:  ZDSVD, PDFST ALLBPZ and CWCC2.
%
%   Reference: Havre K. and S. Skogestad, 1995, ``Effect of RHP Zeros and
%              Poles on Performance in Multivariable Systems''.
%
function [c1, z, seq, fc1, fseq, Zrhp, Prhp] = cwcc1(G, ztol, pst, zst)

   c1 = []; z = []; seq = []; fc1 = []; 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: [c1, z, seq, fc1, fseq] = cwcc1(G, ztol, pst, zst)');
      return
   end

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

   if strcmp(mt, 'syst')==0
      disp( 'System matrix G is required, usage: [I,O] = cwcc1(G, ztol)' );
      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:nz
      [Bpzs fseq]  = allbpz(YP,Prhp,Zrhp(j));
      yzBpz = YZ(:,j)'*Bpzs;
      for i=1:fact(np)
         fc1(j,i) = norm(yzBpz(1,ny*(i-1)+1:ny*i));
      end
   end

   c1         = max(max(fc1));
   [Ic1, Jc1] = find(fc1==c1);
   seq        = fseq(Jc1);
   z          = Zrhp(Ic1);

end
