function [p1,p2,lamp,perr,wellposed,p] = aresolv(a,q,R,Type)
%
% [P1,P2,LAMP,PERR,WELLPOSED,P] = ARESOLV(A,Q,R,TYPE) computes the 
% solution to the algebraic Riccati equation
% 
%                     A'P + PA - PRP + Q = 0
%
%   based on the "Type" one selects:
% 
%                Type = 'eigen' ---- Eigenstructure approach
%
%                Type = 'Schur' ---- Schur vector approach
%
%  Outputs:   P = P2/P1 riccati solution
%             [P1;P2] stable eigenspace of hamiltonian [A,-R;-Q,-A']
%             LAMP closed loop eigenvalues
%             PERR residual error matrix 
%             WELLPOSED = 'TRUE ' if hamiltonian has no imaginary-axis
%                  eigenvalues, otherwise 'FALSE'

% R. Y. Chiang & M. G. Safonov 2/88
% Copyright (c) 1988 by the MathWorks, Inc.
% All Rights Reserved.

wellposed='TRUE ';  %initialize 
if nargin == 3
   Type = 'eigen';
end
%
[n,n] = size(a);
%
% ------ If A is stable and Q is zero, p2 = zero(n), p1 = eye(n)
%
lamA = eig(a);
ind = find(lamA>0);
if (sum(ind) == 0) & (norm(q,'fro') == 0)
   p2 = zeros(n); p1 = eye(n);
   p = p2; perr = p2;
   return
end
%   
ham = real([a -R;-q -a']);
%
% ------ Eigenstructure approach:
%
if Type == 'eigen'
   [v,d] = reig(ham);
   if rank(v) < 2*n
      Type = 'Schur';
   end
   p1 = v(1:n,1:n);
   p2 = v((n+1):(2*n),1:n);
end
%
% ------ Schur vector approach:
%
if Type == 'Schur'
   [v,t,m,swap] = cschur(ham,1);
   vv = [real(v(:,1:m)) imag(v(:,1:m))];
   [vh,rh] = qr(vv);
   p1 = vh(1:n,1:n);
   p2 = vh(n+1:2*n,1:n);
end
%
if nargout == 6
   p = real(p2/p1);
end
%
% ------ Residual of the Riccati solution:
%
perr = [p2' -p1']*[a -R;-q -a']*[p1;p2];
%
% ------ Check jw-axis roots:
%
[gvv,dd] = eig(a*p1-R*p2,p1);
%
% ------ if the generalized e-value is NaN or Inf, assign to -1.e15:
%
for i = 1:n
   if abs(dd(i,i)) == NaN | abs(dd(i,i)) == inf
      dd(i,i) = -1.e15;
   end
end
lamp = diag(dd);
%
if max(real(lamp)+1.e-6*abs(imag(lamp))) > -1.e-13
     wellposed='FALSE';
end
%
% ------- End of ARESOLV.M ---- % RYC/MGS %