function [U,V,W,L,RES] = DARE(JOB,A,B,Q,R,S,E)
%DARE  Implicit discrete-time Riccati solver.
%
%   [U,V,W,L,RES] = ricpack.DARE(JOB,A,B,Q,R,S,E) computes a basis
%   [U;V;W] of the stable (JOB='s') or anti-stable (JOB='a') invariant
%   subspace of the pencil
%
%                          [ A   0   B]            [E   0   0]
%       M - lambda * N  =  [-Q   E' -S] - lambda * [0   A'  0]
%                          [ S'  0   R]            [0  -B'  0]
%
%   where Q,R are symmetric matrices. The matrix [U;V] is orthogonal,
%   X = V/(E*U) is the (anti-)stabilizing solution of the DARE
%
%     A'XA - E'XE - (A'XB + S) (B'XB+R)^(-1) (B'XA + S') + Q = 0 ,
%
%   and K = -W/U is the associated state-feedback gain.
%
%   This utility also returns the vector L of closed-loop eigenvalues and
%   the vector RES of residuals:
%      RES(1)   Relative residual of the least-squares equation for W.
%      RES(2)   Relative asymmetry of V'*E*U.
%
%   Note:
%     * Does not validate inputs or perform any scaling.
%     * The symplectic spectrum is [L;1./L].
%     * U,V,W are set to [] when the the spectrum separation fails.
%     * L is set to NaN when the pencil is singular ([B;S;R] rank deficient)
%
%   See also RICPACK.CARE.

%   Copyright 2017 The MathWorks, Inc.
SFlag = strcmpi(JOB,'s');
n = size(A,1); n2 = 2*n;
m = size(B,2);
if isempty(E)
   E = eye(n);
end

% Form pencil
M = [A zeros(n) B;-Q E' -S;S' zeros(m,n) R];
N = [E zeros(n) ; zeros(n+m,n) [A';-B']];
RealFlag = isreal(M) && isreal(N);

% Compression step on M(:,n2+1:n2+m) = [B;-S;R]
if m>0
   [QD,~,~,rk] = rrqrf(M(:,n2+1:n2+m),ltipack.getTolerance('rank'));
   if rk<m
      U = [];  V = [];  W = [];  L = NaN(n,1);  RES = NaN(1,2);  return
   end
   M11 = QD'*M(:,1:n2);    N11 = QD'*N;
   M11 = M11(m+1:n2+m,:);  N11 = N11(m+1:n2+m,:);
else
   M11 = M;  N11 = N;
end
%[svd(M11) svd(N11)]

% QZ algorithm
if SFlag
   % Exchange M11 and N11 so that we can use UDO
   tmp = M11;  M11 = N11;  N11 = tmp;
end
M110 = M11;  N110 = N11;
if RealFlag
   [M11,N11,QQ,Z] = qz(M11,N11,'real');
else
   [M11,N11,QQ,Z] = qz(M11,N11,'complex');
end
% Reorder eigenvalues to push eigenvalues outside the unit circle to the top
% Note: Generalized eigenvalues have a tendency to deflate out in decreasing
% order of magnitude, so UDO minimizes need for reordering (Laub trick)
[M11,N11,~,Z,Success] = ordqz(M11,N11,QQ,Z,'udo');

RFAIL = false;
if ~Success && RealFlag
   % RETRY WITH COMPLEX ARITHMERIC
   RFAIL = true;
   % disp('***Retrying ORDQZ with COMPLEX flag')
   [M11,N11,QQ,Z] = qz(M110,N110,'complex');
   [M11,N11,~,Z,Success] = ordqz(M11,N11,QQ,Z,'udo');
   %    if ~Success
   %       disp('### Complex ORDQZ failed ###')
   %    end
end

ev = ordeig(M11,N11);
mev = abs(ev);
Success = Success && all(mev(1:n,:)>1) && all(mev(n+1:n2,:)<1);

% Compute outputs
if Success
   % L
   if SFlag
      L = localDSORT(1./ev(n:-1:1,:));  % preserve a+jb,a-jb pairing order
   else
      L = localDSORT(ev(1:n,:));
   end
   % U,V
   Z = Z(:,1:n);
   if RFAIL
      [ZR,~,~] = qr([real(Z) imag(Z)],'vector');
      ZR = ZR(:,1:n);
      U = ZR(1:n,:);
      V = ZR(n+1:n2,:);
   else
      U = Z(1:n,:);
      V = Z(n+1:n2,:);
   end
   
   % Compute W
   aux = V'*E*U;
   RES2 = norm(aux-aux','fro')/norm(E,'fro'); % asymmetry
   if n==0 || m==0
      W = zeros(m,n);  RES1 = 0;
      if RES2>100*eps && isequal(E,eye(n))
         [U,V] = ricpack.symX(U,V,RealFlag);  RES2 = 0;
      end
   else
      MSCALE = norm(M,1);
      [W,RES1] = localComputeW(A,B,Q,R,S,E,U,V,MSCALE);
      % Try correcting asymmetry (E=I only)
      if RES2>100*eps && isequal(E,eye(n))
         [Ur,Vr] = ricpack.symX(U,V,RealFlag);
         [Wr,RES1r] = localComputeW(A,B,Q,R,S,E,Ur,Vr,MSCALE);
         if RES1r<max(100*eps,2*RES1)
            U = Ur;  V = Vr;  W = Wr;  RES1 = RES1r;  RES2 = 0;
         end
      end
   end
   RES = [RES1 RES2];
else
   U = [];  V = [];  W = [];
   % Beware that unit-circle eigs may end up anywhere
   ev = localDSORT(ev);
   if SFlag
      L = ev(n+1:n2);
   else
      L = ev(1:n);
   end
   RES = NaN(1,2);
end

%---------------------------------------------------------
function [W,RES] = localComputeW(A,B,Q,R,S,E,U,V,SCALE)
% LS computation of W, RES is the corresponding residual
[n,m] = size(B);
[QW,sv,~] = svd([-E*U;A'*V;B'*V]);
QW = QW(:,n+1:2*n+m);
AW = QW'*[B;S;R];
BW = QW'*[A*U;Q*U-E'*V;S'*U];
W = -matlab.internal.math.nowarn.mldivide(AW,BW);
if sv(n,n)>1e2*eps*sv(1,1)
   RES = norm(AW*W+BW,'fro')/(SCALE + norm(abs(AW)*abs(W),'fro'));
else
   % Pencil may have additional infinite eigenvalues when computing
   % the anti-stabilizing solution. W is then partially undetermined
   % and the residual is nonzero, but this is OK since K must be Inf
   % for A-B*K to have an eigenvalue at Inf.
   RES = NaN;
end

function p = localDSORT(p)
% Simplified and IDENT-friendly version of DSORT (do not call DSORT
% which does not ship with IDENT)
[~,is] = sort(-abs(p));
p = p(is);

