%% Исходные данные

Rm = 1738 * 10^3;           % Радиус Луны
mu_m = 4.903 * 10^12;       % Гравитационная постоянная Луны
W = 3540;                   % Эффективная скорость истечения топлива, [м/с]
P = [8350 9500];            % Тяга СВ, [Н]
h_isl = [148 180;
         180 260] .* 10^3;  % Высота орбиты, [м]

m_0 = 3015;                 % Начальная масса СВ, [кг]
m_fuel_k = 1550;            % Предельная масса рабочего запаса топлива, [кг]
m_const = 615;              % Масса конструкции, [кг]
t_vert = 14;                % Продолжительность вертикального участка выведения, [с]

V_k = mu_m / (Rm + h_isl(1,1));     % Первая космическая для Луны (В КВАДРАТЕ!!!)

dt = 0.1;                   % Шаг интегрирования, [c]

% Начальные условия при t = 0
t = 0; x = 0; y = 0; V_x = 0; V_y = 0; m = m_0;

t_1 = 350.541; % 360.323
t_2 = 540.241; % 600.241
theta_dot = -0.00309; % -0.00309
theta_2 = -0.08266;   % -0.08266

%% Вызов функций

% [Coordinates, Sost_list] = Integration(P(1), W, dt, t_vert, t_1, t_2, theta_dot,...
%     theta_2, mu_m, Rm, m_0, m_fuel_k, V_k);
%
% [r_current, Theta_current] = Border_Params(Coordinates, Rm);

% U_i = Border_Task(theta_dot, theta_2, P(1), W, dt, t_vert, t_1, t_2, ...
%     mu_m, Rm, m_0, m_fuel_k, V_k, h_isl(1,1));
%
% [Coordinates, Sost_list] = Integration(P(1), 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);

[X_new, U_i_new] = Optimization(theta_dot, theta_2, P(1), W, dt, t_vert, t_1, t_2, ...
    mu_m, Rm, m_0, m_fuel_k, V_k, h_isl(1,1));

[Coordinates, Sost_list] = Integration(P(1), W, dt, t_vert, ...
    X_new(1, 1), X_new(2, 1), ...
    U_i_new(1, 1), U_i_new(2, 1), ...
    mu_m, Rm, m_0, m_fuel_k, V_k);

Plot_Trajectory(Rm, h_isl(1,1), Coordinates(:, 4), Coordinates(:, 5), Sost_list(:, 4), Sost_list(:, 5));



%% Функции

% Введем вектор состояния, который будет описывать всю ММ
% Sost = [V_x, V_y, x, y, m] ---> Sost(1) = V_x, ...

%% ММ движения СВ

function Mathmodel = MM(t, Sost, mu_m, Rm, t_vert, t_1, t_2, theta_dot,...
    theta_2, P, W)

    % Проекции гравитационного ускорения Луны
    g_x = - (mu_m * Sost(3)) / (Sost(3)^2 + (Rm + Sost(4))^2)^(3 / 2);
    g_y = - (mu_m * (Rm + Sost(4))) / (Sost(3)^2 + (Rm + Sost(4))^2)^(3 /2);

    % Изменение угла тангажа
    if t <= t_vert
        theta = pi / 2;
    elseif ((t > t_vert) && (t <= t_1))
        theta = pi / 2 + theta_dot * (t - t_vert);
    elseif t >= t_2
        theta = theta_2;
    else
        theta = 0;
    end

    % Система уравнений
    Vx_Dot = P / Sost(5) * cos(theta) + g_x;
    Vy_Dot = P / Sost(5) * sin(theta) + g_y;
    x_Dot = Sost(1);
    y_Dot = Sost(2);
    m_Dot = - P / W;

    % Свернем все в один вектор (вуктор-строка)
    Mathmodel = [Vx_Dot Vy_Dot x_Dot y_Dot m_Dot];
end

%% Ф-ия Рунге-Кутта 4-го порядка

