% gam = hinflmi (sys)
%
% [gam, P] = hinflmi (sys)
%
% [k, gam, X, Y] = hinflmi (sys, nmeas, ncont, gamma, par, X0, Y0)
% [k, gam, X, Y] = hinflmi (sys, nmeas, ncont, gamma, par, T0)
%
% This function can be used to determine the Linf/Hinf norm of a LTI
% system by calling it with only one argument. If called with three or
% more arguments it performs an Hinf synthesis of a system.
%
% Inputs
%   sys    an LTI model
%   nmeas  controller inputs
%   ncont  controller outputs
%   gamma  target gamma value
%          (optional, if provided it should be > optimal value)
%   par = [rs iter w]
%     rs     specifies the number of states in the controller (optional)
%     iter   number of iterations
%     w      weighting factor in minimizer: gamma + w trace (X + Y)
%   X0     X from previous iteration
%   Y0     Y from previous iteration
%   T0     Similarity transformation matrix from previous iteration
%
% Outputs
%   k      the LTI controller
%   gam    obtained gamma value
%   X, Y   LMI solutions
%
% If called with three argument, the optimal value is provided by the
% algorithm. If numerical problems occur, try to call hinflmi with a
% gamma value higher than the optimal one. By increasing the value of
% gamma it is also likely that you can find reduced-order controllers.
%
% If rs < 0, a two-step algorithm is used to find a reduced-order
% controller of order -rs.
%
% See also recoverP


function [k, gam, X, Y, T, Ti] = hinflmi (sys, nmeas, ncont, gamma, par, X0, Y0)

if nargin ~= 1 && nargin < 3,
  disp ('usage:  [k, gam, X, Y] = hinflmi (sys, nmeas, ncont, gamma, par, X0, Y0)');
  disp ('usage:  [gam, P] = hinflmi (sys)');
  return;
end


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

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

if nargin > 2,
  p2 = nmeas;
  m2 = ncont;

  if p2 < 1,
    error ('there must be at least one controller input');
  end

  if p2 > p, error ('too many controller inputs'); end

  if m2 < 1,
    error ('there must be at least one controller output');
  end

  if m2 > m, error ('too many controller outputs'); end

else
  p2 = 0;
  m2 = 0;
end

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


stab = 0;
if p1 == 0 || m1 == 0, stab = 1; end

istart = 1;
maxiter = 1;
w = 0;

if nargin < 5,
  rs = [];
  w = 0;
else
  if length (par) > 3, ep0 = par(4); else ep0 = []; end
  if length (par) > 2, w  = par(3); else w = 0; end
  if length (par) > 1,
    maxiter = par(2);
    rs = abs (par(1));
  else
    maxiter = 2;
    if isempty (par),
      rs = [];
    else
      rs = abs (par);
    end
  end

end % nargin < 5

if rs >= ns,
  disp (sprintf ('you must specify rs < ns = %d', ns));
  disp ('if you want a full-order controller, you shouldn''t specify rs');
  rs = ns - 1;
end

gamma0 = [];
if stab,
  gam = [];
elseif nargin > 3 && ~isempty (gamma),
  gamma0 = gamma;
  gam = gamma(end);
end



% adapted to yalmip 3.0
opts = sdpsettings ('solver',    'sedumi', ...
                    'verbose',         0, ...
                    'beeponproblem',  []);

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

if nargin == 1,
  
  
  yalmip ('clear');
  
  if ns > 0,
    P = sdpvar (ns, ns, 'symmetric');
  else
    P = [];
  end
  
  gam = sdpvar (1, 1);
  
  F = synset (P, gam, sys);
  if (ns*(ns+1) > (m+p)*(2*ns+m+p+1)) && exist ('kypd', 'file'),
    disp ('kypd solver');
    sol = optimize (F, gam, sdpsettings (opts, 'solver', 'kypd'));
  else
    disp ('standard solver');
    sol = optimize (F, gam, opts);
  end
  
  gam = value (gam);
  disp (sprintf ('gam = %.8g', gam));
  
  if sol.problem, disp ('Problems finding Hinf norm'); end
  P = value (P);
  [Pp , pp] = chol (P);
  if pp, disp ('system might be unstable'); end
  k = gam;
  gam = P;
  return;
  
