function [X,lambda]=qp_lcp(H,f,A,b,vlb,vub,X,neqcstr,verbosity)
%QP	Quadratic programming (via PATH)
%	X=QP(H,f,A,b) solves the quadratic programming problem:
%
%            min 0.5*x'Hx + f'x   subject to:  Ax <= b 
%             x    
%
%       [x,LAMBDA]=QP(H,f,A,b) returns the set of Lagrangian multipliers,
%       LAMBDA, at the solution.
%
%       X=QP(H,f,A,b,VLB,VUB) defines a set of lower and upper
%       bounds on the design variables, X, so that the solution  
%       is always in the range VLB < X < VUB.
%
%       X=QP(H,f,A,b,VLB,VUB,X0) sets the initial starting point to X0.
%
%       X=QP(H,f,A,b,VLB,VUB,X0,N) indicates that the first N constraints 
%       defined by A and b are equality constraints.

%This beginning part puts the input into a standard form
% Handle missing arguments
if nargin< 9, verbosity = []; 
  if nargin< 8, neqcstr=[]; 
    if nargin < 7, X=[]; 
      if nargin<6, vub=[]; 
	if nargin<5, vlb=[];
	end, end, end, end, end

nvars = length(f);
if isempty(A)
  rowsa = 0;
  rowsb = 0;
  neqcstr = 0;
else
  if ~issparse(A)
    A = sparse(A);
  end
  [rowsa,colsa] = size(A);
  if (colsa ~= nvars)
    error('Input arguments f and A of incompatible sizes');
  end
  if ~length(neqcstr), neqcstr = 0; end
  [rowsb,colsb] = size(b);
  if (rowsa ~= rowsb)
    error('Input arguments A and b of incompatible sizes');
  end
  if (neqcstr > rowsb)
    error('Too many equality constraints');
  end
  if (colsb ~= 1) 
    error('Only allowed one right hand side');
  end
end

if isempty(H)
  H=sparse(nvars,nvars); 
else
  if ~issparse(H)
    H = sparse(H);
  end
end

if ~length(verbosity), verbosity = 0; end
if ~length(vlb), vlb=-1e20*ones(nvars,1); end
if ~length(vub), vub=1e20*ones(nvars,1); end
vlb = max(vlb,-1e20*ones(nvars,1));
vub = min(vub,1e20*ones(nvars,1));
if ~length(X)
  X=min(max(zeros(nvars,1),vlb),vub); 
else
  X=min(max(X,vlb),vub); 
end

% make sure f and b are column vectors
f=f(:);
b=b(:);

%set up the new bounds
if ~isempty(find(vub < vlb))
  error('bounds incorrect');
end
notfixed = find(vlb < vub);

fixed = [1:nvars]'; fixed(notfixed) = [];
if ~isempty(fixed)
  b = b - A(:,fixed)*vlb(fixed);
  f = f + H(:,fixed)*vlb(fixed);
end
fRowVars = 0;
remove = [];
equalities = [1:neqcstr]';
inequalities = [neqcstr+1:rowsa]';

