%
%  Factorize a pair of complex RHP-zeros in input factorization.
%
%  [GI, BI, Xf, Uf, Zf] = inpcompfact( G, Z, epp )
%  
%  The INPCOMPFACT function is used to obtain an input factorization of
%  a pair of complex RHP-zeros so that G(s) = Gm(s)*BI(s).
%  The input argument Z contains the RHP-zeros. 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-zeros 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] = inpcompfact( G, Z, epp )

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

   [nz, nc] = size(Z);
   if nz < 2
      disp('Error: Two zeros are required.');
      GI = G; BI = eye(nu); Xz = []; Uz = []; Zf = [];
      return     
   end
   if nz > 2
      disp('Warning: Only the first zero and its complexed conjugate is factorized.')
   end
   z1 = Z(1);
   if isreal(z1)
      disp(['Error: Zero is supposed to be complex Z: ', num2str(z), '.']);
      GI = G; BI = eye( nu ); Xz = []; Uz = []; Zf = [];
      return;
   end

   Izc=find(abs(Z-conj(z1))<epp);
   if isempty(Izc)
      disp( ['Error: Unable to find complex conjugate zero ', num2str(conj(z1)),' in Z.'] );
      GI = G; BI = eye(nu); Xz = []; Uz = []; Zf = [];
      return;      
   end
   
   [Ze, uiz, xiz] = izde( G );              % Compute directions.
   
   Iz1 = find( abs(Ze-z1) < epp );  
   if isempty(Iz1)
      disp( ['Error: Requested zero Z(1): ',...
	           num2str(z1), ' is not in system.' ] );      
      GI = G; BI = eye(nu); Xz = []; Uz = []; Zf = [];
      return;
   end

   Xz1 = xiz(:,Iz1);    Uz1 = uiz(:,Iz1);
   [A,B,C,D] = unpck( G );
   evk1 = [Xz1; Uz1];
   M = [A-eye(size(A))*z1   B;  C    D ];
   epvk1=M*evk1;
   nepvk1 = norm(epvk1);
   
   if nepvk1 > epp
      disp(['Error: Unable to solve generalized eigenvalue problem: ', num2str(nepvk1),'.'] )
      GI = G; BI = eye(nu); Xz = []; Uz = []; Zf = [];
      return;
   end
   
   
   Bn = B - 2*real(z1)*Xz1*Uz1';
   Gn = pck(A,Bn,C,D);

   [Ze, uiz, xiz] = izde( Gn );             % Compute directions.
   Iz2 = find( abs(Ze-conj(z1)) < epp );

   if isempty(Iz2)
      disp( ['Error: Requested zero conj(Z(1)): ',...
	           num2str(conj(z1)), ' is not in system.' ] );      
      GI = G; BI = eye(nu); Xz = []; Uz = []; Zf = [];
      return;
   end
   Xz2 = xiz(:,Iz2);
   Uz2 = uiz(:,Iz2);
   evk2 = [Xz2; Uz2];
   M = [A-eye(size(A))*conj(z1)   Bn;  C    D ];
   epvk2=M*evk2;
   nepvk2 = norm(epvk2);
   
   if nepvk2 > epp
      disp(['Error: Unable to solve generalized eigenvalue problem: ', num2str(nepvk2),'.'] )
      GI = G; BI = eye(nu); Xz = []; Uz = []; Zf = [];
      return;
   end
   
   Xz2t = conj(Xz1);
   Uz2t = conj(Uz1);
   
   EUz = Uz2 - conj(Uz1)
   

   Xz     = [Xz1 Xz2];
   Uz     = [Uz1 Uz2];
   Bn     = B - 2*real(z1)*Xz*Uz'
   Bnr    = real(Bn);
   eppBnr = norm(Bn - Bnr);
   
   if eppBnr > epp
      disp( 'Warning: A real realization of GI is not found.' );
      Bnr = Bn;       
   end
   
   GI = pck(A,Bn,C,D);
   Ab  = -conj(z1); Bb = 2*real(z1)*Uz1'; Cb  = -Uz1; Db = eye(nu);
   B1  = pck(Ab,Bb,Cb,Db);
   Ab  = -z1; Bb = 2*real(z1)*Uz2'; Cb  = -Uz2; Db = eye(nu);   
   B2  = pck(Ab,Bb,Cb,Db);
   
   Ac = [-real(z1) imag(z1); -imag(z1) -real(z1)];
   Cc  = [eye(min(ny,2)); zeros(ny-min(ny,2),2)];
   Xo = -pinv(Cc)*Uz;
   
   BI  = mmult(B2, B1);
   [Ab,Bb,Cb,Db] = unpck(BI);
   
   Ab
   
   Af = [-real(z1) imag(z1); -imag(z1) -real(z1)]
   
   T = are(zeros(2,2), Ab, Af)
   
   inv(T)*Af*T-Ab
   
   a1 = Ab(1,1); a2 = Ab(1,2); a3 = Ab(2,1); a4 = Ab(2,2);
   im = imag(z1);  re = real(z1);
   
   M = [ a1+re   0     a2   im
         -im   a1+re    0   a2
	  a3    0    a4+re  im
	  0     a3    -im a4+re ];
   rkm = rank(M)
   
   if rkm == 4
      disp('Warning: Can not find similarity transformation to convet to real realization.')
      T = eye(2);
   else
      [U,S,V] = svd(M);
      X = V(:,4)
      T(1,1) = X(1); T(1,2) = X(2); T(2,1) = X(3); T(2,2) = X(4);
      T
   end
   
   
   
   Af
   An = real(inv(T)*Ab*T)
   eig(An)
   Bn = inv(T)*Bb
   % angle(Bn)*180/pi
   Cn =  Cb*T
   
   % angle(Cn)*180/pi
   
   Dn = Db
   
   BI = pck(An,Bn,Cn,Dn);

%   BI0 = Cn*inv(-An)*Bn + Dn   
%   inv(BI0)  
%   Cn*Bn
%
%   Since BI0 is real and inv(B10) is real and Cn*Bn is real
%   It shuld be possible to find a unitary matrix UB such that
%   B in BI is real.
%
%
   
%   f1 = nd2sys( [1 -conj(z1)], [1 2*real(z1) z1*conj(z1)], 4*real(z1) )
%   f2 = nd2sys( [1 -z1], [1 2*real(z1) z1*conj(z1)], 4*real(z1) )
%   b1 = mmult(Uz1,f1,Uz1');
%   b2 = mmult(Uz2,f2,Uz2');
%   BI = msub(eye(nu), b1, b2);
      
%    f1 = nd2sys( [1 -2*real(z1) z1*conj(z1)], [1 2*real(z1) z1*conj(z1)], 1 );
%    Df = daug(f1, f1);
%    UzUzH  = Uz*Uz';
%    UzUz = real(UzUzH);
%    if norm(UzUzH-UzUz) > epp
%       disp('Warning: Real realization of Uz*Uz^H is not acheived.')
%       UzUz = UzUzH;
%    end
%    BI = mmult(Df, UzUz);
%
%    This BI does not work. 
%    Kjetil Havre 14/12-1995.
%

end








