%  function [GI, BI, Up, Xp, Pf] = opf( G, P, epp )
%  
%  The IPF function is used to obtain an output factorization of RHP-poles 
%  so that G(s) = GI(s)*BI(s) where BI 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 Upi 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.
%  
%  BI(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. 
%
%  GI is constructed as follows:
%           G(s) = |  A  |  B  |      GI(s) = |  Am  |  B  |
%                  |  C  |  D  |              |  Cm  |  D  |
%  where Am and Cm are obtained by running the following formulas repeatedly:
%      Ai = Ai_1 - 2*Re(Pi)*B*Upi*Xpi',  Ao = A for i=1 to Np, where Np
%                                        is the number of RHP-poles.
%                                        Am = Ai, with i = Np.
%      Ci = Ci_1 - 2*Re(Pi)*D*Upi*Xpi'   Co = C for i=1 to Np, where Np
%                                        is the number of RHP-poles.
%                                        Cm = Ci, with i = Np.
%
%  BI(s) is given by:
%      BI(s) = PROD Bi(s), where Bi(s) = I + 2*Re(Pi)/(s-Pi') *Upi*Upi'
%                          the index runs from i=1 to Np.
%  Xpi and Upi are the state and the output direction corresponding to the 
%  RHP-pole Pi for the system Gi_1(s) = |  Ai_1  |  B  |
%                                       |--------|-----|
%                                       |  Ci_1  |  D  |
%
%
%  Written  by: Kjetil Havre,  30/1 - 1996, e-mail: kjetil@ife.no
%
function [GI, BI, Up, Xp, Pf] = opf( G, P, epp, lv )
   if nargin < 2
      disp('usage: opf(G, Z)')
      GO = G; BO = eye( nu );
      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)');
      GI = G; BI = eye( nu );
      Up = []; 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?');
      GI = G; BI = eye(nu);
      Up = []; 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), '.']);
      GI = G; BI = eye(nu); Up = []; Xp = []; Pf = [];
      return;
   else
      [A,B,C,D] = unpck( G );
      An  = A - 2*real( p )*B*up*xip';
      Cn  = C - 2*real( p )*D*up*xip';
      GI  = pck(An,B,Cn,D);
      Ab  = p;
      Bb  = 2*real(p)*up';
      Cb  = up;
      Db  = eye(nu);
      BI  = pck(Ab,Bb,Cb,Db);
      Up  = up;
      Xp  = xip;
      Pf  = p;
   end
   
   if np > 1   % Call recursive for factorizing additional RHP-zeros.
      [Gn, Bn, Un, Xn, Pn] = ipf(GI, P(2:np), epp, lv+1);
      GI = Gn;
      BI = mmult(Bn,BI);
      Up = [Up Un];
      Xp = [Xp Xn];
      Pf = [Pf; Pn];
   end
end