updated = 1;
while (updated > 0) 
  updated = 0;
  % first deal with equalities
  req = [];
  for i=equalities',
    nnzAi = nnz(A(i,notfixed));
    if (nnzAi == 0) 
      if (abs(b(i)) < 1e-6) 
	req = [req,i];
      else
	error('Problem is infeasible');
      end
    elseif (nnzAi == 1)
      [row,notfcol,val] = find(A(i,notfixed));
      col = notfixed(notfcol);
      bnd = b(i)/val;
      if (vlb(col) <= bnd & bnd <= vub(col))
	vlb(col) = bnd;
	vub(col) = bnd;
	b = b - A(:,col)*bnd;
	f = f + H(:,col)*bnd;
	fixed = [fixed,col];
	req = [req,i];
	fRowVars = fRowVars + 1;
	notfixed(notfcol) = [];
	updated = 1;
      else
	error('Problem is infeasible');
      end
    end
  end
  equalities = setdiff(equalities,req);
  remove = [remove,req];
    
  if ~isempty(inequalities) 
    % now deal with inequalities
    req = [];
    for i=inequalities',
      nnzAi = nnz(A(i,notfixed));
      if (nnzAi == 0) 
	if (b(i) >= 0) 
	  req = [req,i];
	else
	  error('Problem is infeasible');
	end
      elseif (nnzAi == 1)
	[row,notfcol,val] = find(A(i,notfixed));
	col = notfixed(notfcol);
	bnd = b(i)/val;
	if (vlb(col) <= bnd & bnd <= vub(col))
	  if (val > 0) 
	    vub(col) = bnd;
	  else
	    vlb(col) = bnd;
	  end
	  if (vlb(col) == vub(col))
	    b = b - A(:,col)*bnd;
	    f = f + H(:,col)*bnd;
	    fixed = [fixed,col];
	    req = [req,i];
	    fRowVars = fRowVars + 1;
	    notfixed(notfcol) = [];
	    updated = 1;
	  end
	  req = [req,i];
	else
	  error('Problem is infeasible');
	end
      end
    end
    inequalities = setdiff(inequalities,req);
    remove = [remove,req];
  end
      
end
    
neqcstr = neqcstr - fRowVars;
rowsa = rowsa - length(remove);

l = [full(vlb(notfixed));-1.0e20*ones(rowsa,1)];
u = [full(vub(notfixed));1.0e20*ones(neqcstr,1);zeros(rowsa-neqcstr,1)];
nvars = length(notfixed);
remain = [1:rowsb]'; 
if (isempty(remain))
  smallA = [];
  b = [];
else
  remain(remove) = [];
  smallA = A(remain,notfixed);
  b = b(remain);
end

%if (neqcstr > 0) 
%  [ai,aj,av] = find(smallA(1:neqcstr,:));
%  theMat = sparse(aj,ai,av,nvars,nvars);
%  [L,U,P] = lu(theMat);
%  [ip,ipinv,throw] = find(P);
%  ipinv(ip) = 1:nvars;
%  norm(theMat(ipinv,:)-L*U,1),
%  rank = find(abs(diag(U)) > 1e-10),
%  b = b(remain);
%end

%g = zeros(nvars,1);
%for i=1:nvars
%  if (X(notfixed(i)) == vlb(notfixed(i)))
%    g(i) = 1.0;
%  elseif (X(notfixed(i)) == vub(notfixed(i)))
%    g(i) = -1.0;
%  end
%end
%lambda = leastsq(smallA',H(notfixed,notfixed)*X(notfixed)+f(notfixed)-g);
lambda = ones(rowsa,1); 

pathX = [full(X(notfixed));lambda];
%Set up F to be the Jacobian of the function path solves
%The function is in the form Fx + f
F = [H(notfixed,notfixed) -smallA';smallA sparse(rowsa,rowsa)];
smallf = full([f(notfixed);-b]);

[m,n] = size(F);
[rows,cols] = size(smallf);
if ((m~=rows)|cols ~=1)
    error('Input arguments are of incompatible sizes');
end
if(m~=n)	
    error('Input arguments are of incompatible sizes');
end

nnzJ = nnz(F);

clear lcppath
[status,ttime]  = lcppath(n, nnzJ, pathX, l, u, F, smallf);
if (status~=1) 
    error('Failure to solve QP');
end

%now put the results in the returned form
X = vlb;
X(notfixed) = pathX(1:nvars);
lambda = zeros(rowsb,1);
if ~isempty(remain)
  lambda(remain) = pathX(nvars+1:m);
end
toinspect = 1:length(vlb);
for i=remove,
  [row,col,val]=find(A(i,toinspect));
  if ~isempty(col)
    col = toinspect(col);
    if ~isempty(remain)
      lambda(i) = (f(col)+H(col,notfixed)*X(notfixed)-A(remain,col)'*lambda(remain))/val;
    else
      lambda(i) = (f(col)+H(col,notfixed)*X(notfixed))/val;
    end
    toinspect = setdiff(toinspect,col);
    remain = [remain;i];
  end   
end
  
return;
