clear; clc;
figure(3); clf; set(gcf, 'Color', 'w');
figure(2); clf; set(gcf, 'Color', 'w');
figure(1); clf; set(gcf, 'Color', 'w');

%% Quadratic Penalty

% Cost f(x) and single constraint h(x) = 0
f  = @(x) x(1) + x(2);
h  = @(x) x(1)^2 + x(2)^2 - 2;

betas = 2.^(-2:4);
hx = zeros(size(betas));

x0 = [1; 0];

figure(1); clf; hold all;
for counter = 1 : numel(betas)

    beta = betas(counter);
    [x, info] = quadratic_penalty(f, h, beta, x0);  % min F_beta
    hx(counter) = h(x);  % constraint violation
    % %
    % x0 = x; % warm start
    % %

    % Plot gradient norm vs iteration number
    semilogy([info.iter], ...
             [info.gradnorm], '.-', 'LineWidth', 2, 'MarkerSize', 20, ...
             'DisplayName', ['\beta = ', num2str(beta)]);
end

% Code below is only for plots (no computation)
xlabel('Iteration #');
title('Norm of {\nabla}F_{\beta}');
set(gca, 'YScale', 'log');
legend('Location', 'northeast');
set(gca, 'FontSize', 16);

% More plots as a function of beta
figure(2); clf;
loglog(betas, hx, '.-', 'LineWidth', 2, 'MarkerSize', 20);
hold all; plot([1, 10], [1, .1], 'LineWidth', 2); % reference slope 1/beta
text(4, .3, '1/\beta', 'FontSize', 16);
xlabel('\beta'); title('Constraint violation h(x^*)');
set(gca, 'FontSize', 16);
figure(3); clf;
semilogx(betas, betas.*hx, '.-', 'LineWidth', 2, 'MarkerSize', 20);
xlabel('\beta'); title('Product of \beta h(x^*)');
set(gca, 'FontSize', 16);

figure(1); % focus on this one when done