function [val,primals,duals,eduals] = iqph(H,c,A,b,E,d,params)

% [val,primals,duals,eduals] = iqph(H,c,A,b,E,d,params)
%
% A simple interior point algorithm for solving the quadratic program
%   maximize  - x'Hx/2 + c'x  st  Ax+b >= 0, Ex + d = 0
% when H is positive semidefinite.  Use H=[] for a linear program.
% E may be [], but A must be nonempty (otherwise it's a linear
% regression not a linear program).
%
% The dual to this program is
%   minimize  x'Hx/2 + b'y - d'z  st  y >= 0, A'y - Hx - E'z + c = 0
% We solve both programs at the same time, and return x, y, and z
% in primals, duals, and eduals respectively.  The objective value
% for the primal and dual programs is the same, and is returned in
% val.
%
% Detection of infeasibility/unboundedness is not very good: we just
% return NaN if it appears that the optimal solution to either the
% primal or the dual is very large.
% General robustness could also be improved.
%
% If last arg params = [maxiter,canreorder,verbose,epsilon] is provided, it
% sets a few parameters.

% Copyright 1998 Geoff Gordon

% a few parameters
epsilon = 1e-8;    % accuracy requirement for solution
toobig = 1e+8;     % give up if solution gets too large
maxiter = 75;      % stop after this many iterations
canreorder = 1;    % set to zero to prevent reordering
verbose = 1;	   % set to zero to prevent messages

if (nargin == 7 & ~isempty(params))
  maxiter = params(1);
  canreorder = params(2);
  verbose = params(3);
  epsilon = params(4);
end

[m,n] = size(A);
[k,j] = size(E);
if (k == 0) 
  E = zeros(k,n);
  d = zeros(0,1);
end
if (m == 0)
  error('Must have at least one inequality constraint');
end

% default args
if (isempty(H)) H = (epsilon * .01) * speye(n); end
if (length(H)==1) H = H * speye(n); end
if (size(H,2)==1) H = spdiags(H,0,n,n); end

% symmetric part of H is all that matters
H = (H + H') / 2;

He=[];
if (isempty(He)) He = (epsilon * .01) * eye(k); end

% improve conditioning by adding a penalty of .5(Ex+d)^2
% (which will reduce to zero in a feasible solution)
%if (~isempty(E))
%  H = H + E' * E;
%  c = c - E' * d;
%end

% reorder for sparsity
if (canreorder & issparse(A) & issparse(H))
  order = symmmd(H + A' * A);
  H = H(order,order);
  A = A(:,order);
  E = E(:,order);
  c = c(order);
else
  order = [];
end

% simple stupid initialization
primals = zeros(n,1);
duals = 100* ones(m,1);
slacks = 100* ones(m,1);
eduals = zeros(k,1);
complementarity = (slacks' * duals) / m;

%primals = [A; E] \ [slacks-b;-d];

% do until complementarity is small
iter = 0; feasresid = 1;
while ((complementarity + feasresid > epsilon) & (iter < maxiter))

  % compute and factor newton matrix
  dg = spdiags(duals ./ slacks, 0, m, m);
  nmat = (H + (A' * dg * A));
  nmat = [nmat E'; E -He];
  [nl, nd] = ldl(nmat);

  % compute affine scaling (predictor) step
  [dprimals, dduals, dslacks, deduals, steplen] = ...
  newtsys(nl, nd, 0, 0, c, A, b, E, d, H, He, primals, duals, slacks, eduals);

  % use Mehrotra's heuristic to pick a target point on the central path
  comp = ((slacks + steplen * dslacks)' * (duals + steplen * dduals)) / m;
  barrier = (comp/complementarity)^2 * comp;

  % compute corrected step
  ho = dslacks .* dduals;
  [dprimals, dduals, dslacks, deduals, steplen] = ...
    newtsys(nl, nd, barrier, ho, c, A, b, E, d, H, He, ...
    primals, duals, slacks, eduals);

  % do the update
  primals = primals + steplen * dprimals;
  slacks = slacks + steplen * dslacks;
  duals = duals + steplen * dduals;
  eduals = eduals + steplen * deduals;
  complementarity = (slacks' * duals) / m;
  feasresid = sum(abs(A * primals + b - slacks)) / m;
  if (k > 0)
    feasresid = feasresid + sum(abs(E * primals + d - He*eduals)) / k;
  end
  iter = iter + 1;

  % give up if numbers get too big
  if max(max(slacks),max(duals)) > toobig
    val = NaN;
    if (~isempty(order)) primals(order) = primals; end
    if (verbose)
      disp('Warning: program appears to be ill-posed');
    end
    return;
  end

  % report progress
  if (verbose)
    fprintf('iter %d, step %g, target %g, actual %g, feas %g\n', ...
      iter, steplen, barrier, complementarity, feasresid);
  end

end

val = primals' * c - primals' * H * primals / 2;
if (~isempty(order)) primals(order) = primals; end

return


% Solve the Newton system of equations
%  -D  A  0  dduals    = rhs
%   A' H  E' dprimals  = rhs
%   0  E  0  deduals   = rhs
% (where D is diagonal and positive) which is the inner loop of
% our interior point method.
% We assume we are given already a factorization of the reduced
% Newton matrix
%   H+A'DA   E'
%     E      0
% The reduced Newton matrix results from pivoting along D in
% the Newton system.

function [dprimals, dduals, dslacks, deduals, steplen] = ...
  newtsys(nl, nd, barrier, ho, c, A, b, E, d, H, He, ...
  primals, duals, slacks, eduals)

% a parameter
backoff = .99995;

m = length(duals);
n = length(primals);
k = length(eduals);

% compute primal step
rhs = c - H * primals + A' * ((barrier - ho) ./ slacks + duals - ...
  (duals ./ slacks) .* (A * primals + b));
sol = nl' \ ((nl \ [rhs-E'*eduals; -E*primals-d+He*eduals]) ./ nd);
dprimals = sol(1:n);
deduals = sol(n+1:n+k);
deduals = deduals(:);		%this prevents bombs when E==[]

% from primal step, compute dual and slack steps
dslacks = A * (primals + dprimals) + b - slacks;
dduals = (barrier - ho - duals .* dslacks) ./ slacks - duals;

% this is more accurate but much slower
%dduals = [spdiags(slacks,0,m,m); A'] \ ...
%  [(barrier - ho - duals .* dslacks) - duals .* slacks; ...
%   - c - A'*duals + E'*(eduals+deduals)];

% compute stepsize: longest possible w/o getting too close to zero
steplen = max(-dduals ./ duals);
steplen = max(steplen, max(-dslacks ./ slacks));
if (steplen <= 0) steplen = Inf; else steplen = 1 / steplen; end
steplen = min(1, .666 * steplen + (backoff - .666) * steplen^2);
%steplen = min(1, .5 * steplen);

return


% [l,d] = ldl(a)
%
% Decompose the matrix A as LDL', where L is lower triangular and D is
% diagonal (returned as a vector).  A must be symmetric quasidefinite.

function [l,d] = ldl(a)

n = min(size(a));

d = zeros(n,1);
l = zeros(n,n);

for i = 1:n
  if (i > 1)
    x = l(i:n,1:(i-1)) * (d(1:(i-1)) .* l(i,1:(i-1))');
  else
    x = 0;
  end
  x = a(i:n,i) - x;
  d(i) = x(1);
  l(i:n,i) = x / d(i);
end

return