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

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 = 360.323;
t_2 = 600.241;
theta_dot = -0.003;
theta_2 = -0.08266;

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

[Sost_list, table, Coordinates] = 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);

Plot_Trajectory(Rm, h_isl(1,1), Coordinates(:, 2), Coordinates(:, 3), 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 [Sost_list, table, 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)

    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)

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

    % Заголовок таблицы
    head = ["t, [s]", "V_x, [m/s]", "V_y, [m/s]", "x, [m]", "y, [m]", "m, [kg]"];

    % Точка 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;

        [x_list, y_list, t_list] = Record(x_list, y_list, t_list, Sost, t, i);
        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) % 300.3
            dt_local = mod(t_1, dt);                   %   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 и доделываем шаг до 300.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;

            [x_list, y_list, t_list] = Record(x_list, y_list, t_list, Sost, t, i);
            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;

            [x_list, y_list, t_list] = Record(x_list, y_list, t_list, Sost, t, i);
            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 = mod(t_2, dt);                   %   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;

            [x_list, y_list, t_list] = Record(x_list, y_list, t_list, Sost, t, i);
            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;

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

    % Интегрирование по скорости
    while abs(Sost(1)^2 + Sost(2)^2 - V_k) > 10^(-8)
        % Сохраняем вектор состояния в момент времени 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;
            [x_list, y_list, t_list] = Record(x_list, y_list, t_list, Sost, t, i);
            i = i + 1;
        end

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



    % Обновление таблицы
    table = cat(1, head, Sost_list);

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

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

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

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

    % Орбита
    x_orb = Rm * cos(theta);
    y_orb = Rm * (sin(theta) - 1) + h_isl;
    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