%% Задача оптимизации
function [X_new, U_i_new] = Optimization(theta_dot, theta_2, P, W, dt, t_vert, t_1, t_2, ...
    mu_m, Rm, m_0, m_fuel_k, V_k, h_isl)

    alpha_0 = 0.0001;
    Iteration_Count = 0;

    % C_1, C_2
    C_1 = 0.5;
    C_2 = 1 / C_1;

    % alpha
    alpha_i = alpha_0;

    % Точность
    epsilon = 0.01;

    % Шаг по времени
    delta_t = 0.01;

    % Считаем краевую задачу и функцию g(X) без оптимизации
    [g_new, U_i_new] = Help_Optimization_Fun( ...
                           theta_dot, theta_2, ...
                           P, W, dt, t_vert,...
                           t_1, t_2, ...
                           mu_m, Rm, m_0, m_fuel_k, V_k, h_isl);

    %__________________частная_производная_________________

    % t1 - delta_t
    [g_grad_11, ~] = Help_Optimization_Fun( ...
                        theta_dot, theta_2, ...
                        P, W, dt, t_vert,...
                        t_1 - delta_t, t_2, ...
                        mu_m, Rm, m_0, m_fuel_k, V_k, h_isl);

    % t1 + delta_t
    [g_grad_12, ~] = Help_Optimization_Fun( ...
                        theta_dot, theta_2, ...
                        P, W, dt, t_vert,...
                        t_1 + delta_t, t_2, ...
                        mu_m, Rm, m_0, m_fuel_k, V_k, h_isl);

    % t2 - delta_t
    [g_grad_21, ~] = Help_Optimization_Fun( ...
                        theta_dot, theta_2, ...
                        P, W, dt, t_vert,...
                        t_1, t_2 - delta_t, ...
                         mu_m, Rm, m_0, m_fuel_k, V_k, h_isl);

    % t2 + delta_t
    [g_grad_22, ~] = Help_Optimization_Fun( ...
                        theta_dot, theta_2, ...
                        P, W, dt, t_vert,...
                        t_1, t_2 + delta_t, ...
                        mu_m, Rm, m_0, m_fuel_k, V_k, h_isl);

    % Считаем частную производную по t1, t2
    g_grad_t1 = Partial_Derivative(g_grad_11, g_grad_12, delta_t);
    g_grad_t2 = Partial_Derivative(g_grad_21, g_grad_22, delta_t);

    %_____________________________________________________

    % Вектор-столбец градиента
    grad_g = [g_grad_t1; g_grad_t2];

    disp(["g_new = " num2str(g_new)]);
    disp(["grad_g = " mat2str(grad_g)]);

    % Вектор-столбец решений
    X_new = [t_1; t_2];
    disp(["t_1, t_2 = " mat2str(X_new)]);

    % База
    while (abs(sqrt(grad_g(1, 1)^2 + grad_g(2, 1)^2)) >= epsilon)

        t_1 = X_new(1, 1);   t_2 = X_new(2, 1);
        g = g_new;

        %_____________смешанная_частная_производная_____________

        % Точка 1 (t_1 - delta_t, t_2 - 2 * delta_t)
        [g_1, ~] = Help_Optimization_Fun( ...
                       U_i_new(1, 1), U_i_new(2, 1), ...
                       P, W, dt, t_vert,...
                       t_1 - delta_t, t_2 - 2 * delta_t, ...
                       mu_m, Rm, m_0, m_fuel_k, V_k, h_isl);

        % Точка 2 (t_1 - delta_t, t_2 + 2 * delta_t)
        [g_2, ~] = Help_Optimization_Fun( ...
                       U_i_new(1, 1), U_i_new(2, 1), ...
                       P, W, dt, t_vert,...
                       t_1 - delta_t, t_2 + 2 * delta_t, ...
                       mu_m, Rm, m_0, m_fuel_k, V_k, h_isl);

        % Точка 3 (t_1 + delta_t, t_2 - 2 * delta_t)
        [g_3, ~] = Help_Optimization_Fun( ...
                       U_i_new(1, 1), U_i_new(2, 1), ...
                       P, W, dt, t_vert,...
                       t_1 + delta_t, t_2 - 2 * delta_t, ...
                       mu_m, Rm, m_0, m_fuel_k, V_k, h_isl);

        % Точка 4 (t_1 + delta_t, t_2 + 2 * delta_t)
        [g_4, ~] = Help_Optimization_Fun( ...
                       U_i_new(1, 1), U_i_new(2, 1), ...
                       P, W, dt, t_vert,...
                       t_1 + delta_t, t_2 + 2 * delta_t, ...
                       mu_m, Rm, m_0, m_fuel_k, V_k, h_isl);

        % Считаем частную производную по dt1 dt2
        f1_dt1 = Partial_Derivative(g_1, g_3, delta_t);
        f2_dt1 = Partial_Derivative(g_2, g_4, delta_t);
        Derivative_t12 = Partial_Derivative(f1_dt1, f2_dt1, 2 * delta_t);

        %______________полная_частная_производная_для_t1______________

        % Точка 1 (t_1 - 2 * delta_t, t_2)
        [g_11, ~] = Help_Optimization_Fun( ...
                        U_i_new(1, 1), U_i_new(2, 1), ...
                        P, W, dt, t_vert,...
                        t_1 - 2 * delta_t, t_2, ...
                        mu_m, Rm, m_0, m_fuel_k, V_k, h_isl);

        % Точка 2 (t_1, t_2)
        [g_12, ~] = Help_Optimization_Fun( ...
                        U_i_new(1, 1), U_i_new(2, 1), ...
                        P, W, dt, t_vert,...
                        t_1, t_2, ...
                        mu_m, Rm, m_0, m_fuel_k, V_k, h_isl);

        % Точка 3 (t_1, t_2)
        [g_13, ~] = Help_Optimization_Fun( ...
                        U_i_new(1, 1), U_i_new(2, 1), ...
                        P, W, dt, t_vert,...
                        t_1, t_2, ...
                        mu_m, Rm, m_0, m_fuel_k, V_k, h_isl);

        % Точка 4 (t_1 + 2 * delta_t, t_2)
        [g_14, ~] = Help_Optimization_Fun( ...
                        U_i_new(1, 1), U_i_new(2, 1), ...
                        P, W, dt, t_vert,...
                        t_1 + 2 * delta_t, t_2, ...
                        mu_m, Rm, m_0, m_fuel_k, V_k, h_isl);

        % Считаем частную производную по dt1 dt1
        f11_dt1 = Partial_Derivative(g_12, g_11, delta_t);
        f12_dt1 = Partial_Derivative(g_14, g_13, delta_t);
        Derivative_t11 = Partial_Derivative(f12_dt1, f11_dt1, delta_t);

        %______________полная_частная_производная_для_t2______________

        % Точка 1 (t_1, t_2 - 2 * delta_t)
        [g_21, ~] = Help_Optimization_Fun( ...
                        U_i_new(1, 1), U_i_new(2, 1), ...
                        P, W, dt, t_vert,...
                        t_1, t_2 - 2 * delta_t, ...
                        mu_m, Rm, m_0, m_fuel_k, V_k, h_isl);

        % Точка 2 (t_1, t_2)
        [g_22, ~] = Help_Optimization_Fun( ...
                        U_i_new(1, 1), U_i_new(2, 1), ...
                        P, W, dt, t_vert,...
                        t_1, t_2, ...
                        mu_m, Rm, m_0, m_fuel_k, V_k, h_isl);

        % Точка 3 (t_1, t_2)
        [g_23, ~] = Help_Optimization_Fun( ...
                        U_i_new(1, 1), U_i_new(2, 1), ...
                        P, W, dt, t_vert,...
                        t_1, t_2, ...
                        mu_m, Rm, m_0, m_fuel_k, V_k, h_isl);

        % Точка 4 (t_1, t_2 + 2 * delta_t)
        [g_24, ~] = Help_Optimization_Fun( ...
                        U_i_new(1, 1), U_i_new(2, 1), ...
                        P, W, dt, t_vert,...
                        t_1, t_2 + 2 * delta_t, ...
                        mu_m, Rm, m_0, m_fuel_k, V_k, h_isl);

        % Считаем частную производную по dt2 dt2
        f21_dt2 = Partial_Derivative(g_22, g_21, delta_t);
        f22_dt2 = Partial_Derivative(g_24, g_23, delta_t);
        Derivative_t22 = Partial_Derivative(f22_dt2, f21_dt2, delta_t);

        %_____________________________________________________________

        % Матрица Гессе
        H_i = [Derivative_t11 Derivative_t12;
               Derivative_t12 Derivative_t22];
        disp(["H_i = ", mat2str(H_i)]);


        % Интегрируем, подставляя X_new

        C_2_flag = false;
        while g_new >= g

            X = X_new;

            % Обновляем вектор-столбец результатов
            X_new = Iteration_Formula_LM(X_new, H_i, alpha_i, grad_g);

            [g_new, U_i_new] = Help_Optimization_Fun( ...
                U_i_new(1, 1), U_i_new(2, 1), ...
                P, W, dt, t_vert,...
                X_new(1, 1), X_new(2, 1), ...
                mu_m, Rm, m_0, m_fuel_k, V_k, h_isl);

            if g_new < g
                if C_2_flag == false
                    alpha_i = alpha_i * C_1;
                end
                disp(["1", num2str(alpha_i)]);
                break
            end

            alpha_i = alpha_i * C_2;
            X_new = X;
            disp(["2", num2str(alpha_i)]);
            C_2_flag = true;
        end

        % Вывод
        disp(["X_new = ", mat2str(X_new)]);
        disp(["U_i_new = ", mat2str(U_i_new)]);
        disp(["g_new = ", num2str(g_new)]);

        % Пересчет вектора градиента
        % t_1
        % Точка 1.1. (X_new(1, 1) - delta_t, X_new(2, 1))
        [g_recalc_grad_11, ~] = Help_Optimization_Fun( ...
            U_i_new(1, 1), U_i_new(2, 1), ...
            P, W, dt, t_vert,...
            X_new(1, 1) - delta_t, X_new(2, 1), ...
            mu_m, Rm, m_0, m_fuel_k, V_k, h_isl);

        % Точка 1.2. (X_new(1, 1) + delta_t, X_new(2, 1))
        [g_recalc_grad_12, ~] = Help_Optimization_Fun( ...
            U_i_new(1, 1), U_i_new(2, 1), ...
            P, W, dt, t_vert,...
            X_new(1, 1) + delta_t, X_new(2, 1), ...
            mu_m, Rm, m_0, m_fuel_k, V_k, h_isl);

        % Точка 2.1. (X_new(1, 1), X_new(2, 1) - delta_t)
        [g_recalc_grad_21, ~] = Help_Optimization_Fun( ...
            U_i_new(1, 1), U_i_new(2, 1), ...
            P, W, dt, t_vert,...
            X_new(1, 1), X_new(2, 1) - delta_t, ...
            mu_m, Rm, m_0, m_fuel_k, V_k, h_isl);

        % Точка 2.2. (X_new(1, 1), X_new(2, 1) + delta_t)
        [g_recalc_grad_22, ~] = Help_Optimization_Fun( ...
            U_i_new(1, 1), U_i_new(2, 1), ...
            P, W, dt, t_vert,...
            X_new(1, 1), X_new(2, 1) + delta_t, ...
            mu_m, Rm, m_0, m_fuel_k, V_k, h_isl);

        % Считаем производные
        g_recalc_t1 = Partial_Derivative(g_recalc_grad_11, g_recalc_grad_12, delta_t);
        g_recalc_t2 = Partial_Derivative(g_recalc_grad_21, g_recalc_grad_22, delta_t);

        grad_g = [g_recalc_t1; g_recalc_t2];
        disp(["grad_g = ", mat2str(grad_g)]);
        disp(["abs(grad_g) = ", num2str(abs(sqrt(grad_g(1, 1)^2 + grad_g(2, 1)^2)))]);

        Iteration_Count = Iteration_Count + 1;

        disp(["Iteration_Count", num2str(Iteration_Count)]);
    end

    % Вспомогательные функции
    function [g, U_i] = Help_Optimization_Fun( ...
        theta_dot, theta_2, ...
        P, W, dt, t_vert,...
        t_1, t_2, ...
        mu_m, Rm, m_0, m_fuel_k, V_k, h_isl)

        U_i = Border_Task(theta_dot, theta_2, P, W, dt, t_vert,...
            t_1, t_2, ...
            mu_m, Rm, m_0, m_fuel_k, V_k, h_isl);

        [Coordinates, ~] = Integration(P, W, dt, t_vert, ...
            t_1, t_2, ...
            U_i(1, 1), U_i(2, 1), ...
            mu_m, Rm, m_0, m_fuel_k, V_k);

        g = m_0 - Coordinates(end, 6);
    end

    % Итерационная формула
    function [X_new] = Iteration_Formula_LM(X_i, H_i, alpha_i, grad_g)
        X_new = X_i - inv(H_i + alpha_i .* eye(2, 2)) * grad_g;
    end
end