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

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


% Point x0 = (0, 0)
% =========================================================================
point = 1;
x0 = [0, 0];
f = fun(x0(1), x0(2));
gf = grad_fun(x0(1), x0(2));
hf = hessian_fun(x0(1), x0(2));
ev = eig(hf);
fprintf('Initial point (%d, %d), f = %f, grad = [%f;%f], hessian = [%f %f ; %f %f]. Eigenvalues= [%f, %f], Can NOT use method\n', x0, f, gf, hf, ev);
disp(' ');


% Point x0 = (-1, 1)
% =========================================================================
point = 2;
x0 = [-1, 1];
point_str = "[" + x0(1) + ", " + x0(2) + "]";

f = fun(-1, 1);
gf = grad_fun(x0(1), x0(2));
hf = hessian_fun(x0(1), x0(2));
ev = eig(hf);
fprintf('Initial point (%d, %d), f = %f, grad = [%f;%f], hessian = [%f %f ; %f %f]. Eigenvalues= [%f, %f], Can use method\n', x0, f, gf, hf, ev);


% 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;
    [x, f, k(j)] = method_lev_mar(fun, grad_fun, hessian_fun, 0.3, x0, tol, max_iter, 'fixed');
    if ~(x(end, 1) < -1.57 && x(end, 1) > -1.59 && x(end, 2) < 0.01 && x(end,2) > -0.01 && f(end) < -0.8 && f(end) > -0.82)
        k(j) = 300;
    end
    j = j + 1;
end
plotItersOverGamma(n, k, "Iteration for different $\gamma$ values", "figures/LevMar_Iter_o_gamma_" + point + ".png");

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

[x_fixed, f_fixed, kk] = method_lev_mar(fun, grad_fun, hessian_fun, 0.3, 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, [-3, 0], [-2, 2], 100, point_str + ": Levenberg-Marquardt $\gamma$ = " + gamma_fixed_step, "figures/LevMar_fixed_" + point + ".png");

[x_minimized, f_minimized, kk] = method_lev_mar(fun, grad_fun, hessian_fun, 0.3, 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, [-3, 0], [-2, 2], 100, point_str + ": Levenberg-Marquardt minimized $f(x_k + \gamma_kd_k)$", "figures/LevMar_minimized_" + point + ".png");

% Armijo Rule

% Methods tuning
amijo_beta = 0.4;  % typical range: [0.1, 0.8]
amijo_sigma = 0.1; % typical range: [0.01, 0.3]

[x_armijo, f_armijo, kk] = method_lev_mar(fun, grad_fun, hessian_fun, 0.3, 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, [-3, 0], [-2, 2], 100, point_str + ": Levenberg-Marquardt Armijo method", "figures/LevMar_armijo_" + point + ".png");  
disp(' ');

% Compare methods
plotConvCompare(x_fixed, "Fixed", x_minimized, "Minimized", x_armijo, "Armijo", Xmin, "Convergence compare", "figures/LevMar_compare_" + point + ".png");

% Point x0 = (1, -1)
% =========================================================================
point = 3;
x0 = [1, -1];
point_str = "[" + x0(1) + ", " + x0(2) + "]";

f = fun(-1, 1);
gf = grad_fun(x0(1), x0(2));
hf = hessian_fun(x0(1), x0(2));
ev = eig(hf);
fprintf('Initial point (%d, %d), f = %f, grad = [%f;%f], hessian = [%f %f ; %f %f]. Eigenvalues= [%f, %f], Can use method\n', x0, f, gf, hf, ev);


% 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;
    [x, f, k(j)] = method_lev_mar(fun, grad_fun, hessian_fun, 0.3, x0, tol, max_iter, 'fixed');
    if ~(x(end, 1) < -1.57 && x(end, 1) > -1.59 && x(end, 2) < 0.01 && x(end,2) > -0.01 && f(end) < -0.8 && f(end) > -0.82)
        k(j) = 300;
    end
    j = j + 1;
end

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

[x_fixed, f_fixed, kk] = method_lev_mar(fun, grad_fun, hessian_fun, 0.3, 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, [-3, 2], [-2, 2], 100, point_str + ": Levenberg-Marquardt $\gamma$ = " + gamma_fixed_step, "figures/LevMar_fixed_" + point + ".png");

[x_fixed, f_fixed, kk] = method_lev_mar(fun, grad_fun, hessian_fun, 0.3, 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_fixed(end, :), f_fixed(end));
plotPointsOverContour(x_fixed, fun, [-3, 2], [-2, 2], 100, point_str + ": Levenberg-Marquardt minimized $f(x_k + \gamma_kd_k)$", "figures/LevMar_minimized_" + point + ".png");

% Armijo Rule

% Methods tuning
amijo_beta = 0.4;  % typical range: [0.1, 0.8]
amijo_sigma = 0.1; % typical range: [0.01, 0.3]

[x_armijo, f_armijo, kk] = method_lev_mar(fun, grad_fun, hessian_fun, 0.3, 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, [-3, 2], [-2, 2], 100, point_str + ": Levenberg-Marquardt Armijo method", "figures/LevMar_armijo_" + point + ".png");  
disp(' ');