function Sost = RK4(t, dt, Sost, mu_m, Rm, t_vert, t_1, t_2, theta_dot,...
    theta_2, P, W)

    K1 = dt * MM(t, Sost, mu_m, Rm, t_vert, t_1, t_2, theta_dot,...
        theta_2, P, W);
    K2 = dt * MM(t + dt / 2, Sost + K1/2, mu_m, Rm, t_vert, t_1, t_2, ...
        theta_dot, theta_2, P, W);
    K3 = dt * MM(t + dt / 2, Sost + K2/2, mu_m, Rm, t_vert, t_1, t_2, ...
        theta_dot, theta_2, P, W);
    K4 = dt * MM(t + dt, Sost + K3, mu_m, Rm, t_vert, t_1, t_2, theta_dot,...
        theta_2, P, W);

    % Обновляем результаты
    Sost = Sost + (K1 + 2 * K2 + 2 * K3 + K4) / 6;
end

%% Интегрирование

function [Coordinates, Sost_list] = Integration(P, W, dt, t_vert, t_1, t_2, theta_dot,...
    theta_2, mu_m, Rm, m_0, m_fuel_k, V_k)

    t = 0; x = 0; y = 0; V_x = 0; V_y = 0; m = m_0;  % Начальные условия при t = 0
    Sost = [V_x V_y x y m];                          % Вектор состояния с начальными данными
    Sost_list = zeros(1, 6);                         % "Хранилище" (Вектор строка, образованная из t и Sost)

    % Создадим пустые строки для записи координат и времени
    Vx_list = zeros();     Vy_list = zeros();
    x_list = zeros();      y_list = zeros();
    m_list = zeros();      t_list = zeros();

    i = 1;

    % Точка t_vert
    while t < t_vert
        Sost = RK4(t, dt, Sost, mu_m, Rm, t_vert, t_1, t_2, theta_dot,...
            theta_2, P, W);
        t = t + dt;

        [Vx_list, Vy_list, x_list, y_list, m_list, t_list] = Record(Sost, t, i, ...
            Vx_list, Vy_list, x_list, y_list, m_list, t_list);
        i = i + 1;
        if round(t, 1) == t_vert
            Sost_list = cat(2, t, Sost);
        end
    end

    % Точка t_1
    while t < t_1
        if round(t, 1) == round(t_1 - mod(t_1, dt), 1) % 360.3
            dt_local = t_1 - t;                        %   0.023
            Sost = RK4(t, dt_local, Sost, mu_m, Rm, t_vert, t_1, t_2, theta_dot,...
                theta_2, P, W);
            Sost_list = cat(1, Sost_list, cat(2, t_1, Sost));

            % Выключаем тягу P = 0 и доделываем шаг до 360.4
            Sost = RK4(t_1, dt - dt_local, Sost, mu_m, Rm, t_vert, t_1, t_2, ...
                theta_dot, theta_2, 0, W);
            t = t + dt;

            [Vx_list, Vy_list, x_list, y_list, m_list, t_list] = Record(Sost, t, i, ...
                Vx_list, Vy_list, x_list, y_list, m_list, t_list);
            i = i + 1;
        else
            Sost = RK4(t, dt, Sost, mu_m, Rm, t_vert, t_1, t_2, theta_dot,...
                theta_2, P, W);
            t = t + dt;

            [Vx_list, Vy_list, x_list, y_list, m_list, t_list] = Record(Sost, t, i, ...
                Vx_list, Vy_list, x_list, y_list, m_list, t_list);
            i = i + 1;
        end
    end

    % Точка t_2
    while t < t_2
        if round(t, 1) == round(t_2 - mod(t_2, dt), 1) % 600.2
            dt_local = t_2 - t;                        %   0.041
            Sost = RK4(t, dt_local, Sost, mu_m, Rm, t_vert, t_1, t_2, theta_dot,...
                theta_2, 0, W);
            Sost_list = cat(1, Sost_list, cat(2, t_2, Sost));

            % Включаем тягу и доделываем шаг до 600.3
            Sost = RK4(t_2, dt - dt_local, Sost, mu_m, Rm, t_vert, t_1, t_2, theta_dot,...
                theta_2, P, W);
            t = t + dt;

            [Vx_list, Vy_list, x_list, y_list, m_list, t_list] = Record(Sost, t, i, ...
                Vx_list, Vy_list, x_list, y_list, m_list, t_list);
            i = i + 1;
        else
            Sost = RK4(t, dt, Sost, mu_m, Rm, t_vert, t_1, t_2, theta_dot,...
                theta_2, 0, W);
            t = t + dt;

            [Vx_list, Vy_list, x_list, y_list, m_list, t_list] = Record(Sost, t, i, ...
                Vx_list, Vy_list, x_list, y_list, m_list, t_list);
            i = i + 1;
        end
    end

    % Интегрирование по скорости
    while abs(sqrt(Sost(1)^2 + Sost(2)^2) - sqrt(V_k)) > 10^(-10)
        % Сохраняем вектор состояния в момент времени t = 600.3
        Sost_previous = Sost;

        Sost = RK4(t, dt, Sost, mu_m, Rm, t_vert, t_1, t_2, theta_dot,...
                theta_2, P, W);

        if (Sost(1)^2 + Sost(2)^2) > V_k
            Sost = Sost_previous;
            dt = dt / 10;
        else
            t = t + dt;
            [Vx_list, Vy_list, x_list, y_list, m_list, t_list] = Record(Sost, t, i, ...
                Vx_list, Vy_list, x_list, y_list, m_list, t_list);
            i = i + 1;
        end

        % Условие на массу СВ
        % if round(Sost(5), 8) < (m_0 - m_fuel_k)
        %     Sost = Sost_previous;
        %     t = t - dt;
        %     disp('Карамба!');
        %     break
        % end
    end

    % Таблица [t_list' Vx_list' Vy_list' x_list', y_list' m_list']
    Coordinates = cat(2, t_list', Vx_list', Vy_list', x_list', y_list', m_list');

    % Вложенная вспомогательная функция записи координат и времени
    function [Vx_list, Vy_list, x_list, y_list, m_list, t_list] = Record(Sost, t, i, ...
                Vx_list, Vy_list, x_list, y_list, m_list, t_list)
        Vx_list(i) = Sost(1);  Vy_list(i) = Sost(2);
        x_list(i) = Sost(3);   y_list(i) = Sost(4);
        m_list(i) = Sost(5);   t_list(i) = t;
    end
