function [x,zmin,xuyvz] = quad_prog(G,c,A,b)
% usage: [xopt,zmin,xuyvz] = 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 complementaries x,u,y,v
T = [-(G+G'),A',eye(n),zeros(n,m),c;A, zeros(m,mpn),-eye(m),b]
% determine basic variables
basicrows = [zeros(1,n),zeros(1,m),(c'>=0),(b'<=0)]
% make rhs nonnegative
T = diag([(c==0)+sign(c);(b==0)+sign(b)])*T;
% insert the artificial variables
if ((nav = sum(basicrows==1))< mpn)
  Id = eye(mpn);
  T = [T(:,1:2*mpn),Id(:,find(diag(T(1:mpn,mpnp1:2*mpn))==-1)),T(:,2*mpn+1)]
  basicrows = [basicrows,ones(1,mpn-nav)]
end
nav = mpn-nav
nend = 2*mpn+nav
% append objective function equation for solving system
T,basicrows

T = [T;basicrows,0]
basicrows(find(basicrows == 1))=1:mpn;
% in best of all possible worlds, we would balance,
% pivot, etc., but here's a cheap fix
tol = tol*norm(T,inf);
% 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(mpn+1:2*mpn),basicrows(1:mpn),zeros(1,nav)];
T(mpnp1,1:nend).*~complement;
  [val,colndx] = min(T(mpnp1,1:nend).*~complement);
  if (val >= -tol) % all done?
    xuyvz = zeros(nend,1); %yup
    tmpndx = find(basicrows ~= 0);
    xuyvz(tmpndx) = T(basicrows(tmpndx),nend+1);
    x = xuyvz(1:n);
    zmin =  x'*G*x -c'*x;
    % if solution exists, objective value should be 0
    if (abs(T(mpnp1,nend+1))>tol)
      disp('Warning: this problem may not have a solution.')
    end
    return;
  end
  x = T(basicrows(find(basicrows ~= 0)),nend+1);
  % find elegible row
  minndx = find(T(1:mpn,colndx)>=tol);
  [val,rowndx] = min(T(minndx,nend+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')
xuyvz = zeros(nend,1);
tmpndx = find(basicrows ~= 0);
xuyvz(tmpndx) = T(basicrows(tmpndx),nend+1);
x = xuyvz(1:n);
zmin =  x'*G*x -c'*x;