% function [x,la,flag] = eqp(G,c,A,b,opts)
%
% solves equality constrained quadratic programming (EQP) problem
%
%   min 1/2 x'*G*x + c'*x
%   s.t. A*x = b
% using the nullspace approach
% 
% opts.tol   tolerance for detecting vectors in the nullspace of A
%            (default 1e-8)
%
% See Nocedal and Wright section 16.2

% Fernando Guevara Vasquez
function [x,la,flag] = eqp(G,c,A,b,opts)

flag=0;
if (nargin<5) opts=[]; end;
% set default tolerance for nullspace of A any diagonal entry in the R of the
% QR facto of A smaller than this tolerance indicates the corresponding column
% in Q is in the nullspace of A.

if (~isfield(opts,'tol')) opts.tol=1e-8; end;

% get dimensions
n = size(G,1); m=size(A,1);

% sanity checks
if (m>=n) error('n must be > m'); end;
if (any(size(G) ~= [n,n])) error('G must be n x n'); end;
if (any(size(A) ~= [m,n])) error('A must be m x n'); end;
if (any(size(c) ~= [n,1])) error('c must be n x 1'); end;
if (any(size(b) ~= [m,1])) error('b must be m x 1'); end;

% obtain a basis Z for the nullspace of the constraints and its orthogonal
% complement Y
[Q,R,E] = qr(A');
iz = [find(abs(diag(R))<opts.tol*abs(R(1,1))), m+1:n ];
Z=Q(:,iz);
Y=Q(:,setdiff(1:n,iz));

% Solve for py
h = -b; g = c;
py=-(A*Y)\h;

% Solve system with reduced Hessian using Cholesky
%pz = (Z'*G*Z)\rhs;
[R,p]=chol(Z'*G*Z);
if (p>0) 
 fprintf('EQP: Hessian is not pos def in null space of constraint\n'); 
 flag=1; x=[]; la=[];
 return;
end;
rhs = - Z'*(G*Y*py) - Z'*g; 
pz = R\(R'\rhs);

% get back solution
x = Y*py + Z*pz;

% and Lagrange multipliers if needed
if (nargout>1) la = ((A*Y)')\(Y'*(g+G*x)); end;