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

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.323; % 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);

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^(-7)
        % Сохраняем вектор состояния в момент времени 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 Plot_Trajectory(Rm, h_isl, x_list, y_list, x_special, y_special)
    figure('Name', "Траектория");
    hold on;

    % Луна
    theta = linspace(deg2rad(70), 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