%
%  Factorize one single real RHP-zero in input factorization.
%
%  [GI, BI, Xf, Uf, Zf] = inprealfact( G, Z, epp )
%  
%  The INPREALFACT function is used to obtain an input factorization of
%  a single RHP-zero so that G(s) = Gm(s)*BI(s).
%  The input argument Z contains the RHP-zero. The optional EPP argument 
%  is used to decide the difference between the zero given in Z and the 
%  zeros calculated by the function. If the function does not calculate
%  any zero Ze so that abs(Z(i) - Ze) < EPP the function returns with 
%  BI = eye(nu) and GI = G. The default EPP value is 1e-12.
%  The IDZE function is used to obtain the zero state direction Xz and the
%  input zero direction Uz for RHP-zero Z.
%  
%  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) = |  A  |   Bm  |
%                  |-----|-----|              |-----|-------|
%                  |  C  |  D  |              |  C  |   D   |
%  where Bm is obtained by:
%      Bm = B - 2*Z*Xz*Uz'.
%
%  Xz and Uz are the state and the input direction corresponding to the 
%  RHP-zero Z for the system G(s) = |  A  |  B  |  (see IZDE)
%                                   |-----|-----|
%                                   |  C  |  D  |
%
%  Possible malfunctions: Multiple zeros located at the same place in the 
%                         complex plane.
%
%  Written by: Kjetil Havre,  12/12 - 1995, e-mail: kjetil@ife.no                         
%
%  See also: IZDE, INPCOMPFACT, OUTREALFACT, OUTCOMPFACT, INPFACT, OUTFACT.
%

function [GI, BI, Xz, Uz, Zf] = inprealfact( G, Z, epp )

   if nargin < 2
      disp('Usage: inprealfact(G, Z)');
      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)');
      GI = G; BI = eye( nu ); Xz = []; Uz = []; Zf = [];
      return
   end

   [nz, nc] = size(Z);
   z = Z(1);
   if nz < 1
      disp('Message: No zeros?');
      GI = G; BI = eye(nu); Xz = []; Uz = []; Zf = [];
      return     
   end
   if nz > 1
      disp('Warning: Only the first zero is factorized.')
   end
   
   if ~isreal(z)
      disp(['Error: Zero is supposed to be real Z: ', num2str(z), '.']);
      GI = G; BI = eye( nu ); Xz = []; Uz = []; Zf = [];
      return;
   end

   [Ze, uiz, xiz] = izde( G );   % Compute directions.
   Iz = find( abs(Ze-z) < epp );  
   if isempty(Iz)
      disp( ['Warning: Requested zero Z(', int2str(lv),'): ',...
	           num2str(Z(1)), ' is not in system.' ] );      
      GI = G; BI = eye(nu); Xz = []; Uz = []; Zf = [];
      return;
   end
   z = real(Ze(Iz));
   Xz = xiz(:,Iz); Uz = uiz(:,Iz);
   inz = find( abs(Uz)>1000*epp );
   vh = angle(Uz(inz(1)))
   Uz
   Xz
   Uz = exp(-vh*sqrt(-1))*Uz
   Xz = exp(-vh*sqrt(-1))*Xz
   err = norm([Xz; Uz] - real([Xz; Uz]));
   if err > epp
      disp('Error: State and input direction is not real.');
      Xz
      Uz
      GI = G; BI = eye( nu ); Xz = []; Uz = []; Zf = [];
      return;      
   end
   Xz = real(Xz); Uz = real(Uz);
   
   [A,B,C,D] = unpck( G );
   Bn  = B - 2*z*Xz*vtp(Uz);
   GI  = pck(A,Bn,C,D);
   Ab  = -z; Bb = 2*z*vtp(Uz); Cb  = -Uz; Db = eye(nu);
   BI  = pck(Ab,Bb,Cb,Db);
   Zf  = z; 
end
  
