function [x,zmin,xyuvz] = quad_prog(G,c,A,b)
% usage: [xopt,zmin,xyuvz] = quad_prog(G,c,A,b)
% description: This code solves the quadratic program
%                 Min W = x'*G*x - c'*x
%                 Sbj
%                         A*x >= b, x >= 0.
% It assumes that A is mxn and that G is nxn,
% c is nx1 and b is mx1. This program calculates
% an approximate solution using a modified
% simplex method applied to the KKT conditions.
% Some theoretical conditions that ensure its validity
% are that G is positive semi-definite and either c is
% zero or the objective function is strictly convex,
% e.g., G is symmetric positive definite.

% First step is to convert it into a maximization problem:
%                Max Z = c'*x - x'*G*x
%                Sbj
%                    -A*x <= -b, x >= 0.
% The KKT conditions for this problem are computed to be
%              -(G+G')*x + A'*u + y = c
%                           A*x - v = b
%                       x'*y + u'*v = 0
%                       x, y, u, v >= 0.
% This system is solved by a modified simplex method that
% preserves the (non-linear) complementarity equation. No
% attempt to account for multiple solutions is made.

% programmer: t. shores      last rev: 2/24/07

% set some constants ... adjust these if tuning needed
tol = 1e2*eps; % approximate zero
maxiter = 100;
% some minimal bullet-proofing
[m,n] = size(A);
mpn = m+n;
mpnp1 = m+n+1;
if ( nargin < 4)
  error('quad_prog: arguments G,d,A,b are required')
end
if (size(b) ~= [m,1])
  error('quad_prog: b must be mx1, where A is mxn');
end
if (size(G) ~= [n,n])
  error('quad_prog: G must be nxn, where A is mxn');
end
if (size(c) ~= [n,1])
  error('quad_prog: d must be nx1, where A is mxn');
end
% set up the tableau with adjacent complementaries x,y,u,v
T = [-(G+G'),eye(n),A',zeros(n,m),c;A, zeros(m,mpn),-eye(m),b];
% make rhs nonnegative
T = diag([(c==0)+sign(c);(b==0)+sign(b)])*T;
% insert the artificial variables
T = [T(:,1:2*mpn),eye(mpn),T(:,2*mpn+1)];
% in best of all possible worlds, we would balance,
% pivot, etc., but here's a cheap fix
tol = tol*norm(T,inf);
% append objective function equation for solving system
T = [T;zeros(1,2*mpn),ones(1,mpn),0];
% identify basic variables and row of occurrence
basicrows = [zeros(1,2*mpn),1:mpn];
% initialize artificial variables to basic
T(mpn+1,:) = T(mpnp1,:)-sum(T(1:mpn,:));
% main loop
for ii = 1:maxiter
  % find elegible column
  complement = [basicrows(n+1:2*n),basicrows(1:n),basicrows(n+mpnp1:2*mpn),...
                basicrows(2*n+1:n+mpn),zeros(1,mpn)];
  [val,colndx] = min(T(mpnp1,1:3*mpn).*~complement);
  if (val >= -tol) % all done?
    xyuvz = zeros(3*mpn,1); % yup
    tmpndx = find(basicrows ~= 0);
    xyuvz(tmpndx) = T(basicrows(tmpndx),3*mpn+1);
    x = xyuvz(1:n);
    zmin = x'*G*x - c'*x;
    % if solution exists, objective value should be 0
    if (abs(T(mpnp1,3*mpn+1))>tol)
      disp('Warning: this problem may not have a solution.')
    end
    return;
  end
  x = T(basicrows(find(basicrows ~= 0)),3*mpn+1);
  % find elegible row
  minndx = find(T(1:mpn,colndx)>=tol);
  [val,rowndx] = min(T(minndx,3*mpn+1)./T(minndx,colndx));
  if (rowndx == []) % Houston, we have a problem ...
    error('quad_prog: program error...unbounded nonnegative minimum.');
  end
  % do Gaussian elimination step without loops
  rowndx = minndx(rowndx);
  tmprow = T(rowndx,:)/T(rowndx,colndx);
  T = T - T(:,colndx)*tmprow;
  T(rowndx,:) = tmprow;
  % do the updates
  basicrows(find(basicrows == rowndx)) = 0;
  basicrows(colndx) = rowndx;
end
disp('Iteration limit was exceeded')
xyuvz = zeros(3*mpn,1);
tmpndx = find(basicrows ~= 0);
xyuvz(tmpndx) = T(basicrows(tmpndx),3*mpn+1);
x = xyuvz(1:n);
zmin =  x'*G*x -c'*x;