

% [P, s, T, Ti, rs] =  recoverP (X, Y, rs, silent);
%
% 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 recoverP tries to find an
% rs < ns within certain tolerances.


function [P, s, T, Ti, rs] =  recoverP (X, Y, rs, silent)

if nargin < 2,
  disp ('usage:  [P, s, T, Ti, rs] = recoverP (X, Y, rs, silent)');
  return;
elseif nargin < 3,
  rs = [];
end

if nargin < 4,
  silent = 0;
end

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

if ~silent,
  disp (sprintf ('cond X = %.3g, cond Y = %.3g', cond (X), cond (Y)));
end

[Ry, pp] = chol (Y);  % Ry'*Ry = Y, inv(Y) = inv(Ry)*inv(Ry)'

if pp,
  
  disp ('Cholesky decomposition of Y failed -- trying backup solution');
  [u, s, v] = 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));
  
else
  [Rx, pp] = chol (X);  % Rx'*Rx = X
  if pp,
    
    disp ('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);
    
  else
    
    % disp ('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' = diag (s), Ti = inv (T);
    gam2 = (s + 1.0) .* (s - 1.0);
    if isempty (rs), rs = sum (gam2 > min (1e-2, 1e-5*max(gam2))); end
    Gamr = [diag(sqrt(gam2(1:rs))); zeros(ns-rs,rs)];
    L = diag (s(1:rs));
    X = T' * blkdiag (L, eye(ns-rs)) * T;
    N = T' * Gamr;
    
  end
  if ~silent,
    
    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));
    disp ('sqrt (eig (X*Y)) - 1 =');
    while length (gdisp) > ndisp,
      disp (sprintf ('%9.2g ', gdisp(1:ndisp)));
      gdisp = gdisp(ndisp+1:end);
    end
    disp (sprintf ('%9.2g ', gdisp));
    
  end
end


if ~silent,
  text = sprintf ('The controller has %i state', rs);
  if rs ~= 1, text = [text 's']; end
  if rs == ns - 1,
    text = [text sprintf(' (%i state truncated)',ns-rs)];
  elseif rs < ns - 1,
    text = [text sprintf(' (%i states truncated)',ns-rs)];
  end
  disp (text)
end


P = [X N; N' L];