else
  
  if nargin < 6,
  
  disp ('Find an initial solution')
  
  yalmip ('clear');
  
  if ~stab && (nargin < 4 || isempty (gamma)),
    gam = sdpvar (1, 1);
  end
  
  if ns > 0,
    X = sdpvar (ns, ns, 'symmetric');
    Y = sdpvar (ns, ns, 'symmetric');
    Is = eye(ns);
    F = [X Is; Is Y] >= 0;
  else
    X = []; Y = [];
    F = [];
  end
  
  
  if nargin < 4 || isempty (gamma) || ns > 0,
    F = F + synset (X, gam, sys, p2, m2);
    F = F + synset (Y, gam, transpose(sys), m2, p2);
  end
  
  
  if ns == 0,
    sol = optimize (F, gam, opts);
    gam = max (0.0, value (gam));
    disp (sprintf ('gam = %.8g', gam));
    X = [];  Y = [];
    T = [];  Ti = [];
    maxiter = 0;
  else
    if stab,   % minimize the trace of X and Y
      minimizer = trace (X + Y);
      sol = optimize (F, minimizer, opts);
      istart = 2;
    elseif nargin < 4 || isempty (gamma),
      % minimize with respect to gam
      sol = optimize (F, gam, opts);
      gam = max (0.0, value (gam));
      istart = 2;
    elseif nargin < 6,   % minimize the trace of X and Y
      minimizer = trace (X + Y);
      sol = optimize (F, minimizer, opts);
      gam = max (0.0, value (gam));
      istart = 2;
    end
  
    if nargin < 6 || isempty (gamma),
      X0 = value (X);  Y0 = value (Y);
    end
  
    X = X0;  Y = Y0;
  
    if ~stab,
      gam = max (0.0, value (gam));
      disp (sprintf ('iter 1 => gam = %.8g', gam));
      gam0 = gam;
    end
  
    if (ns > 0),
      disp (sprintf ('trace (X + Y) = %.4g', trace (X + Y)));
    end
  end
  
  
  else
    if nargin < 7,
      T0 = X0;
      X0 = [];
      Y0 = [];
    end
    if ~isempty (gamma), gam = gamma(end); end
    gam0 = gam;
    X = X0;
    Y = Y0;
  end
  if maxiter >= istart,
  
  gamma = [];
  
  if ~stab,
    if maxiter > 2,
      disp ('Trying to reduce gamma iteratively');
    elseif maxiter == 1,
      disp ('Trying to reduce gamma');
    end
    gami = zeros(maxiter,1);
    if (istart > 1), gami(1) = gam; end
    dzi = zeros(maxiter,1);
  end
  
  sys0 = sys;
  i = istart;
  if ns > 0,
    while i <= maxiter,
      if ~isempty (X) && ~isempty (Y),
        
        if ns > 0,
          if nargin < 6 && sol.problem,
            disp ('Warning: problems finding X-Y solution');
            disp ('You can try to call hinflmi again with increased gamma');
          end
          % find P such that P = [X N; N' L] = inv [Y M; M' *]
          % where M and N has rs columns.
          [P, s, T, Ti, rs] =  recoverP (X, Y, rs);
        else
          P = X;
          rs = 0;
        end
        
      else
        T = T0;
        Ti = inv (T0);
        s = [10*ones(rs,1); ones(ns-rs,1)]; % fake
      end
      
        sys = ss2ss (sys0, T);    % balance the original system
        
        yalmip ('clear');
        
        if ~stab && (nargin < 4 || isempty (gamma)),
          gam = sdpvar (1, 1);
        end
        
        if ns > 0,
          X = sdpvar (ns, ns, 'symmetric');
          Y = sdpvar (ns, ns, 'symmetric');
          Is = eye(ns);
          F = [X Is; Is Y] >= 0;
        else
          X = []; Y = [];
          F = [];
        end
        
        
        if nargin < 4 || isempty (gamma) || ns > 0,
          F = F + synset (X, gam, sys, p2, m2);
          F = F + synset (Y, gam, transpose(sys), m2, p2);
        end
        
        
        if ns > 0,
          trw = [zeros(1,rs) ones(1,ns-rs)];
          s2m1 = (s + 1).*(s - 1);
          cn = poly (-s2m1);
        end
        
        % cn(rs+2)/cn(rs+1) is used as a measure of the closeness to rank(XY-I) <= r
        for k = 1:ns,
          ck = poly (-s2m1([1:k-1 k+1:ns]));  % char poly without kth element
          % ck = deconv (cn, [1 s2m1(k)]);  % this is faster but not as accurate
        
          % diff (cn(rs+2)/cn(rs+1), xk)
          if rs > 0 && rs < ns-1,
            trw(k) = max (0, s(k)*(ck(rs+1)^2 - ck(rs)*ck(rs+2))/cn(rs+1)^2);
          else
            trw(k) = max (0, s(k)*(ck(rs+1)/cn(rs+1))^2);
          end
        
          % diff (cn(rs+2), xk) / cn(rs+1)
          % trw(k) = max (0, s(k)*ck(rs+1)/cn(rs+1));
        end
        if ns > 0,
          disp (sprintf ('gap: %9.3g ', cn(rs+2)/cn(rs+1)));
          
          traceXY = sum (trw*(diag(X) + diag(Y))) - 2.0 * sum (trw);
          if stab,
            minimizer = traceXY;
          elseif w < 0,
            F = [F, traceXY <= abs(w)];
            minimizer = gam;
          else
            minimizer = gam + w * traceXY;
          end
        end
        
      
        s0 = diag (s);
        sol = optimize (F, minimizer, opts);
      
        if sol.problem == 1,
          disp ('aborting -- keeping last solution'); 
          X = X0; Y = Y0; gam = gam0;
          if ~stab,
            gami = gami(1:i-1);
          end
          [P, s, T, Ti, rs] =  recoverP (X, Y, rs);
          break;
        end
      
        traceXY = value (traceXY);
        if stab,
          disp (sprintf ('iter %i => trace = %.6g', i, traceXY));
        else
          gam = value (gam);
          disp (sprintf ('iter %i => w = %.3g => gam = %.8g', i, w, gam));
        end
      
        X = value (X); Y = value (Y);
        
        [P , s, TT, TTi] =  recoverP (X, Y, rs, 1);
        G = diag (sqrt (max (0, s - 1)));
        
        W2 = triu (qr ([TT-TTi'; G*TT; G*TTi']));
        dzi (i) = norm (W2*diag (sqrt (trw)), 'fro');
        
        ep = [];
        if ~isempty (ep0),
          if mod (i, 2) == 0, ep = ep0; end;
        elseif i > 1 && dzi(i-1) > 0,
          alpha = dzi(i)/dzi(i-1);
          ep = min (1/dzi(i), 1/(1-alpha));
          disp (sprintf ('step = %.6g, traceXY = %.6g, alpha = %.6g, ep = %.6g', ...
                         dzi(i), traceXY, alpha, ep));
          if ep < 2 || mod (i, 3) ~= 2,
            ep = [];
          end
        else
          disp (sprintf ('step = %.6g, traceXY = %.6g', dzi(i), traceXY));
        end
        
        if i < maxiter && ~isempty (ep) && ep > 2 && w >= 0,
          if ~stab,
            gam0 = gam;      gami(i) = gam;
          end
          i = i + 1;
          
          X1 = X;
          Y1 = Y;
          gam1 = gam;
          [P, s, TT, TTi, rs] =  recoverP (X, Y, rs);
          TT = TT(rs+1:ns,:);
          TTi = TTi(:,rs+1:ns);
          % we should try to find an orthogonal matrix, U, that minimizes
          % || TT - [0 U] || and || TTi - [0 U]' ||
          [u, s, v] = svd (TT(:,rs+1:ns) + TTi(rs+1:ns,:)');
          ZU = [zeros(ns-rs,rs), u * v'];
          
          % extrapolate in TT and TTi
          TT  = TT  + ep*(TT  - ZU);
          TTi = TTi + ep*(TTi - ZU');
          % assure that TT*TTi becomes identity matrix
          [u, s, v] = svd (TT*TTi);
          si2 = diag (1 ./ sqrt (diag (s)));
          TT = si2*u'*TT;
          TTi = TTi*v*si2;
          
          disp (sprintf ('trying again with extrapolation using ep = %.3g', ep));
          
          yalmip ('clear');
          
          if ~stab && (nargin < 4 || isempty (gamma)),
            gam = sdpvar (1, 1);
          end
          
          if ns > 0,
            X = sdpvar (ns, ns, 'symmetric');
            Y = sdpvar (ns, ns, 'symmetric');
            Is = eye(ns);
            F = [X Is; Is Y] >= 0;
          else
            X = []; Y = [];
            F = [];
          end
          
          
          if nargin < 4 || isempty (gamma) || ns > 0,
            F = F + synset (X, gam, sys, p2, m2);
            F = F + synset (Y, gam, transpose(sys), m2, p2);
          end
          
          
          traceXY = trace (TTi'*X*TTi + TT*Y*TT');
          minimizer = gam + w * traceXY;
          sol = optimize (F, minimizer, opts);
          gam = value (gam);
          traceXY = value (traceXY);
          if stab,
            disp (sprintf ('iter %i => trace = %.6g', i, traceXY));
          else
            disp (sprintf ('iter %i => w = %.3g => gam = %.8g', i, w, gam));
          end
          if gam < gam1,
            X = value (X); Y = value (Y);
          else
            disp ('extrapolation failed; keeping old solution')
            X = X1; Y = Y1; gam = gam1;
          end
          
        end
      
        % convert back to original system
        X = T' * X * T;   Y = Ti * Y * Ti';
      
        if ~stab,
          gam0 = gam;      gami(i) = gam;
        end
      
      i = i + 1;
    end
  end
  sys = sys0;
  
  end
  
  if ns > 0,
    if nargin < 6 && sol.problem,
      disp ('Warning: problems finding X-Y solution');
      disp ('You can try to call hinflmi again with increased gamma');
    end
    % find P such that P = [X N; N' L] = inv [Y M; M' *]
    % where M and N has rs columns.
    [P, s, T, Ti, rs] =  recoverP (X, Y, rs);
  else
    P = X;
    rs = 0;
  end
  
  
  % find the inverse of P assuming it's positive definite; here we try to
  % use Cholesky decomposition in order to improve the precision and force
  % the inverse to become symmetric. If this fails, compute the inverse in
  % the traditional way.
  
  [Rp, pp] = chol (P);  % Rp'*Rp = P, inv(P) = inv(Rp)*inv(Rp)'
  if pp,
    Pi = inv (P);
  else
    Rpi = inv (Rp);
    Pi = Rpi*Rpi';
  end
  
  disp (sprintf ('||P(X)-X|| = %0.4g, ||Pi(Y)-Y|| = %0.4g', ...
                 norm (P(1:ns,1:ns)-X), norm (Pi(1:ns,1:ns)-Y)));
  
  
  a   = sys.a;
  b1  = sys.b(:,1:m1);
  b2  = sys.b(:,m1+1:m);
  c1  = sys.c(1:p1,:);
  c2  = sys.c(p1+1:p,:);
  d11 = sys.d(1:p1,1:m1);
  d12 = sys.d(1:p1,m1+1:m);
  d21 = sys.d(p1+1:p,1:m1);
  
  Im1 = eye(m1);
  Ip1 = eye(p1);
  
  
  x = P(1:ns,1:ns);
  n = P(1:ns,ns+(1:rs));
  
  Q = [x*a+a'*x  a'*n           x*b1       c1'
       n'*a      zeros(rs,rs)   n'*b1      zeros(rs,p1)
       b1'*x     b1'*n          -gam*Im1   d11'
       c1        zeros(p1,rs)   d11        -gam*Ip1
  ];
  
  U = [P*blkdiag(b2,eye(rs))
       zeros(m1,m2+rs)
       d12    zeros(p1,rs)
  ];
  
  V = [blkdiag(c2,eye(rs))  blkdiag(d21,zeros(rs,p1))];
  
  k = recoverK (Q, U, V);
  if isempty (k),
    disp ('trying LMI-solver for finding K');
    k = recoverKlmi (Q, U, V, opts);
  end
  
  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));
  
  
  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-7)*gam,
    disp (sprintf ('Warning: Hinf norm (%.6g) higher than target (%.6g)', ...
                   hinfnorm, gam));
  end
  
  if max (real (pole (cl))) > 0,
    disp ('Warning: closed loop system is unstable');
  end
  
  
