clear; close all; clc;
global Q c A b eps;

%% Exercise 6

Q = [ 1 0 ; 0 2 ] ;
c = [ -3 ; -4 ] ;
A = [-2 1 ; 1 1 ; 0 -1 ];
b = [ 0 ; 4 ; 0 ];

tau = 0.5;
eps0 = 5;
tolerance = 1e-3;

%% method

fprintf('Penalty method\n\n');
fprintf('eps \t\t x(1) \t\t x(2) \t\t max(Ax-b)\n\n');

options = optimoptions('fminunc','GradObj','on',...
    'Algorithm','quasi-newton','Display','off');

eps = eps0;

while true
    x = fminunc(@p_eps,[0;0],options);
    infeas = max(A*x-b);
    fprintf('%1.2e \t %1.6f \t %1.6f \t %1.3e\n',eps,x(1),x(2),infeas);
    if infeas < tolerance
        break
    else
        eps = tau*eps;
    end
end


%% penalized function 

function [v,g] = p_eps(x) 

    global Q c A b eps;

    v = 0.5*x'*Q*x + c'*x ;
    g = Q*x + c ;

    for i = 1 : size(A,1)
        v = v + (1/eps)*(max(0,A(i,:)*x-b(i)))^2 ;
        g = g + (2/eps)*(max(0,A(i,:)*x-b(i)))*A(i,:)';
    end

end
