% [t, xi, dxi] = hamint (plant, time, X0);
%
% plant is a function handle to
%
%   [p, dim, gam] = plant (t);
%
% If time is negative, FY is computed from abs(time) downto 0,
% otherwise FX is computed from 0 to time.
% if time a vector it defines the integration step, for instance
% 0:dt:tend or tend:-dt:0
%
% Version 2015-06-11

function [t, xi, dxi] = hamint (plant, time, X0);

if nargin < 1
  disp ('usage:  [t, xi, dxi] = hamint (plant, time, X0)');
  return;
end

if isa (plant, 'function_handle'),
  par.plant = plant;
else
  error ('plant must be a function handle');
end

opts = odeset ();
opts = odeset ('RelTol', 1e-4, 'AbsTol', 1e-7);

if length (time) == 1,
  if time > 0,
    time = linspace (0, time, 1001);
  else
    time = linspace (-time, 0, 1001);
  end
end
[p, dim, gam] = plant (time(1));

par.type = 1 + (time(1) > time(end));

n = size (p.a, 1);

if nargin < 3,
  % find LTI solution at t = time(1)
  H2 = kcfham (p, dim, gam, par.type);
  [u, lam] = eig (H2);
  if par.type == 1,
    xi0 = u(:, find (real (diag (lam)) > 0));
  else
    xi0 = u(:, find (real (diag (lam)) < 0));
  end
else
  if size (X0,1) == 2*n,
    if size (X0, 2) > n,
      error ('X0 is incompatible with plant');
    end
    xi0 = X0;	  
  elseif all (size (X0) == n),
    xi0 = [eye(n); X0];
  else
    error ('X0 is incompatible with plant');
  end
end
% normalize xi0
xi0 = orth (xi0);

y0 = reshape (xi0, prod (size(xi0)), 1);

% [t, y] = ode15s (@f, time, y0, opts, par);
[t, y] = ode23t (@f, time, y0, opts, par);

xi = permute (reshape (y, length(t), 2*n, size(y,2)/(2*n)), [2 3 1]);

if nargout > 2,
  dxi = zeros (size (xi));
  for i = 1:length(t),
    dxi(:,:,i) = reshape (f (t(i), y(i,:), par), size(xi,1), size(xi,2));
  end
end

if t(end) < t(1),
  xi = xi(:,:,end:-1:1);
  if nargout > 2,
    dxi = dxi(:,:,end:-1:1);
  end
  t = t(end:-1:1);
end

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

function dy = f (t, y, par)

type = par.type;
[p, dim, gam] = par.plant (t);
H  = kcfham (p, dim, gam, type);
n  = size (H, 1);
xi = reshape (y, n, length(y)/n);
dy  = reshape ((eye(n)-xi/(xi'*xi)*xi')*H*xi, length(y), 1);
% dy  = reshape ((eye(n)-L*xi/(xi'*L'*L*xi)*xi'*L')*H*xi, length(y), 1);

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

% [H, L, lam, xi] = kcfham (sys, dim, gam, type)
% 
% computes the eigenvalues of the Hamiltonian corresponding to the
% H-infinity synthesis problem using guptri
%
% H = [ A    B1    0    0     0;
%       C1   D11   0  -gam    0;
%       C2   D21   0    0     0;
%       0    0    -A'  -C1'  -C2';
%       0    gam  -B1' -D11' -D21']
%
% and L = blkdiag (I, 0, I, 0);
%
% This is translated to the generalized eigenvalue problem: H - lam*L
% The KCF structure should be symmetric: J should have symmetry about
% real and imaginary axes and L' and L come in pairs.
%
% The sign of type choses sign the eigenvalues to select xi

function [H, L] = kcfham (sys, dim, gam, type)

if nargin < 3,
  disp ('usage:  [H, L] = kcfham (sys, dim, gam, type)');
  return;
end

if nargin < 4,
  type = 1;
end

if abs (type) == 2,
  sys = sys.';
  dim = dim([2 1]);
end

[p, m] = size (sys);
n = size (sys.a, 1);
p2 = dim(1);
m2 = dim(2);
m1 = m - m2;
p1 = p - p2;

Q = 0*eye(n+m1) + blkdiag (0*eye(n), -gam*eye(m1));
R = blkdiag (0*eye(n), -gam*eye(p1), 0*eye(p2));
U = [sys.a sys.b(:,1:m1); sys.c sys.d(:,1:m1)]';
V = blkdiag (eye(n), zeros(p,m1))';
H = [U' R; -Q, -U];
if abs (type) == 2,
  H = -H;
end
L = blkdiag (V', V);

if nargout < 2,
  i1 = find (sum (L, 2) == 1);
  i2 = find (sum (L, 2) == 0);
  j1 = find (sum (L, 1) == 1);
  j2 = find (sum (L, 1) == 0);

  % H = H(i1,j1) - H(i1,j2) * pinv (H(i2,j2)) * H(i2,j1);
  H = H(i1,j1) - H(i1,j2) / H(i2,j2) * H(i2,j1);
end
