%  function [GO, BO, Yp, Xp, Pf] = opf( G, P, epp )
%  
%  The OPF function is used to obtain an output factorization of RHP-poles 
%  so that G(s) = BO(s)*GO(s) where BO contains the RHP-poles.
%  The input argument P is a vector containing the RHP-poles. 
%  The PDFST function is used to obtain the pole state direction Xpi and 
%  the output zero direction Ypi for the RHP-pole P(i). The optional EPP 
%  argument is sendt to the PDFST function as the SST argument. The default 
%  EPP value is 1e-12.
%  
%  BO(s) is an all pass transfer function for s=jw. All singular values
%  are equal to one, and the absolute value of all eigenvalues are 
%  equal to one. 
%
%  GO is constructed as follows:
%           G(s) = |  A  |  B  |      GO(s) = |  Am  |  Bm  |
%                  |  C  |  D  |              |  C   |  D   |
%  where Am and Bm are obtained by running the following formulas repeatedly:
%      Ai = Ai_1 - 2*Re(Pi)*Xpi*Ypi'*C,  Ao = A for i=1 to Np, where Np
%                                        is the number of RHP-poles.
%                                        Am = Ai, with i = Np.
%      Bi = Bi_1 - 2*Re(Pi)*Xpi*Ypi'*D   Bo = B for i=1 to Np, where Np
%                                        is the number of RHP-poles.
%                                        Bm = Bi, with i = Np.
%
%  BO(s) is given by:
%      BO(s) = PROD Bi(s), where Bi(s) = I + 2*Re(Pi)/(s-Pi') *Ypi*Ypi'
%                          the index runs from i=1 to Np.
%  Xpi and Ypi are the state and the output direction corresponding to the 
%  RHP-pole Pi for the system Gi_1(s) = |  Ai_1  |  Bi_1  |
%                                       |--------|--------|
%                                       |    C   |    D   |
%
%
%  Written  by: Kjetil Havre,  30/1 - 1996, e-mail: kjetil@ife.no
%
function [GO, BO, Yp, Xp, Pf] = opf( G, P, epp, lv )
   if nargin < 2
      disp('usage: opf(G, Z)')
      GO = G; BO = eye( ny );
      Yp = []; Xp = []; Pf = [];
      return
   end
   if nargin < 3
      epp = 1e-12;
   end
   if nargin < 4
      lv = 1;
   end
  
   [mtype,ny,nu,nx] = minfo(G);
   if mtype ~= 'syst'
      disp('Error: System matrix G is required.');
      disp('usage: opf(G, Z)');
      GO = G; BO = eye( ny );
      Yp = []; Xp = []; Pf = [];
      return
   end

   [np, nc] = size(P);
   if nc > 1                % Vector  is horizontal.
      P = vtp(P);
      [np, nc] = size(P);
   end

   if np < 1
      disp('Message: No poles?');
      GO = G; BO = eye(ny);
      Yp = []; Xp = []; Pf = [];
      return     
   end
   
   p = P(1);
   [up, yp, xip, xop, err] = pdfst( G, p, epp );   % Compute directions.
   if err != 0 
      disp( ['Warning: Requested pole P(', int2str(lv),'): ',...
	           num2str(P(1)), ' is not in system.' ] );      
      disp(['Error when computing pole direction, ',...
            'error number: ', int2str(err), '.']);
      GO = G; BO = eye(ny); Yp = []; Xp = []; Pf = [];
      return;
   else
      [A,B,C,D] = unpck( G );
      An  = A - 2*real( p )*xop*yp'*C;
      Bn  = B - 2*real( p )*xop*yp'*D;
      GO  = pck(An,Bn,C,D);
      Ab  = p;
      Bb  = 2*real(p)*yp';
      Cb  = yp;
      Db  = eye(ny);
      BO  = pck(Ab,Bb,Cb,Db);
      Yp  = yp;
      Xp  = xop;
      Pf  = p;
   end
   
   if np > 1   % Call recursive for factorizing additional RHP-zeros.
      [Gn, Bn, Yn, Xn, Pn] = opf(GO, P(2:np), epp, lv+1);
      GO = Gn;
      BO = mmult(BO, Bn);
      Yp = [Yp Yn];
      Xp = [Xp Xn];
      Pf = [Pf; Pn];
   end
end