end

if exist ('gami', 'var'), gam = [gamma0(:); gami(:)]; end;

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


function F = synset (X, gam, sys, p2, m2)

h = 0;

if nargin < 4,
  p2 = 0;
  m2 = 0;
end

[p, m] = size (sys);
p1 = p - p2;
m1 = m - m2;
a   = sys.a;
b1  = sys.b(:,1:m1);
c1  = sys.c(1:p1,:);
d11 = sys.d(1:p1,1:m1);
c2  = sys.c(p1+1:p,:);
d21 = sys.d(p1+1:p,1:m1);

ns = size (a, 1);
Im1 = eye(m1);
Ip1 = eye(p1);

% [p1, m1] = size (d11);
Q = [X*a+a'*X  X*b1      c1';
     b1'*X     -gam*Im1  d11';
     c1        d11       -gam*Ip1];

if h > 0,
  ab0 = [a b1 zeros(n,p1)];
  Q = Q + h * ab0' * X * ab0;
end

if h <= 0 && nargin < 4,
  M = [zeros(ns,ns) zeros(ns,m1)  c1';
       zeros(m1,ns) -gam*Im1      d11';
       c1           d11       -gam*Ip1];
  F = kyp (a, [b1 zeros(ns,p1)], X, M) <= 0;
else
  NX = null ([c2 d21]);
  NXI = blkdiag (NX, Ip1);
  F =  NXI' * Q * NXI <= 0;
end



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

yalmip ('clear');

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

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

sol = optimize (F, gap, opts);

if sol.problem,
  disp ('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
T1 = null ([U  V']');
T2 = null ([V' T1]');
T3 = null ([U  T1]');
T4 = null ([T1 T2 T3]');
if size (T4, 2) > 0,
  disp ('Warning: T4 case in recoverK not fully tested');
end

if size(T1,2) > 0,
  Q1 = Q*T1;
  Q11 = T1'*Q1;
  Q = Q - Q1/Q11*Q1';
end
K23 = - T2'*Q*T3;
K24 = - T2'*Q*T4;
K43 = - T4'*Q*T3;
K44 = - 0.5 * T4'*Q*T4;

K = ([T2 T4]'*U)\[K23 K24; K43 K44]/(V*[T3 T4]);


