% k = ltvsyn (plant, sol, t)
% computes an LTV controller at time t
% input arguments
%    plant is a function handle:  [p, dim, gam] = plant (t)
%    sol a structure containing the solutions obtained from hamint
%    for instance
%    [sol.t, sol.xi, sol.dxi] = hamint (@plant, T);
%    [sol.t, sol.yi, sol.dyi] = hamint (@plant, -T);
%
%    t: instant for which the controller is to be computed in [0,T]
%
% output argument
%    k the computed controller (at time t) in a state-space (ss) structure
%
% See also hamint
% Version 2015-06-11

function k = ltvsyn (plant, sol, t)

if nargin < 3,
  disp ('usage:  k = ltvsyn (plant, sol, t)');
  return;
end

% find i closest to t
[~, i] = min (abs (sol.t - t));
t = sol.t(i);

[p, dim, gam] = plant (t);
n = size (p.a, 1);

% find X and Y
X = sol.xi(n+1:end,:,i) / sol.xi(1:n,:,i);
X = (X + X')/2;
Y = sol.yi(n+1:end,:,i) / sol.yi(1:n,:,i);
Y = (Y + Y')/2;

% check eigenvalues of X*Y
if min (real (eig (X*Y))) < 1,
  warning ('min eig X*Y is less than 1: %f', min (real (eig(X*Y))));
end

% find dX and dY
dX = (sol.dxi(n+1:end,:,i) - X*sol.dxi(1:n,:,i)) / sol.xi(1:n,:,i);
dX = (dX + dX')/2;

dY = (sol.dyi(n+1:end,:,i) - Y*sol.dyi(1:n,:,i)) / sol.yi(1:n,:,i);
dY = (dY + dY')/2;

% build P and dP from X, Y, dX and dY

N = chol (X - inv(Y))';
dN = dnn (N, dX + Y\dY/Y);
P = [X N; N' eye(n)];
dP = [dX dN; dN' zeros(n,n)];

k = findk (p, dim, gam, P, dP);

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

function dN = dnn (N, dNN)
% dN = dnn (N, dNN)
% solves dNN = N*dN' + dN*N where N and dN are lower triangular matrices
% we assume that N = chol (NN)'; so that N*N' = NN;
if nargin < 2,
  disp ('usage:  dN = dnn (N, dNN)');
  return;
end

dNN = (dNN + dNN')/2;
n = size (N, 1);
dN = zeros (n, n);

for i = 1:n,
  dd = dNN - N*dN' - dN*N';
  dN(i,i) = 0.5*dd(i,i)/N(i,i);
  dd = dNN - N*dN' - dN*N';
  dN(i+1:n,i) = dd(i+1:n,i)/N(i,i);
end

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

function k = findk (sys, dim, gam, P, dP)

if nargin < 4,
  disp ('usage:  K = findk (sys, dim, gam, P, dP)');
  return;
end

[n, m] = size (sys.b);
p = size (sys.c, 1);

p2  = dim(1);
m2  = dim(2);
p1  = p - p2;  % p - nmeas
m1  = m - m2;  % m - ncont

a   = sys.a;
b1  = sys.b(:,1:m1);
b2  = sys.b(:,m1+1:end);
c1  = sys.c(1:p1,:);
c2  = sys.c(p1+1:end,:);
d11 = sys.d(1:p1,1:m1);
d12 = sys.d(1:p1,m1+1:end);
d21 = sys.d(p1+1:end,1:m1);
d22 = sys.d(p1+1:end,m1+1:end);

X = P(1:n,1:n);
N = P(1:n,n+1:end);
L = P(n+1:end,n+1:end);
r = size (P, 1) - n;

Q = [ X*a+a'*X  a'*N        X*b1          c1';
      N'*a      zeros(r,r)  N'*b1         zeros(r,p1);
      b1'*X     b1'*N       -gam*eye(m1)  d11';
      c1        zeros(p1,r)  d11          -gam*eye(p1) ];

if nargin >= 5,
  Q(1:n+r,1:n+r) = Q(1:n+r,1:n+r) + dP;
end

U = [ N            X*b2;
      L            N'*b2;
      zeros(m1,r)  zeros(m1,p2);
      zeros(p1,r)  d12 ];

V = [ zeros(r,n) eye(r,r)     zeros(r,m1) zeros(r,p1);
      c2         zeros(p2,r)  d21         zeros(p2,p1) ]';

NU = null (U');
NV = null (V');

gapU = max (real (eig (NU' * Q * NU)));
gapV = max (real (eig (NV' * Q * NV)));
if gapU > 1e-8,
  warning ('NU''*Q*NU is large: %e', gapU);
end
if gapV > 1e-8,
  warning ('NV''*Q*NV is large: %e', gapV);
end

try
  K = recoverKlmi (Q, U, V);
  gap = max (real (eig (Q + U*K*V' + V*K'*U')));
  if gap > max (1e-7, 1.2 * (max(0, gapU) + max (0, gapV))),
    warning ('Q + U*K*V''+ V*K''*U'' is large: %e', gap);
  end

  k = ss (K(1:r,1:r), K(1:r,r+1:end), ...
          K(r+1:end,1:r),     K(r+1:end,r+1:end));

  % modify for non-zero d22
  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
catch
  warning ('failed to find controller');
  k = [];
end

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

function K = recoverKlmi (Q, U, V)

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))];

opts = sdpsettings ('verbose',        0, ...
                    'beeponproblem',  []);

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

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

K = value (K);
