% [k, gam, X, Y] = hinfkcf (sys, dim, key1, value1, ...)
%
% This function can be used to find a Hinf-controller using LMIs
%
% Inputs
%   sys    an LTI model
%   dim    [nmeas, ncont]
%          nmeas  controller inputs
%          ncont  controller outputs
%   key           value
%     'sdpsettings'  sdpsettings options used for yalmip, overrides solver
%     'solver'    'lmilab', 'sedumi', 'sdpt3'
%     'tol'       tolerance in Guptri algorithm, default 1e-12
%     'gain'      value of target Hinf gain / gamma
%     'gamma'     value of target Hinf gain / gamma
%     'method'    solution method: 0, 1, 2
%     'verbose'   verbosity in LMI solver: 0, 1, ...
%     'debug'     verbosity in hinfkcf: 0, 1, ...
%
% Outputs
%   k      the LTI controller
%   gam    obtained gamma value
%   X, Y   LMI solutions
%
% method = 0 : full LMI followed by state reduction
% method = 1 : find min gamma with full LMI truncation
% method = 2 : LMI truncation of strictly stable KCF blocks
%
% Version 2014-12-18

function [k, gam, X, Y] = hinfkcf (sys, dim, varargin)

if nargin < 1,
  disp ('usage:  [k, gam, X, Y] = hinfkcf (sys, dim, key1, value1, ...)');
  return;
end

k = [];
X = [];
Y = [];
gam0 = [];

% default parameters
solver = 'sedumi';
tol = 1e-12;
method = 0; % standard LMI algorithm
verb = 0;
debug = 0;

for i = 1:2:nargin-3,
  if strcmp (varargin{i},     'sdpsettings'),
    opts = varargin{i+1};
  elseif strcmp (varargin{i}, 'solver'),
    solver = varargin{i+1};
  elseif strcmp (varargin{i}, 'tol'),
    tol = varargin{i+1};
  elseif strcmp (varargin{i}, 'verbose'),
    verb = varargin{i+1};
  elseif strcmp (varargin{i}, 'debug'),
    debug = varargin{i+1};
  elseif strcmp (varargin{i}, 'gain') || strcmp (varargin{i}, 'gamma'),
    gam0 = varargin{i+1};
  elseif strcmp (varargin{i}, 'method'),
    method = varargin{i+1};
    if method > 2 || method < 0,
      error ('invalid method');
    end
  else
    warning ('unrecognized key: %s', varargin{i});
  end
end

% adapted to yalmip 3.0
yalmip ('clear');
if exist ('opts', 'var'),
  if debug,
    disp ('options provided by sdpsettings parameters passed direct to yalmip');
  end
  % use settings provided by sdpsettings options
  % overrides solver option
elseif strcmp (solver, 'lmilab'),
  opts = sdpsettings ('solver',    'lmilab', ...
                      'verbose',      verb, ...
                      'beeponproblem',  []);
  opts.lmilab.reltol = 1e-6;
elseif strcmp (solver, 'sdpt3'),
  opts = sdpsettings ('solver',    'sdpt3', ...
                      'debug',           1, ...
                      'verbose',      verb, ...
                      'beeponproblem',  []);
elseif strcmp (solver, 'sedumi'),
  opts = sdpsettings ('solver',    'sedumi', ...
                      'verbose',       verb, ...
                      'beeponproblem',  []);
else
  opts = sdpsettings ('solver',     solver, ...
                      'verbose',      verb, ...
                      'beeponproblem',  []);
end

sys = ss (sys);   % convert any LTI system to state space

[p, m] = size (sys);
ns = size (sys, 'order');  % number of states

if debug,
  if sys.Ts > 0,
    disp (sprintf ('The system is discrete time with %d states', ns));
  else
    disp (sprintf ('The system is continuous time with %d states', ns));
  end
end


if nargin > 1,
  p2 = dim(1);  % nmeas
  m2 = dim(2);  % ncont
  if length(dim) > 2,
    rs = dim(3);
  else
    rs = [];
  end

  if p2 < 1,
    error ('there must be at least one controller input');
  elseif p2 > p,
    error ('too many controller inputs');
  end

  if m2 < 1,
    error ('there must be at least one controller output');
  elseif m2 > m,
    error ('too many controller outputs');
  end

else
  p2 = 0;
  m2 = 0;
end

p1 = p - p2;   % performance block
m1 = m - m2;   % H-inf

% We first assume that d22 is zero.
% Finally, we modify K by an LFT if this is not the case.

if isempty (gam0),
  gam = sdpvar (1, 1);
else
  gam = gam0;
end

