%% Define the QP
% minimize    -x1 - 2*x2
% subject to  -2*x1 + x2 <= 2
%             -x1 + 2*x2 <= 7
%              x1        <= 3
%              x1, x2 >= 0
%
% Except, we need this in standard form:
% minimize f'*x subject to. A*x <= b

% the objective is 1/2 x'*G*x + c'*x;
n = 2;
G = eye(n);
c = zeros(n,1);
c(1) = 3; c(2) = -5;
f = @(x) 0.5*(x'*G*x) + c'*x;
A = [-2 1; -1 2; 1 0; -0.5 -1];
b = [2; 7; 3; -1];
lb = [];
ub = [];

%% Plot the feasible polytope and solution contours
clf;
fxy = @(x,y) f([x;y]);
ezcontour(fxy,[-1 3,-1 5]); hold on;
plotregion(-A,-b,lb,ub); 

%%
x0 = [3;0];
W = [3];
flag = 0;
xk = x0;
iter = 1;
X = [];
while ~flag && iter < 1000
    X(:,iter) = xk;
    plot(xk(1), xk(2), 'ro','MarkerSize',16);
    [xn,Wnew,flag,f,lam,mu] = qp_activeset_step(xk,G,c,[],[],-A,-b,W);
    if numel(Wnew) > numel(W), note=sprintf('added c%i',setdiff(Wnew,W)); 
    elseif numel(Wnew) == numel(W), note=sprintf('stepped'); 
    else note=sprintf('removed c%i',setdiff(W,Wnew)); 
    end
    
    fprintf('Iter %3i  f=%4.1f  x=[%3.1f %3.1f]  mu=[%3.1f %3.1f %3.1f %3.1f]  note=%s\n',...
        iter, f, xn(1), xn(2), mu(1), mu(2), mu(3), mu(4), note);
    %xn
    xk = xn; W = Wnew;
    iter = iter + 1;
end
  
  
plot(xk(1), xk(2), 'r*','MarkerSize',16);
plot(X(1,:), X(2,:), 'r');
hold off;


%% Show the problem from the book
% Nocedal and Wright, Figure 16.3
% the objective is 1/2 x'*G*x + c'*x;
n = 2;
G = 2*eye(n);
c = [-2; -5];
f = @(x) 0.5*(x'*G*x) + c'*x;
A = [1 -2; -1 -2; -1 2; 1 0; 0 1];
b = [-2; -6; -2; 0; 0];
% Ax >= b

clf;
%fxy = @(x,y) f([x;y]);
fxy = @(x,y) (x-1)^2 + (y-2.5)^2;
ezcontour(fxy,[0 5 0 5]); hold on;
plotregion(A,b,[],[]); 
%%
x0 = [2;0];
W = [3 5];
flag = 0;
xk = x0;
iter = 1;
X = [];
while ~flag && iter < 1000
    X(:,iter) = xk;
    plot(xk(1), xk(2), 'ro','MarkerSize',16);
    [xn,Wnew,flag,f,lam,mu] = qp_activeset_step(xk,G,c,[],[],A,b,W);
    if numel(Wnew) > numel(W), note=sprintf('added c%i',setdiff(Wnew,W)); 
    elseif numel(Wnew) == numel(W), note=sprintf('stepped'); 
    else note=sprintf('removed c%i',setdiff(W,Wnew)); 
    end
    
    fprintf('Iter %3i  f=%4.1f  x=[%3.1f %3.1f]  mu=[%3.1f %3.1f %3.1f %3.1f %3.1f]  note=%s\n',...
        iter, f, xn(1), xn(2), mu(1), mu(2), mu(3), mu(4), mu(5), note);
    %xn
    xk = xn; W = Wnew;
    iter = iter + 1;
end
  
  
plot(xk(1), xk(2), 'r*','MarkerSize',16);
plot(X(1,:), X(2,:), 'r');
hold off;