%
%  Factorize one single real RHP-pole p in output factorization.
%
%  [GI, BI, Xf, Uf, Zf] = orpf( G, p, epp )
%  
%
%  Written by: Kjetil Havre,  16/12 - 1995, e-mail: kjetil@ife.no                         
%
%  See also: IZDE, INPCOMPFACT, OUTREALFACT, OUTCOMPFACT, INPFACT, OUTFACT.
%

function [GO, BO, Yp, Xp, Pf] = orpf( G, p, epp )

   if nargin < 2
      disp('Usage: orpf(G, p)');
      return
   end
   if nargin < 3
      epp = 1e-12;
   end
   
   [mtype,ny,nu,nx] = minfo(G);
   if mtype ~= 'syst'
      disp('Error: System matrix G is required.');
      disp('usage: inprealfact(G, Z)');
      GO = G; BO = eye( ny ); Xp = []; Up = []; Pf = [];
      return
   end

   [np, nc] = size(p);
   p = p(1);
   if np < 1
      disp('Message: No pole?');
      GO = G; BO = eye( ny ); Xp = []; Up = []; Pf = [];
      return     
   end
   if np > 1
      disp('Warning: Only the first pole is factorized.')
   end
   
   if ~isreal(p)
      disp(['Error: Zero is supposed to be real Z: ', num2str(z), '.']);
      GI = G; BI = eye( nu ); Xz = []; Uz = []; Zf = [];
      return;
   end

   [Up, Yp, XI, XO, err] = pdfst(G, p);
   
   pr = real(p); Ypr = real(Yp); Xpr = real(XO);

   err = norm([XO; Yp] - [Xpr; Yp]);
   if err > epp
      disp('Error: State and input direction is not real.');
      GO = G; BO = eye( ny ); Xp = []; Up = []; Pf = [];
      return;      
   end
   Yp = Ypr; Xp = Xpr;

   [A,B,C,D] = unpck( G );
   An = A - 2*real(p)*Xp*Yp'*C;
   Bn = B - 2*real(p)*Xp*Yp'*D;
   GO  = pck(An,Bn,C,D);

   Ab  = -p; Bb = 2*p*vtp(Yp); Cb  = -Yp; Db = eye(ny);
   BO  = minv(pck(Ab,Bb,Cb,Db));
   Pf  = p; 
end


% This works OK as long as D is zero.