if debug, fprintf ('FX: '); end
[Fx, X22, Rx, gamx, Qx] = syn (gam, sys, dim, method, tol);
if debug, fprintf ('FY: '); end
[Fy, Y22, Ry, gamy, Qy] = syn (gam, sys.', [m2 p2], method, tol);
F = Fx + Fy;
if ns > 0,
  F = F + ([X22 Rx'*Ry; Ry'*Rx Y22] >= 0);
end

gammin = max ([gamx gamy]);
if ~isempty (gammin),
  if isempty (gam0),
    F = F + (gam >= gammin);
  elseif gam0 < gammin,
    error ('target gam0 lower than bound: %10.4g', gammin);
  end
end

warning_state = warning;
if isempty (gam0),
  sol = optimize (F, gam, opts);
else
  sol = optimize (F, trace (X + Y), opts);
end
warning (warning_state);

if sol.problem,
  warning ('problems finding X-Y solution: %s', yalmiperror (sol.problem));
end

gam = max (0.0, value (gam));
if debug, fprintf ('gam = %.8g\n', gam); end

if ns > 0,
  X22 = value (X22);
  Y22 = value (Y22);
  X = Rx * X22 * Rx';
  Y = Ry * Y22 * Ry';
  nx2 = size (Rx, 2);
  ny2 = size (Ry, 2);
  nx = [ns-nx2 nx2];
  ny = [ns-ny2 ny2];

  % Find upper triangular U and V such that
  % Perm = U' * Qx' * Qy * V
  % = [I 0 0 0;
  %    0 0 I 0;
  %    0 I 0 0;
  %    0 0 0 I];
  % where each row/column have dimensions given in nr
  %
  % nr(1,1)+nr(1,2) = size(Rx,2) and nr(1,1)+nr(2,1) = size (Ry,2)

  [Perm, U, V, nr] = partition (Qx, nx, Qy, ny);

  if norm (U'*Qx'*Qy*V - Perm) > 1e-10,
    warning ('partition failed');
  end

  if norm (Perm - eye(size(Perm)), 'fro') > 1e-10,
    if norm (Perm - round (Perm), 'fro') < 1e-8,
      disp ('Perm =');
      disp (round (Perm)),
    else
      Perm
    end
    warning ('This case is not yet tested');
  end

  % Use Schur complement on X22 and Y22
  indx = [sum(sum(nr(1,:)))+(1:sum(nr(end,:)))];
  X22m = U(indx,indx) \ X22 / U(indx,indx)';
  if all (nr(2,1:2) > 0),
    % modify U with respect to Schur complement
    indx1 = nr(2,1)+(1:nr(2,2));
    indx2 = 1:nr(2,1);
    [X22m, T] = schurcompl (X22m, indx1, indx2);
    % X22m = T' * X22m * T

    U(:,indx) = U(:,indx) / T';
    indy = [nr(1,1)+indx2 sum(nr(1,:))+indx1];
    V(:,indy) = V(:,indy) * T;
  end

  indy = [nr(1,1)+(1:nr(1,2)) nr(1,1)+nr(1,2)+nr(2,1)+(1:nr(2,2))];
  Y22m = V(indy,indy) \ Y22 / V(indy,indy)';
  if all (nr(1:2,2) > 0),
    % modify V with respect to Schur complement
    indy1 = nr(1,2)+(1:nr(2,2));
    indy2 = 1:nr(1,2);
    [Y22m, T] = schurcompl (Y22m, indy1, indy2);
    % Y22m = T' * Y22m * T
    V(:,indy) = V(:,indy) / T';
    indx = [nr(1,1)+indy2 sum(nr(:,1))+indy1];
    U(:,indx) = U(:,indx) * T;
  end

  if norm (U'*Qx'*Qy*V-Perm) > 1e-10,
    warning ('Schur scaling failed');
    disp ('U''*Qx''*Qy*V = ')
    disp (U'*Qx'*Qy*V)
  end

  rr = nr;

  R = [];
  S = [];
  if nr(1,1) > 0,
    Inr1 = eye(nr(1,1));
    R11 = [Inr1  Inr1;  Inr1 Inr1];
    S11 = [Inr1 -Inr1; -Inr1 Inr1];
    if nr == [1 1; 0 3],
      disp ('NN9 and similar')
      R11 = Inr1; S11 = 0*Inr1; rr(1,1)=0;
    elseif nr == [8 0; 0 0],
      disp ('NN10 and similar')
      R11 = Inr1; S11 = 0*Inr1; rr(1,1)=0;
    elseif nr == [2 0; 0 3],
      disp ('ROC6 and similar')
      R11 = Inr1; S11 = 0*Inr1; rr(1,1) = 0;
    end
    R = blkdiag (R, R11);
    S = blkdiag (S, S11);
  end

  if nr(1,2) > 0,
    % TF1
    if nr == [0 7;0 0],
      disp ('TF1 or similar structure')
      rr(1,2) = 3;

      nn12 = nr(1,2) - rr(1,2);
      indx1 = 1:nn12;
      indx2 = nn12+1:nr(1,2);
      [Y22m, T] = schurcompl (Y22m, indx1, indx2);
      % Y22m = T' * Y22m * T
      V(:,indy) = V(:,indy) / T';
      U(:,indy) = U(:,indy) * T;
      % square root implementation
      [R22m, pR22m] = chol (Y22m(indx1,indx1));
      % R22m'*R22m = Y22m
      if pR22m == 0,
        [~, s, v] = svd (R22m); % u not needed
        i = find(diag(s) < 1e-4);
        s(i,i) = 0;
        Y22m(indx1,indx1) = v*s^2*v';
      else
        warning ('Cholesky decomposition failed');
        [u, s, v] =  svd (Y22m(1:nn12,1:nn12));
        i = find (diag(s) < 1e-8);
        s(i,i) = 0;
        Y22m(1:nn12,1:nn12) = u*s*v';
      end
      TS = [eye(nr(1,2)) [zeros(nn12,rr(1,2));
                          -eye(rr(1,2))]];

      R = blkdiag (R, eye(nr(1,2)), eye(rr(1,2)));
      S = blkdiag (S, TS' * Y22m * TS);

    elseif nr == [0 3;0 1],
      disp ('REA1 or similar')
      rr(1,2) = 0;
      S22 = Y22m(1:nr(1,2),1:nr(1,2));
      R = blkdiag (R, eye(nr(1,2)), eye(rr(1,2)));
      S = blkdiag (S, [S22 -S22; -S22 S22]);
    else
      S22 = Y22m(1:nr(1,2),1:nr(1,2));
      R = blkdiag (R, eye(nr(1,2)), eye(rr(1,2)));
      S = blkdiag (S, [S22 -S22; -S22 S22]);
    end
  end

  if nr(2,1) > 0,
    if nr == [2 0; 2 0],
      disp ('AC12 and similar');
      R33 = X22m(1:nr(2,1),1:nr(2,1));
      R = blkdiag (R, [R33 R33; R33 R33]);
      S = blkdiag (S, eye(nr(2,1)), eye(rr(2,1)));
    else
      R33 = X22m(1:nr(2,1),1:nr(2,1));
      R = blkdiag (R, [R33 R33; R33 R33]);
      S = blkdiag (S, eye(nr(2,1)), eye(rr(2,1)));
    end
  end

  if nr(2,2) > 0,
    indx = nr(2,1)+(1:nr(2,2));
    indy = nr(1,2)+(1:nr(2,2));
    [R44 , S44, rr(2,2)] = recoverRS (X22m(indx,indx), ...
                                      Y22m(indy,indy), ...
                                      rr(2,2), debug);
     R = blkdiag (R, R44);
     S = blkdiag (S, S44);
  end

  if debug,
    nr, rr
  end

  indx = [1:nr(1,1) ...
          nr(1,1)+rr(1,1)+(1:nr(1,2)) ...
          nr(1,1)+rr(1,1)+nr(1,2)+rr(1,2)+(1:nr(2,1)) ...
          nr(1,1)+rr(1,1)+nr(1,2)+rr(1,2)+nr(2,1)+rr(2,1)+(1:nr(2,2)) ...
          nr(1,1)+(1:rr(1,1)) ...
          nr(1,1)+rr(1,1)+nr(1,2)+(1:rr(1,2)) ...
          nr(1,1)+rr(1,1)+nr(1,2)+rr(1,2)+nr(2,1)+(1:rr(2,1)) ...
          nr(1,1)+rr(1,1)+nr(1,2)+rr(1,2)+nr(2,1)+rr(2,1)+nr(2,2)+(1:rr(2,2))];

  rs = sum (sum (rr));
  R = R(indx,indx);
  S = S(indx,indx);

  R = R * blkdiag (Perm * U' * Qx', eye(rs));
  S = S * blkdiag (V' * Qy', eye(rs));

  if nr(2,2) == ns,
    [X, Y, rs] = rankdef (X, Y, sys, dim, debug);
    if rs > ns, rs = []; end
    [R , S, rs] =  recoverRS (X, Y, rs, debug);
  end
end
d11 = sys.d(1:p1,1:m1);
d12 = sys.d(1:p1,m1+1:m);
d21 = sys.d(p1+1:p,1:m1);

gIm1 = gam*eye(m1);
gIp1 = gam*eye(p1);

% The standard Bounded Real Lemma is given by
%
%  Q + U*K*V' + V*K'*U' < 0
%
% Continuous time
%  Q = [P*A+A'*P   P*B       C'
%       B'*P     -gam*Im     D'
%       C           D     -gam*Ip ];
%
% We modify this by a applying a similarity transformation, S, applied
% to the first row and column
%  Q = [S*P*A*S'+S*A'*P*S'   S*P*B        S*C'
%       B'*P*S'             -gam*Im        D'
%       C*S'                    D       -gam*Ip ];
% Let R=S*P and we get
%  Q = [R*A*S'+(R*A*S')'    R*B      (C*S')'
%        (R*B)'           -gam*Im      D'
%        C*S'                D     -gam*Ip ];
%
% Discrete time
%  Q = [A'*P*A'-P   A'*P*B          C'
%       B'*P*A     B'*P*B-gam*Im    D'
%       C            D           -gam*Ip ];
%
% whis is rewritten as
%
%  Q = [-P      0      A'*P    C'
%        0   -gam*Im   B'*P    D'
%       P*A    P*B     -P      0
%        C'     D       0  -gam*Ip ];
%
% Congruence transformation with S
%  Q = [-S*P*S'    0      S*A'*P*S'   S*C'
%        0       -gam*Im   B'*P*S'      D'
%      S*P*A*S'   S*P*B    -S*P*S'      0
%        C*S'        D        0     -gam*Ip ];
%
% or with R=S*P
%  Q = [-S*R'*R*S'    0      S*A'*R'     S*C'
%        0        -gam*Im     B'*R'       D'
%        R*A*S'      R*B     -S*R'*R*S'   0
%        C*S'         D        0      -gam*Ip ];
%
% One important property of this formulation is that we can allow
% for nonsingular R and S. We can also reduce the condition number of
% R and S.

if ns > 0,
  b2  = sys.b(:,m1+1:m);
  c2  = sys.c(p1+1:p,:);
  RAS = R(:,1:ns) * sys.a * S(:,1:ns)';
  RB1 = R(:,1:ns) * sys.b(:,1:m1);
  C1S = sys.c(1:p1,:) * S(:,1:ns)';

  if sys.Ts > 0,
    % discrete time
    nR = size (R, 1);
    z12 = zeros (nR, m1);
    z43 = zeros (p1, nR);
    RS  = R * S';
    SRRS = RS'*RS;

    Q = [-SRRS   z12    RAS'   C1S'
          z12'   -gIm1   RB1'   d11'
          RAS     RB1   -SRRS   z43'
          C1S     d11    z43   -gIp1 ];

    U = [zeros(nR+m1,m2+rs);
         R*blkdiag(b2,eye(rs));
         d12    zeros(p1,rs)];

    V = [blkdiag(c2,eye(rs))*S'  blkdiag(d21,zeros(rs,nR+p1))]';
  else
    % continuous time
    Q = [RAS+RAS'  RB1    C1S'
         RB1'     -gIm1   d11'
         C1S       d11   -gIp1];

    U = [R*blkdiag(b2,eye(rs));
         zeros(m1,m2+rs);
         d12    zeros(p1,rs)];

    V = [blkdiag(c2,eye(rs))*S'  blkdiag(d21,zeros(rs,p1))]';
  end
else
  Q = [-gam*Im1   d11';
        d11      -gam*Ip1];
  U = [zeros(m1,m2)  d12];
  V = [d21  zeros(p2,p1)]';
end

try
  K = recoverK (Q, U, V);
  if isempty (K),
    disp ('trying LMI-solver for finding K');
    K = recoverKlmi (Q, U, V, opts);
  end

  if sys.Ts > 0,
    k = ss (K(m2+1:end,p2+1:end), K(m2+1:end,1:p2), ...
            K(1:m2,p2+1:end),     K(1:m2,1:p2), sys.Ts);
  else
    k = ss (K(m2+1:end,p2+1:end), K(m2+1:end,1:p2), ...
            K(1:m2,p2+1:end),     K(1:m2,1:p2));
  end
  d22 = sys.d(p1+1:p,m1+1:m);
  try
    k = star ([zeros(m2,p2), eye(m2); eye(p2) -d22], k);
  catch
    disp ('problem occured when modifying k for nonzero d22');
    if min (svd(eye(p2)+d22*k.d)) < 10*eps,
      disp ('(I + d22*k.d) seems to be (close to) singular');
    end
    disp ('you can try to modify d22 or set it to zero');
    disp ('the k corresponding to d22 = 0 is returned');
  end

  % check solution

  cl = star (sys, k);
  hinfnorm = norm (cl, inf, 1e-6);
  if hinfnorm > (1.0+1e-6)*gam,
    warning ('Hinf norm (%.6g) higher than target (%.6g)', hinfnorm, gam);
  end

  if cl.Ts > 0 && max (abs (pole (cl))) > 1 ...
     || cl.Ts == 0 && max (real (pole (cl))) > 0,
    warning ('closed loop system is unstable');
  end

  rs = size (k.a, 1);

catch
  warning ('failed to find controller');
  k = [];
end



%%%%%%%%%%%%%%%%%%%% internal functions %%%%%%%%%%%%%%%%%%%%

function [F, X, Q0, gammin, Q] = syn (gam, sys, dim, method, tol)

ns = size (sys.a, 1);

% Continuous time
%
%     [I]                []'   [0      0      c1' ]
% Q = [0]  X [a  b1 0] + []  + [0  -gam*Im1  d11' ]
%     [0]                []    [c1    d11 -gam*Ip1];
%
% Discrete time
%     [ a']               [I]             [0      0      c1' ]
% Q = [b1'] X [a  b1 0] - [0] X [I 0 0] + [0  -gam*Im1  d11' ]
%     [ 0 ]               [0]             [c1    d11 -gam*Ip1];
%
% LMI = blkdiag(NX,I)' * Q * blkdiag(NX,I) < 0

[nx1, nx2, nx3, nx4] = computeNX (sys, dim);

if method == 0,
  a0 = nx1';
  b0 = nx2';
  c0 = nx3';
  d0 = nx4';
  Q0 = eye(ns);
  gammin = [];
  Q = eye(ns);
else
  % sort the blocks for controller synthesis
  % L, J(-), J(+), N, L', J(0)

  if method == 2,
    [a, b, Q, P, kstr] = guptri2 (nx1, nx2, tol, 'rhp');
    a = a(end:-1:1,end:-1:1)';
    b = b(end:-1:1,end:-1:1)';
    Q = Q(:,end:-1:1);
    P = P(:,end:-1:1);
  else
    [a, b, P, Q, kstr] = guptri2 (nx1', nx2', tol);
  end

  norma = norm (P'*nx1'*Q - a);
  normb = norm (P'*nx2'*Q - b);
  if norma > 2*tol*norm([nx1;nx2]) || normb > 2*tol*norm([nx1;nx2]),
    if norma > 2*tol*norm([nx1;nx2]),
      warning ('P''*nx1''*Q-a is large: %9.2e\n', norma);
    end
    if normb > 2*tol*norm([nx1;nx2]),
      warning ('P''*nx2''*Q-b is large: %9.2e\n', normb);
    end
  end

  c = P'*nx3';
  d = P'*nx4';
  if method == 2,
    [a0, b0, c0, d0, Q0, gammin] = evalguptri2 (a, b, c, d, Q, kstr, 2);
  else
    [a0, b0, c0, d0, Q0, gammin] = evalguptri2 (a, b, c, d, Q, kstr);
  end

  if gammin > eps,
    fprintf ('gammin = %9.2e\n', gammin);
  else
    gammin = [];
  end

end

% find a stable X for the truncated parts of X
if method > 0,
  nx = size (a0, 2);
  if nx < ns,
    fprintf ('%d state', nx);
    if nx ~= 1, fprintf ('s'); end
  end
  if max (max (abs (imag(Q0)))) > tol,
    if nx < ns,
      X = sdpvar (nx, nx, 'hermitian', 'complex');
    end
    fprintf (' in complex LMI\n')
  else
    X = sdpvar (nx, nx, 'symmetric');
    if nx < ns,
      fprintf (' in real LMI\n')
    end
  end
else
  X = sdpvar (ns, ns, 'symmetric');
end

try
  if sys.Ts > 0,
    UXU = a0*X*a0';
    VXV = b0*X*b0';
    F = ([-gam*(d0*d0')+UXU-VXV'  c0;
            c0'                 -gam*eye(size(c0,2))] <= 0);
  else
    UXV = a0*X*b0';
    F = ([-gam*(d0*d0')+UXV+UXV'  c0;
            c0'                 -gam*eye(size(c0,2))] <= 0);
  end
catch
  F = [];
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function K = recoverKlmi (Q, U, V, opts)

yalmip ('clear');

K = sdpvar (size (U,2), size (V,2), 'full');
gap = sdpvar (1, 1);

F = [Q + U*K*V' + (U*K*V')' < gap*eye(size(Q))];

warning_state = warning;
sol = optimize (F, gam, opts);
warning (warning_state);

if sol.problem,
  warning ('problems occurred when trying to find K');
end

K = value (K);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function K = recoverK (Q, U, V)
% finds a solution to the problem Q + U*K*V' + (U*K*V')' < 0

T = orth ([Q U V]);
Q=T'*Q*T;
U=T'*U;
V=T'*V;

if min(size(U)) > rank(U),
  warning ('U is not full rank: loosing %d; pole placement not yet implemented', ...
           min(size(U)) - rank(U));
end
if min(size(V)) > rank(V),
  warning ('V is not full rank: loosing %d; pole placement not yet implemented', ...
           min(size(V)) - rank(V));
end

T1 = null ([U  V]');
T2 = null ([V  T1]');
T3 = null ([U  T1]');
T4 = null ([T1 T2 T3]');

TQT = [T1 T2 T3 T4]'*Q*[T1 T2 T3 T4];
n1 = size(T1,2);
n2 = size(T2,2);
n3 = size(T3,2);
n4 = size(T4,2);

% Schur complement on first row/column
Q11 = TQT(1:n1,1:n1);
if max (real (eig(Q11))) > -eps,
  warning ('Q11 not negative definite');
end
if n1 > 0,
  TQT = TQT(n1+1:end,n1+1:end) ...
      - TQT(n1+1:end,1:n1)/TQT(1:n1,1:n1)*TQT(1:n1,n1+1:end);
end

% K is in [Q23 Q24; Q43 Q44]
Q23 = TQT(1:n2,n2+(1:n3));
Q24 = TQT(1:n2,n2+n3+1:end);
Q43 = TQT(n2+n3+1:end,n2+(1:n3));
Q44 = TQT(n2+n3+1:end,n2+n3+1:end);

if n4 > 0,
  gap = max (0, max (real(eig(Q44))));
  warning ('T4(%d) case in recoverK not fully tested\n', n4);
else
  gap = 0;
end

K = (U'*[T2 T4])' \ ...
     [-Q23 -Q24; -Q43 -gap*eye(size(Q44))] ...
     / (V'*[T3 T4]);

if 0,
  nU = null (U');
  disp ('nU''*Q*nU')
  max (real (eig(nU'*Q*nU)))
  nV = null (V');
  disp ('nV''*Q*nV')
  max (real (eig(nV'*Q*nV)))
  max (real (eig(Q+U*K*V'+(U*K*V')')))
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [NX1, NX2, NX3, NX4] = computeNX (sys, dim)
[p, m] = size (sys);
ns = size (sys.a, 1);
p2 = dim(1);
m2 = dim(2);

a   = sys.a;
b1  = sys.b(:,1:m-m2);
b2  = sys.b(:,m-m2+1:m);
c1  = sys.c(1:p-p2,:);
c2  = sys.c(p-p2+1:p,:);
d11 = sys.d(1:p-p2,1:m-m2);
d12 = sys.d(1:p-p2,m-m2+1:m);
d21 = sys.d(p-p2+1:p,1:m-m2);

NX = null ([c2 d21]);
NX1 = [a b1]*NX;
NX2 = NX(1:ns,:);
NX3 = [c1 d11]*NX;
NX4 = NX(ns+1:end,:);  % [0 eye(m1)]*NX

if rank(NX3) < min (size (NX3)),
  fprintf ('[c1 d11]*NX looses rank: %d < %d\n', rank(NX3), min(size(NX3)));
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [Xmod, Ymod, rs] = rankdef (X, Y, sys, dim, verb)

[p, m] = size (sys);
ns = size (X, 1);
[NX1, NX2] = computeNX (sys, dim);
[NY1, NY2] = computeNX (transpose(sys), [dim(2) dim(1)]);
rx1 = rank (NX1');
rx2 = rank (NX2');
ry1 = rank (NY1');
ry2 = rank (NY2');

rx = min (rx1, rx2);
ry = min (ry1, ry2);
rs = min (rx, ry);

if verb && rs < ns,
  fprintf ('NX1 has rank %d and NY1 has rank %d,', rx, ry);
end

if  rs >= ns,
  Ymod = Y;
  Xmod = X;
elseif rx <= ry,
  if rx1 <= rx2,
    Tn = [orth(NX1) null(NX1')];
    if verb, fprintf (' using NX1;'); end
  else
    Tn = [orth(NX2) null(NX2')];
    if verb, fprintf (' using NX2;'); end
  end

  [R, pxy] = chol ([X eye(ns); eye(ns) Y]);
  mod = 1e-9;
  while pxy > 0 && mod < 1e-5,
    [R, pxy] = chol ([X eye(ns); eye(ns) Y]+mod*eye(ns+ns));
    mod = 2*mod;
  end
  if pxy == 0,
    R = triu (qr (R(:,[ns+1:end 1:ns])));
    RXYT = R(ns+1:end,ns+1:end)*Tn;
  else
    warning ('chol of [X I; I Y] failed ');
  end

  [Rx, p] = chol (X);
  if p == 0,
    TX = Rx*Tn;
    TXT = TX'*TX;
  else
    warning ('chol of X failed')
    TXT = Tn'*X*Tn;
  end

  [Ry, p] = chol (Y);  % Y = Ry'*Ry
  if p == 0,
    TY = Ry'\Tn;
    TYT = TY'*TY;
  else
    warning ('chol of Y failed')
    TYT = Tn'*inv(Y)*Tn;
  end
   % TYT = Tn'*inv(Y)*Tn;
  TXYT = TXT - TYT;
  if pxy == 0,
    TXYT = RXYT'*RXYT;
  end
  TXT(rs+1:end,rs+1:end) = TYT(rs+1:end,rs+1:end) ...
    + TXYT(rs+1:end,1:rs)/(TXYT(1:rs,1:rs))*TXYT(1:rs,rs+1:end);
  Xmod = Tn' \TXT / Tn;
  % Xmod = RT(1:ns,1:ns)'*RT(1:ns,1:ns);
  Ymod = Y;
else
  Tn = [orth(NY1) null(NY1')];
  if ry1 <= ry2,
    Tn = [orth(NY1) null(NY1')];
    if verb, fprintf (' using NY1;'); end
  else
    Tn = [orth(NY2) null(NY2')];
    if verb, fprintf (' using NY2;'); end
  end

  TYT = Tn'*Y*Tn;
  TXT = Tn'*inv(X)*Tn;
  TYXT = TYT - TXT;
  TYT(rs+1:end,rs+1:end) = TXT(rs+1:end,rs+1:end) ...
    + TYXT(rs+1:end,1:rs)/(TYXT(1:rs,1:rs))*TYXT(1:rs,rs+1:end);
  Xmod = X;
  Ymod = Tn' \TYT / Tn;
end

if verb && rs < ns,
  fprintf (' controller reduced from order %d to %d\n', ns, rs);
else
	if verb, fprintf ('\n'); end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% [R, S, rs, s, T, Ti] =  recoverRS (X, Y, rs, debug);
%
% Finds P such that
%
% P = [X N; N' L] = inv ([Y M; M' *])
%
% In addition it finds the transformation matrix T, and Ti = inv (T),
% such that Ti' X Ti = T Y T' = diag (s)
%
% The argument rs can be used to force the size of P to (ns+rs)^2.
% If rs is not given as input argument recoverRS tries to find an
% rs < ns within certain tolerances.

function [R, S, rs, s, T, Ti] =  recoverRS (X, Y, rs, debug)

if nargin < 2,
  disp ('usage:  [R, S, rs, s, T, Ti] = recoverRS (X, Y, rs, debug)');
  return;
elseif nargin < 3,
  rs = [];
end

if nargin < 4,
  debug = 0;
end

ns = size (X, 1);
T  = eye(ns);
Ti = eye(ns);

if debug > 1,
  disp (sprintf ('cond X = %.3g, cond Y = %.3g', cond (X), cond (Y)));
end

[Ry, py] = chol (Y);  % Ry'*Ry = Y, inv(Y) = inv(Ry)*inv(Ry)'
[Rx, px] = chol (X);  % Rx'*Rx = X

if px && py,
  warning ('Cholesky decomposition of X and Y failed -- trying backup solution');
  [u, s] = svd (X - inv (Y));
  gam2 = diag (s);
  disp (sprintf ('%.3g  ', gam2));
  if isempty (rs), rs = sum (gam2 > 1e-5); end
  N = u(:,1:rs) * diag (sqrt (s(1:rs)));
  L = eye (rs);
  s = sqrt (eig(X*Y));
  R = [X N; N' L];
  S = eye (size(R));
elseif py,
  warning ('Cholesky decomposition of Y failed -- trying backup solution');
  [u, s] = svd (Rx*X*Rx' - eye (ns));
  gam2 = diag (s);
  if isempty (rs), rs = sum (gam2 > 1e-5); end
  N = Rx \ u(:,1:rs) * diag (sqrt (gam2(1:rs)));
  L = eye (rs);
  s = sqrt (gam2 + 1);
  S = [X -N; -N' L];
  R = eye (size(S));
elseif px,
  warning ('Cholesky decomposition of X failed -- trying backup solution');
  [u, s] = svd (Ry*X*Ry' - eye (ns));
  gam2 = diag (s);
  if isempty (rs), rs = sum (gam2 > 1e-5); end
  N = Ry \ u(:,1:rs) * diag (sqrt (gam2(1:rs)));
  L = eye (rs);
  s = sqrt (gam2 + 1);
  R = [X N; N' L];
  S = eye (size(R));
else
  % Cholesky decomposition of X and Y succeeded
  [u, s, v] = svd (Ry*Rx');
  s = diag (s);
  ssemi = diag (1./sqrt(s));
  T = ssemi*v'*Rx;
  Ti = Ry'*u*ssemi;
  % Ti'*X*Ti = T*Y*T' = Sig = diag (s), Ti = inv (T);
  % X = T'*diag(s)*T;  Y' = Ti*diag(s)*Ti', Ti = inv (T);

  sig  = sqrt ((s+1)/2);  % "sqrt" of Sig
  gam2 = (s-1)/2;
  if isempty (rs) || rs >= ns,
    rs = sum (gam2 > min (1e-2, 1e-5*max(gam2)));
  end
  Gamr = [diag(sqrt(max(0,gam2(1:rs)))); zeros(ns-rs,rs)];
  Sigr = diag (sig(1:rs));
  Sig = blkdiag (Sigr, eye(ns-rs));
  R = [Sig*T     Gamr; Gamr'*T     Sigr];
  S = [Sig*Ti'  -Gamr; -Gamr'*Ti'  Sigr];

  % P = S \ R
end

if debug > 1,
  try
    sz = get (0, 'CommandWindowSize');
    nw = floor (sz(1)/10);
  catch
    nw = 8;
  end
  gdisp = s - 1;
  ndisp = ceil (length(s) / ceil (length(gdisp)/nw));
  fprintf ('sqrt (eig (X*Y)) - 1 =\n');
  while length (gdisp) > ndisp,
    fprintf ('%9.2g ', gdisp(1:ndisp));
    fprintf ('\n');
    gdisp = gdisp(ndisp+1:end);
  end
  fprintf ('%9.2g ', gdisp);
  fprintf ('\n');

  fprintf ('The controller has %i state', rs);
  if rs ~= 1, fprintf ('s'); end
  if rs <= ns - 1,
    fprintf(' (%i state', ns-rs);
    if rs < ns - 1, fprintf ('s'); end
    fprintf (' truncated)');
  end
  fprintf ('\n');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [R, S, rs] = recoverRS_X_truncated (X, Y, u1, v1, n2, NN, debug)
tol = 1e-8;
ns = size (X, 1);
if n2 == 0,
  oY = orth(NN);
  S = [eye(ns); -oY']*Y*[eye(ns) -oY];
  rs = size (S, 1) - ns;
  R = eye(ns+rs);
else
  Xn = u1' * X * u1;
  Yn = v1' * Y * v1;
  n1 = ns - n2;
  Y1 = Yn(1:n1,1:n1);
  X2 = Xn(n1+1:end,n1+1:end);
  Y2 = Yn(n1+1:end,n1+1:end) - Yn(n1+1:end,1:n1)/Y1*Yn(1:n1,n1+1:end);
  S1 = [eye(n1); -eye(n1)] * Y1 * [eye(n1); -eye(n1)]';
  rs1 = n1;
  R1 = blkdiag (eye(n1), eye(rs1));
  % fix for AC14 in order to reduce to r=39, X2 = 1/Y2
  [R2 , S2, rs2, s2, TT2, TT2i] =  recoverRS (X2, Y2, [], debug);
  S = blkdiag (S1, S2);
  R = blkdiag (R1, R2);
  ord = [1:n1, n1+rs1+(1:n2), n1+(1:rs1), n1+rs1+n2+(1:rs2)];
  % modify u1 with respect to the Schur complement used
  % to obtain Y2
  u2 = [eye(n1) Y1\Yn(1:n1,n1+1:end); zeros(n2,n1) eye(n2)]';
  v2 = [eye(n1) -Y1\Yn(1:n1,n1+1:end); zeros(n2,n1) eye(n2)];
  u12 = u1*u2;
  v12 = v1*v2;
  if norm(u12'*v12-eye(size(u12))) > 2e-13*size(u12,1),
    warning ('u12''*v12 ~= eye');
  end
  S = S(ord,ord) * blkdiag (u12', eye(n1+rs2));
  R = R(ord,ord) * blkdiag (v12', eye(n1+rs2));
  rs = rs1 + rs2;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [R, S, rs] = recoverRS_XY_truncated (X, Y, u1, v1, nn, debug)

ns = size (X, 1);
Xn = u1' * X * u1;
Yn = v1' * Y * v1;
n1 = ns - nn;

n1 = 2; % fix

Y1 = Yn(1:n1,1:n1);
X1 = Xn(1:n1,1:n1);
X2 = Xn(n1+1:end,n1+1:end);
Y2 = Yn(n1+1:end,n1+1:end);

R1 = [eye(n1); eye(n1)] * [eye(n1); eye(n1)]';
S1 = [eye(n1); -eye(n1)] * [eye(n1); -eye(n1)]';

n2 = 2;
R2 = [eye(n2); eye(n2)] * [eye(n2); eye(n2)]';
S2 = blkdiag (Y2, Y2);
rs2 = n2;

rs1 = n1;
% [R3 , S3, rs3] =  recoverRS (X3, Y3, [], debug);
S = blkdiag (S1, S2);
R = blkdiag (R1, R2);
ord = [1:n1, 2*n1+(1:nn), n1+(1:rs1), n1+rs1+nn+(1:rs2)];
% modify u1 with respect to the Schur complement used
% to obtain Y2
u2 = [eye(n1)  Y1\Yn(1:n1,n1+1:end); zeros(nn,n1) eye(nn)]';
v2 = [eye(n1) -Y1\Yn(1:n1,n1+1:end); zeros(nn,n1) eye(nn)]';
u12 = u1*u2;

v12 = v1*v2;
if norm(u12'*v12-eye(size(u12,2))) > 2e-13*size(u12,2),
  warning ('u12''*v12 ~= eye');
  norm(u12'*v12-eye(size(u12,2)))
end
S = S(ord,ord) * blkdiag (u12', eye(n1+rs2));
R = R(ord,ord) * blkdiag (v12', eye(n1+rs2));
rs = n1 + rs2;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [Y, T] = schurcompl (X, indx1, indx2);
n1 = length (indx1);
n2 = length (indx2);
if (n1+n2)*[1 1]  ~= size(X), error ('X and indx not compatible'); end
indx = [indx1(:); indx2(:)];
X11 = X(indx1,indx1);
X12 = X(indx1,indx2);
X21 = X(indx2,indx1);
X22 = X(indx2,indx2);
T = eye(n1+n2);
T(indx2,indx1) = -X22\X21;
% Y = T' * X * T
Y(indx,indx) = blkdiag (X11 - X12/X22*X21, X22);
