%  function [GI, BI, Uz, Xz, Zf] = izf( G, Z, epp )
%  
%  The IZF function is used to obtain an input factorization of
%  RHP - zeros so that G(s) = GI(s)*BI(s).
%  The input argument Z is a vector containing the RHP-zeros. The optional
%  EPP argument is used to decide the difference between the zeros given
%  in Z and the zeros calculated by the IZF function. If the function does
%  not calculate any zeros Ze so that abs(Z(i) - Ze) < EPP the function 
%  skipps Z(i) in the factorization. The default EPP value is 1e-12.
%  The IDZE function is used to obtain the zero state direction Xzi and the
%  input zero direction Uzi for RHP-zero Z(i).
%  
%  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 running the following formula repeatedly:
%      Bi = Bi_1 - 2*Re(Zi)*Xzi*Uzi',   Bo = B for i=1 to Nz, where Nz
%                                       is the number of RHP - zeros.
%                                       Bm = Bi, with i = Nz.
%
%  BI(s) is given by:
%      BI(s) = PROD Bi(s), where Bi(s) = I - 2*re(Zi)/(s+Zi') *Uzi*Uzi'
%                          the index runs from i=Nz to one.
%  Xzi and Uzi are the state and the input  direction corresponding to the 
%  RHP-zero Zi for the system Gi_1(s) = |  A  |  Bi_1  |
%                                       |-----|--------|
%                                       |  C  |    D   |
%
%  Note In order to obtain an real input factorization for complex RHP-zeros
%  the two etc.
%
%  Possible malfunctions: Multiple zeros located at the same place in the 
%                         complex plane.
%                         RHP-zeros on the jw axis, pure imaginary zeros.
%                         
%
%
%  Written  by: Kjetil Havre,  15/5 - 1995, e-mail: kjetil@ife.no 
%  Modified by: Kjetil Havre,  30/1 - 1998, e-mail: kjetil@ife.no
%     Modification was to also return the state direction, and change of name.
%

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

   [nz, nc] = size(Z);
   if nc > 1                % Vector  is horizontal.
      Z = vtp(Z);
      [nz, nc] = size(Z);
   end
  
   if nz < 1
      disp('Message: No zeros?');
      GI = G;
      BI = eye(nu);
      Uz = []; Xz = []; Zf = [];
      return     
   end
  
   z = Z(1);
   [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);
      Uz = []; Xz = []; Zf = [];
   else
                     % Iz contains the index to the zero in Ze. 
                     % Factorize this zero.
      if isreal(z)
         xiz(:,Iz) = real(xiz(:,Iz));
         uiz(:,Iz) = real(uiz(:,Iz));
	 Ze(Iz) = real(Ze(Iz));
      end
      [A,B,C,D] = unpck( G );
      Bn  = B - 2*real( Ze(Iz) )*xiz(:,Iz)*uiz(:,Iz)';
      GI  = pck(A,Bn,C,D);
      Ab  = -conj(Ze(Iz));
      Bb  = 2*real(Ze(Iz))*uiz(:,Iz)';
      Cb  = -uiz(:,Iz);
      Db  = eye(nu);
      BI  = pck(Ab,Bb,Cb,Db);
      Uz  = uiz(:,Iz);
      Xz  = xiz(:,Iz);
      Zf  = Ze(Iz); 
   end
  

   if nz > 1   % Call recursive for factorizing additional RHP-zeros.
      [Gn, Bn, Un, Xn, Zn] = izf(GI, Z(2:nz), epp, lv+1);
      GI = Gn;
      BI = mmult(Bn, BI);
      Uz = [Uz Un];
      Xz = [Xz Xn];
      Zf = [Zf; Zn];
   end

end

