%  function [Gm, BO] = outfact( G, Z, epp )
%  
%  The OUTFACT function is used to obtain an output factorization of
%  RHP - zeros so that G(s) = BO(s)*Gm(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 function. If the function does
%  not calculates any zeros Zf so that abs(Z(i) - Zf) < EPP the function 
%  skipps Z(i) in the factorization. The default EPP value is 1e-6.
%  The ODZE function is used to obtain the zero state direction Xzi and the
%  output zero direction Yzi for the RHP-zero Z(i).
%  
%  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. 
%
%  Gm is constructed as follows:
%           G(s) = |  A  |  B  |      Gm(s) = |  A   |   B  |
%                  |  C  |  D  |              |  Cm  |   D  |
%  where Cm is obtained by running the following formula repeatedly:
%      Ci = Ci_1 - 2*Re(Zi)*Yzi*Yzi',   Co = C for i=1 to Nz, where Nz
%                                       is the number of RHP - zeros.
%                                       Cm = Ci, with i = Nz.
%
%  BO(s) is given by:
%      BO(s) = PROD Bi(s), where Bi(s) = I - 2*re(Zi)/(s+Zi') *Yzi*Xzi'
%                          the index runs from i=Nz to one.
%  Xzi and Yzi are the state and the output direction corresponding to the 
%  RHP-zero Zi.
%
%  Possible malfunctions: Multiple zeros located at the same place in the 
%                         complex plane.
%                         RHP-zeros on the jw axis, pure imaginary zeros.
%                         The function has not been tested for number of 
%                         zeros Nz greater than one. However, it should
%                         work OK with Nz > 1. Can also be used for complex
%                         zeros.
%
%  Written by: Kjetil Havre,  15/5 - 1995, e-mail: kjetil@ife.no                         
%
function [Gm, BO, Yz, Zf] = outfact( G, Z, epp, lv )
   if nargin < 2
      disp('usage: outfact(G, Z)')
      Gm = G;
      BO = eye( ny );
      Yz = []; Zf = [];
      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: inpfact(G, Z)');
      Gm = G;
      BO = eye( ny );
      Yz = []; 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?');
      Gm = G;
      BO = eye(ny);
      Yz = []; Zf = [];
      return     
   end
   
   z = Z(1);
   [Ze, yiz, xiz] = ozde( 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.' ] );      
      Gm = G; BO = eye(ny); Yz = []; Zf = [];
   else
      % Iz contains the index to the zero in Ze. 
      % Factorize this zero.
      [A,B,C,D] = unpck( G );
      
      H = [A-z*eye(size(A))  B;   C   D];
      errvk  = [xiz(:,Iz)' yiz(:,Iz)']*H;
      nerrvk = norm(errvk);
      if norm(errvk) > epp
         disp('Error: Could not find generalized eigenvalues.')
         Gm = G; BO = eye(ny); Yz = []; Zf = [];
      end

      Cn  = C - 2*real( Ze(Iz) )*yiz(:,Iz)*xiz(:,Iz)';
      Gm  = pck(A,B,Cn,D);
      br  = nd2sys( [1], [1 conj(Ze(Iz))], 2*real(Ze(Iz)) );
      brm = mmult( yiz(:,Iz), br, yiz(:,Iz)' );
      BO = msub( eye(ny), brm );
      Yz  = yiz(:,Iz);
      Zf  = Ze(Iz);
   end
   
   if nz > 1   % Call recursive for factorizing additional RHP-zeros.
      [Gn, Bn, Yn, Zn] = outfact(Gm, Z(2:nz), epp, lv+1);
      Gm = Gn;
      Ba = mmult(BO, Bn);
      BO = Ba;
      Ua = [Yz Yn];
      Yz = Ua;
      Za = [Zf; Zn];
      Zf = Za;
   end

end


   
   
   
   
   
   
   
   
   
   
   
