%   [I, O, err] = pdisvd(G, P, )
%
%   The ``Pole-Direction-Inverse-SVD'' function computes the input 
%   and output pole directions through the use of Singular Value 
%   Decomposition of G^{-1}(p) where p is a pole.
%   The system matrix G(s) is inverted before evaluation of G(p).
%   If largest singular value of G^{-1}(p) is smaller than EPP then 
%   the corrosponding input and output singular vectors are stored 
%   in columns of I and O. Otherwise, zero vectors are stored in the 
%   corrosponding columns.
%   A warning message is printed if also the second singular 
%   value is grater than EPP.
%   
%   Inputs:  G   - system matrix.
%            P   - vector of poles.
%            epp - tolerance (optional) for singular values, 
%                  default value is 1/eps which most often is
%                  to large typical value around 1e14.
%
%   Outputs: I   - Matrix containing the input directions, 
%                  column i corresponds to pole P(i).
%            O   - Matrix containing the output directions, 
%                  column i corresponds to pole P(i).
%            err - Error indicator:
%                     err =  0, all is OK.
%                     err =  1, No direction with gain larger 
%                               than EPP.
%                     err = -1, One basis vector is not enough
%                               to describe the subspace with 
%                               gains larger than EPP.
%
%   Written by: Kjetil Havre, e-mail: kjetil@ife.no
%
%   See also: izde, ozde, zdsvd.
%
   function [I, O, err] = pdisvd(G, P, vtol, mst, maxitr, delts)

   I = []; O = []; err = 0;

   [mt,ny,nu, nx] = minfo(G);
   
   if strcmp(mt, 'syst')==0
      disp( 'System matrix G is required, usage: [I,O] = pdisvd(G, P, epp)' );
      return
   end
   
   if nargin < 2
      disp( 'Usage: [I,O] = pdisvd(G, P, epp)' );
      return
   end
   
   if nargin < 6
      delts = 100;
   end
   if nargin < 5
      maxitr = 14;
   end
   if nargin < 4
      mst = 1e-10;
   end
   if nargin < 3
      vtol = 1e-6;
   end

   [A,B,C,D] = unpck(G); Bm = B; Cm = C; Db = D;
   if ny ~= nu
      if ny > nu
	 Bm = [B zeros(nx, ny-nu)];   % Add inputs.
         Db = [D zeros(ny, ny-nu)];   % Add inputs.
      else
         Cm = [C; zeros(nu-ny, nx)];  % Add outputs.
         Db = [D; zeros(nu-ny, nu)];  % Add outputs.
      end
   end
      
   nm = max(ny,nu);

   for i=1:max(size(P))
      erc = maxitr;
      delt = delts;
      for j=1:maxitr
         if rank(Db) < nm
            Dm = Db + delt*10*vtol*eye(nm);
         else
            Dm = Db;
         end
	 
         Gm  = pck(A, Bm, Cm, Dm);
         Gi  = minv(Gm);
         wp  = P(i)/sqrt(-1);
         Gip = frsp(Gi, wp);
         [u,s,v] = svd(Gip(1:nm,1:nm));
	 if j > 1;
 	    ypo = yp; upo = up;
	 end
         yp = v(:,1);	 up = u(:,1);
	 if j > 1
            if (norm(yp-ypo) + norm(up-upo)) < 2*sqrt(nm)*vtol
	       erc = 0
	       break;
	    end
	 end
	 delt = delt/2
      end            
      
      % Check and save directions
      nr = min(nu,ny);
      if (s(nr,nr) > mst) | (erc~=0)
         disp('Error: Required tolerance not acheived.');
         I(:,i) = zeros(nu,1);
         O(:,i) = zeros(ny,1);
	 err = 1;
      else
	 if nr > 2 
	    if s(nr-1,nr-1) < mst
               disp(['Warning: Second smallest singular value is less than eep: ', num2str(epp),','])
               disp(['         S(',int2str(nr-1),',',int2str(nr-1),'): ', num2str(s(nr-1,nr-1)),'.'])
               err = -1;
	    end
	 end
         I(:,i) = up(1:nu);
         O(:,i) = yp;(1:ny);
      end
   end
end

   