end

% Граничные параметры
function [r, Theta] = Border_Params(Coordinates, Rm)
    % Распаковка
    Vx = Coordinates(end, 2);   Vy = Coordinates(end, 3);
    x  = Coordinates(end, 4);   y  = Coordinates(end, 5);

    % Вычисление
    V = sqrt(Vx^2 + Vy^2);
    r = sqrt((Rm + y)^2 + x^2);
    Theta = asin((Vx * x + Vy * (Rm + y)) / (r * V));
end

% Функция расчета частной производной методом центральных разностей
function [PD] = Partial_Derivative(f1, f2, delta)
    PD = (f2 - f1) / (2 * delta);
end

% Итерационная формула
function [U_i_new, F_U] = Iteration_Formula(theta_dot, theta_2, r_current, Theta_current,...
    delta_theta_dot, delta_theta_2, ...
    r_f1_theta_dot, r_f2_theta_dot, r_f1_theta_2, r_f2_theta_2, ...
    Theta_f1_theta_dot, Theta_f2_theta_dot, Theta_f1_theta_2, Theta_f2_theta_2,...
    Rm, h_isl)

    U_i = [theta_dot; theta_2];

    % Расчет производных для Якобиана
    dr_dtheta_dot = Partial_Derivative(r_f1_theta_dot, r_f2_theta_dot, delta_theta_dot);
    dr_dtheta_2 = Partial_Derivative(r_f1_theta_2, r_f2_theta_2, delta_theta_2);

    dTheta_dtheta_dot = Partial_Derivative(Theta_f1_theta_dot, Theta_f2_theta_dot, delta_theta_dot);
    dTheta_dtheta_2 = Partial_Derivative(Theta_f1_theta_2, Theta_f2_theta_2, delta_theta_2);

    % Якобиан
    J = [dr_dtheta_dot       dr_dtheta_2;
         dTheta_dtheta_dot   dTheta_dtheta_2];

    % Матрица невязок
    F_U = -[r_current - (Rm + h_isl);
            Theta_current];

    U_i_new = U_i + inv(J) * F_U;
