%  function [GO, BO, Yz, Xz, Zf] = ozf( G, Z, epp )
%  
%  The OZF function is used to obtain an output factorization of RHP-zeros 
%  so that G(s) = BO(s)*GO(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 OZF function. If the function does
%  not calculate any zeros Zf so that abs(Z(i) - Zf) < EPP the function 
%  skipps Z(i) in the factorization. The default EPP value is 1e-12.
%  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. 
%
%  GO is constructed as follows:
%           G(s) = |  A  |  B  |      GO(s) = |  A   |   B  |
%                  |  C  |  D  |              |  Cm  |   D  |
%  where Cm is obtained by running the following formula repeatedly:
%      Ci = Ci_1 - 2*Re(Zi)*Yzi*Xzi',   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*Yzi'
%                          the index runs from i=Nz to one.
%  Xzi and Yzi are the state and the output direction corresponding to the 
%  RHP-zero Zi for the system Gi_1(s) = |    A   |  B  |
%                                       |--------|-----|
%                                       |  Ci_1  |  D  |
%
%  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
%  Modified by: Kjetil Havre,  30/1 - 1996, e-mail: kjetil@ife.no
%      Modified to also return the state direction.
%
function [GO, BO, Yz, Xz, Zf] = ozf( G, Z, epp, lv )
   if nargin < 2
      disp('usage: ozf(G, Z)')
      GO = G; BO = eye( ny );
      Yz = []; Xz = []; 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: ozf(G, Z)');
      GO = G; BO = eye( ny );
      Yz = []; 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?');
      GO = G; BO = eye(ny);
      Yz = []; Xz = []; 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.' ] );      
      GO = G; BO = eye(ny); Yz = []; Xz = []; 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.')
         GO = G; BO = eye(ny); Yz = []; Xz = []; Zf = [];
      end

      if isreal(z)
         xiz(:,Iz) = real(xiz(:,Iz));
         yiz(:,Iz) = real(yiz(:,Iz));
	 Ze(Iz) = real(Ze(Iz));
      end

      Cn  = C - 2*real( Ze(Iz) )*yiz(:,Iz)*xiz(:,Iz)';
      GO  = pck(A,B,Cn,D);
      Ab  = -conj(Ze(Iz));
      Bb  = 2*real(Ze(Iz))*yiz(:,Iz)';
      Cb  = -yiz(:,Iz);
      Db  = eye(ny);
      BO  = pck(Ab,Bb,Cb,Db);
%      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);
      Xz  = xiz(:,Iz);
      Zf  = Ze(Iz);
   end
   
   if nz > 1   % Call recursive for factorizing additional RHP-zeros.
      [Gn, Bn, Yn, Xn, Zn] = ozf(GO, Z(2:nz), epp, lv+1);
      GO = Gn;
      BO = mmult(BO, Bn);
      Yz = [Yz Yn];
      Xz = [Xz Xn];
      Zf = [Zf; Zn];
   end
end


   
   
   
   
   
   
   
   
   
   
   
