%
%   Function OUTCPRFACT finds factorization such that G(s) = N(s)*M(s)^{-1},
%   where N(jw)^H*N(jw) = I, and both N and M are stable. That means
%   all RHP-zeros in N and all RHP poles as zeros in M. 
%   System G must be square and have no jw-axis poles. 
%
%   Reference: Morari M. ``Robust Process Control'', Prentice Hall 1989.
%

function [N, M, Mi, Q, R, E, X, Xerr] = outcprfact( G, deps, epp )

   if nargin < 2
      deps = 1e-6;
   end
   if nargin < 3
      epp = 1e-12;
   end

   [mt, ny, nu, nx] = minfo(G);
   N = []; M =[];

   if mt ~= 'syst'
      disp('Error: System matrix is requierd');
      return
   end
   
   if ny ~= nu
      disp('Error: Square system matrix is required.');
      return
   end
   
   
   [A,B,C,D] = unpck(G);
   Dn = D;
   SD = svd(D);
   if SD(1,1) < deps
      disp(['Warning: adding diagonal part to D of size: ', num2str(deps), 'I.']);
      Dn = D + deps*eye(size(D));
   end
   
   [Q,R,E] = qr(Dn)
   
   Q'*Q
   
   SQ = svd(vtp(Q)*Q-eye(nu,nu));
   if SQ(1,1) > epp
      disp('Warning: QR decomposition of D is not acheived.');
      return
   end
   
   Aric = A - B*inv(R)*vtp(Q)*C;
   Bric = B*inv(R)*vtp(B*inv(R));
   Cric = zeros(nx,nx);
   

   X = are(Aric, Bric, Cric);
   Xerr = vtp(Aric)*X + X*Aric - X*Bric*X + Cric;

%   SXerr = svd(Xerr)
%
%   Control system Toolbox: tolerance not an input argument.
%   Resulting error is in order od 1e-3.
%

%   H = [ Aric -Bric; -Cric -A' ];
%   eig(H)
%   [x1, x2, fail, reig_min] = ric_eig(H)
%   if fail==1
%      disp('Error: jw - axis eigenvalues.')
%      return
%   end
%   X = x2/x1;
%   Xerr = vtp(Aric)*X + X*Aric - X*Bric*X + Cric;
%   SXerr = svd(Xerr)
%
%
%   Mu - tools are not usable, it returns that H has jw axis 
%        eigenvalues althoug reig_min = 7.5326e-02.
%
%   Conclusion: We need robust control toolbox. 
%

   F = vtp(Q)*C + vtp(B*inv(R))*X;
%   F1 = Q'*C+inv(R)'*B'*X;
%   EF = F-F1
   An = A-B*inv(R)*F;
   Bn = B*inv(R);
   Cn = C-Q*F;
   Dn = Q;
   N = pck(An,Bn,Cn,Dn);
   
   Ami = A;
   Bmi = B;
   Cmi = F;
   Dmi = R;
   Mi = pck(Ami, Bmi, Cmi, Dmi);
   
   Am = Ami-Bmi*inv(Dmi)*Cmi;
   Bm = -Bmi*inv(Dmi);
   Cm = inv(Dmi)*Cmi;
   Dm = inv(Dmi);
   M  = pck(Am, Bm, Cm, Dm);
   
end
