% Define environment (functions, gradients etc...)
GivenEnv

% Define parameters
max_iter = 300; % Maximum iterations
tol = 1e-4; % Tolerance

% Methods tuning
amijo_beta = 0.5; % Armijo reduction factor
amijo_sigma = 0.1; % Armijo condition constant

m = 0.01;

% Point x0 = (0, 0)
% x0 = [0, 0];


% Point x0 = (0, 0)
x0s = [0, 0; -1, 1 ; 1, -1]; % Initial points
for i = 1:size(x0s, 1)
    x0 = x0s(i, :);
    point_str = "[" + x0(1) + ", " + x0(2) + "]";

    % Find the best fixed gamma
    k = zeros(100, 1);
    j = 1;
    n = linspace(0.1, 1.5, 100);
    for g = n
        gamma_fixed_step = g;
        [~, ~, k(j)] = lev_mar(fun, grad_fun, hessian_fun, m, x0, tol, max_iter, 'fixed');
        j = j + 1;
    end
    plotIterationsOverGamma(n, k, "Iteration for different $\gamma$ values", "LevMar_Iter_o_gamma_" + i + ".png");

    [~, j] = min(k);
    gamma_fixed_step = n(j);

    [x_fixed, f_fixed, kk] = lev_mar(fun, grad_fun, hessian_fun, m, x0, tol, max_iter, 'fixed');
    fprintf('Fixed step:     Initial point (%f, %f), steps:%d, Final (x,y)=(%f, %f), f(x,y)=%f\n', x0, kk, x_fixed(end, :), f_fixed(end));
    plotPointsOverContour(x_fixed, fun, [-2, 2], [-3, 3], 100, point_str + ": LevMar $\gamma$ = " + gamma_fixed_step, "LevMar_fixed_" + i + ".png");


    % Minimized f
    [x_minimized, f_minimized, kk] = lev_mar(fun, grad_fun, hessian_fun, m, x0, tol, max_iter, 'minimized');
    fprintf('Minimized f(g): Initial point (%f, %f), steps:%d, Final (x,y)=(%f, %f), f(x,y)=%f\n', x0, kk, x_minimized(end, :), f_minimized(end));
    plotPointsOverContour(x_minimized, fun, [-2, 2], [-3, 3], 100, point_str + ": LevMar minimized $f(x_k + \gamma_kd_k)$", "LevMar_minimized_" + i + ".png");  


    % Armijo Rule
    [x_armijo, f_armijo, kk] = lev_mar(fun, grad_fun, hessian_fun, m, x0, tol, max_iter, 'armijo');
    fprintf('Armijo step:    Initial point (%f, %f), steps:%d, Final (x,y)=(%f, %f), f(x,y)=%f\n', x0, kk, x_armijo(end, :), f_armijo(end));
    plotPointsOverContour(x_armijo, fun, [-2, 2], [-3, 3], 100, point_str + ": LevMar Armijo method", "LevMar_armijo_" + i + ".png");  
end