end

% Краевая задача
function [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)

    % Определим шаг для theta_dot и theta_2
    delta_theta_dot = theta_dot / 1000;
    delta_theta_2 = theta_2 / 1000;

    r_diff = 1;
    Theta_diff = 1;

    U_i = [theta_dot; theta_2];

    while (abs(Theta_diff) > 10^(-6)) || (abs(r_diff) > 10^(-6))

        theta_dot = U_i(1, 1);
        theta_2 = U_i(2, 1);


        % Для текущих значений r_current, Theta_current
        [Coordinates, ~] = Integration(P, W, dt, t_vert, t_1, t_2, theta_dot,...
            theta_2, mu_m, Rm, m_0, m_fuel_k, V_k);
        [r_current, Theta_current] = Border_Params(Coordinates, Rm);

        % Вычисление производных
        [Coordinates, ~] = Integration(P, W, dt, t_vert, t_1, t_2,...
            theta_dot - delta_theta_dot,...
            theta_2, mu_m, Rm, m_0, m_fuel_k, V_k);
        [r_f1_theta_dot, Theta_f1_theta_dot] = Border_Params(Coordinates, Rm);

        [Coordinates, ~] = Integration(P, W, dt, t_vert, t_1, t_2,...
            theta_dot + delta_theta_dot,...
            theta_2, mu_m, Rm, m_0, m_fuel_k, V_k);
        [r_f2_theta_dot, Theta_f2_theta_dot] = Border_Params(Coordinates, Rm);

        [Coordinates, ~] = Integration(P, W, dt, t_vert, t_1, t_2, theta_dot,...
            theta_2 - delta_theta_2, ...
            mu_m, Rm, m_0, m_fuel_k, V_k);
        [r_f1_theta_2, Theta_f1_theta_2] = Border_Params(Coordinates, Rm);

        [Coordinates, ~] = Integration(P, W, dt, t_vert, t_1, t_2, theta_dot,...
            theta_2 + delta_theta_2, ...
            mu_m, Rm, m_0, m_fuel_k, V_k);
        [r_f2_theta_2, Theta_f2_theta_2] = Border_Params(Coordinates, Rm);

        [U_i, F_U] = Iteration_Formula(theta_dot, theta_2, r_current, Theta_current,...
            delta_theta_dot, delta_theta_2, ...
            r_f1_theta_dot, r_f2_theta_dot, r_f1_theta_2, r_f2_theta_2, ...
            Theta_f1_theta_dot, Theta_f2_theta_dot, Theta_f1_theta_2, Theta_f2_theta_2,...
            Rm, h_isl);

        r_diff = F_U(1, 1);
        Theta_diff = F_U(2, 1);

        % disp([F_U(1,1), F_U(2, 1)]);
    end
end

%% Задача оптимизации
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
%% Визуализация

function Plot_Trajectory(Rm, h_isl, x_list, y_list, x_special, y_special)
    figure('Name', "Траектория");
    hold on;

    % Луна
    theta = linspace(deg2rad(40), deg2rad(100), 10^3);
    x_moon = Rm * cos(theta);
    y_moon = Rm * (sin(theta) - 1);
    plot(x_moon, y_moon, 'k');

    % Орбита
    x_orb = (Rm + h_isl) * cos(theta);
    y_orb = (Rm + h_isl) * sin(theta) - Rm;
    plot(x_orb, y_orb, 'k--');

    % Траектория
    plot(x_list, y_list, 'b');

    % Критические точки
    scatter(x_special, y_special, 'rs');

    % Настройка осей и сетки
    grid on;
    grid minor;
    xline(0, 'k--');
    yline(0, 'k--');
    axis equal;
    xlabel('x, [m]');
    ylabel('y, [m]');
    title('Траектория СВ');
    legend('Луна', 'Орбита', 'Траектория СВ', 'Особые точки', 'Location', 'Northwest');

    hold off;
end