function [x,zmin,xuyvz] = maxquad_prog(G,c,A,b)
% usage: [xopt,zmin,xyuvz] = maxquad_prog(G,c,A,b)
%                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.
%% 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.

% programmer: t. shores      last rev: 2/25/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,mpn),(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))< mpn)
  ndx = find(basicrows(1+mpn:2*mpn)==0)+mpn;
  T = [T(:,1:2*mpn),abs(T(:,ndx)),T(:,2*mpn+1)]
  basicrows = [basicrows,ones(1,mpn-nav)]
end
% define basic row numbers
basiccols = find(basicrows~=0);
for ii = basiccols
  basicrows(ii) = find(T(:,ii)~=0);
end
nav = mpn-nav;
nend = 2*mpn+nav;
% append objective function equation for solving system
T = [T;zeros(1,2*mpn),ones(1,nav),0]
% 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(mpnp1,:) = T(mpnp1,:) - sum(T(basicrows(nend-nav+1:nend),:),1)
% main loop
for ii = 1:maxiter
  % find elegible column
  complement = [basicrows(mpn+1:2*mpn),basicrows(1:mpn),zeros(1,nav)];
  [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 = c'*x - x'*G*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
T%degub
basicrows
    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 = c'*x - x'*G*x;