clear
clc
close all
syms xsym ysym

%% 2.4 
f1 = (ysym - 4 * xsym^2)^2 + (1 - xsym)^2;
[x1, xs1, ~] = trust_region(f1, 2, 1, 0.2, [0;0], 1e-8, 1000);

%% 2.4c - Energy function plot
surf_iterates(f1, xs1, -0.25:0.01:1.25, -0.5:0.01:4.25);
figure;

%% 2.5 - Rosenbrock's function
f2 = (1 - xsym)^2 + 100 * (ysym - xsym^2)^2;
[x2, xs2, gnorms2] = trust_region(f2, 2, 1, 0.2, [0;0], 1e-8, 1000);

%% 2.5b - Energy function plot
surf_iterates(f2, xs2, -0.25:0.01:1.25, -0.25:0.01:1.25);

%% 2.5c - Log gradient norms
figure;
semilogy(0:size(gnorms2, 2)-1, gnorms2, '.-k');

%% Helper functions
function surf_iterates(f, xs, xrange, yrange)
    fl = matlabFunction(f);
    Z = zeros(size(yrange,2), size(xrange,2));

    for x = 1:size(xrange, 2)
        for y = 1:size(yrange, 2)
            Z(y,x) = fl(xrange(x), yrange(y));
        end
    end

    surf(xrange, yrange', Z, 'EdgeColor', 'none', 'FaceAlpha', 0.4);

    yrange = zeros(1, size(xs, 2));
    for i=1:size(xs, 2)
        yrange(1, i) = fl(xs(1, i), xs(2, i));
    end

    hold on
    plot3(xs(1, :), xs(2, :), yrange, '.r-');
    plot3(xs(1, 1), xs(2, 1), yrange(1), '.w-');
    plot3(xs(1, size(xs,2)), xs(2, size(xs,2)), yrange(size(xs,2)), '.k-');
    hold off
end