function [xn,W,flag,f,lam,mu] = qp_activeset_step(xk,G,c,A,b,B,d,W)
% Take a step for an active set method for
%   min  1/2 x'*G*x + c'*x
%   s.t. Ax = b
%        Bx >= d
% where 
% xk - current iterate
% G - hessian in objective func.
% c - constant in objective
% A,b - equality constraint
% B,d - inequality constraint
% W - current working set

n = size(G,1);
m = size(A,1);
flag = 0;

nonworking = 1:size(B,1);
nonworking(W) = []; % downselect

% verify that xk is feasiable
if ~isempty(B), assert(all(B*xk >= d - 100*eps)); end
if ~isempty(A), assert(all(abs(A*xk - b) < 100*eps));  end

Aeq = [A; B(W,:)];

[p,lamall] = kkt_solve(G,G*xk + c,Aeq);

lam = lamall(1:m);
mu(nonworking) = 0;
mu(W) = lamall(m+1:end);



if norm(p) < 100*eps(1)
    % in this case, we aren't going anywhere
    xn = xk;
    if all(mu > -100*eps(1))
        % SOLVED IT!
        flag = 1;
    else
        [~,j] = min(mu);
        W = setdiff(W,j); % remove it from the active set
        
    end
else
    % okay, we are moving, find the 
    % first constraint we hit that
    % isn't in the working set

    alpha = 1; ind = [];
    for i=1:length(nonworking)
        bi = B(nonworking(i),:);
        di = d(nonworking(i));
        if bi*p < -100*eps(1)
            testalpha = (di - bi*xk)/(bi*p);
            if testalpha <= alpha
                ind = nonworking(i);
                alpha = testalpha;
            end
        end
    end
    
    xn = xk + alpha*p;
    if ~isempty(ind)
        W = union(W,ind);
    end
    
end
    
f = 1/2*(xn'*G*xn) + xn'*c;
    

function [p,lam] = kkt_solve(G,g,A)
% A fast human implementation of a KKT solver
% for 
%   min  1/2*p'*G*p + g'*p
%   s.t. A*p = 0
n = size(G,1);
m = size(A,1);
sys = [G A'; A zeros(m,m)];
rhs = [g; zeros(m,1)];
sol = sys\rhs;
p = -sol(1:n);
lam = sol(n+1:n+m);

