%% Начальные данные

V_0 = 113.6;                     % Начальная скорость пули, [м/с]
m = 0.34 * 10^(-3);              % Масса пули, [кг]
d = 4.5 * 10^(-3);               % Калибр, [м]
C_xa = 0.4;                      % Коэффициент лобового сопративления, [б/р]
theta_0 = deg2rad(0.61);         % Начальный угол бросания, [рад]
S = pi * (d / 2)^2;              % Площадь миделя, [м^2]

b = 18.5 * 10^(-3);

%% Константы

rho = 1.225;                     % Плотность воздуха при НУ, [кг/м^3]
g = 9.80665;                     % Ускорение свободного падения, [м/с^2]

%% Начальные условия

t = 0;
x = 0;
y = 1e-5;
V = V_0;
theta = theta_0;

%% Траектория пули при theta = theta_0

% Найдем пересечение параболы безопасности с y = 0 --> final_x
final_x = V_0^2 / g;

% Построим параболу безопасности
Parabola_Safe(final_x, V_0, g);

% Создадим пустые вектора для графика
time_vector = zeros();
x_vector = zeros();
y_vector = zeros();
V_vector = zeros();
theta_vector = zeros();
%vizirovanie_zeros = zeros();

% Еще пустые вектора для таблицы
time_table = zeros();
x_table = zeros();
y_table = zeros();
V_table = zeros();
theta_table = zeros();

% Интегрирование
i = 1;
j = 1;
k = 1;
dt = 0.00001;
while y > 0
    % Запись
    time_vector(i) = t;
    x_vector(i) = x;
    y_vector(i) = y;
    V_vector(i) = V;
    theta_vector(i) = theta;

    % Сортировка таблицы
    if mod(i - 1, 1000) == 0
        time_table(k) = t;
        x_table(k) = x;
        y_table(k) = y;
        V_table(k) = V;
        theta_table(k) = theta;
        k = k + 1;
    end

    % Координаты точек пересечния линии визирования с траекторией
    if round(y, 5) == b
        %vizirovanie_zeros(j) = x;
        j = j + 1;
    end

    [x, y, V, theta] = RK4(x, y, V, theta, dt, g, S, C_xa, rho, m);

    % Обновление времени и счетчика
    t = t + dt;
    i = i + 1;
end

% Создадим таблицу для t = [0 0.01 0.02 ... time_vector(i - 1)]
head = ["t, [с]", "x, [м]", "y, [м]", "V, [м/с]", "theta, [град.]"];
table_1 = cat(1, time_table, x_table, y_table, V_table, rad2deg(theta_table));
table_1 = table_1';
last_row = [time_vector(i - 1), x_vector(i - 1), y_vector(i - 1), V_vector(i - 1),...
    rad2deg(theta_vector(i - 1))];
table_1 = cat(1, head, table_1, last_row);

% График траектории пули
figure('Name',"График траектории пули")
plot(x_vector, y_vector, "r-"); grid on;
xlim([0 max(x_vector)]);
ylim([0 max(y_vector)]);
hold on;
%plot(vizirovanie_zeros, [b b], "k--x");
hold off;
title('График траектории пули при \theta_0 = 0.61 [град.]');
xlabel('x, [м]');
ylabel('y, [м]');

%% Прямой выстрел

coef = 0;
Height = 0;
while Height < 1
    % Для каждой новой итерации переписываем начальные условия
    t = 0;
    x = 0;
    y = 1e-5;
    V = V_0;
    theta = theta_0 + coef;

    % Создадим пустые вектора для графика
    time_vector_straight = zeros();
    x_vector_straight = zeros();
    y_vector_straight = zeros();
    V_vector_straight = zeros();
    theta_vector_straight = zeros();

    % Создадим пустые вектора для графика
    time_table_straight = zeros();
    x_table_straight = zeros();
    y_table_straight = zeros();
    V_table_straight = zeros();
    theta_table_straight = zeros();

    % Интегрирование
    i = 1;
    j = 1;
    while y > 0
        time_vector_straight(i) = t;
        x_vector_straight(i) = x;
        y_vector_straight(i) = y;
        V_vector_straight(i) = V;
        theta_vector_straight(i) = theta;

        % Сортировка таблицы
        if mod(i - 1, 1000) == 0
            time_table_straight(k) = t;
            x_table_straight(k) = x;
            y_table_straight(k) = y;
            V_table_straight(k) = V;
            theta_table_straight(k) = theta;
            k = k + 1;
        end

        [x, y, V, theta] = RK4(x, y, V, theta, dt, g, S, C_xa, rho, m);

        t = t + dt;
        i = i + 1;
    end
    Height = max(y_vector_straight);
    coef = coef + 0.0001;

    disp(Height);
end

% Создадим таблицу для t = [0 0.01 0.02 ... time_vector_straight(i - 1)]
table_2 = cat(1, time_table_straight, x_table_straight, y_table_straight, ...
    V_table_straight, rad2deg(theta_table_straight));
table_2 = table_2';
last_row_straight = [time_vector_straight(i - 1), x_vector_straight(i - 1),...
    y_vector_straight(i - 1), V_vector_straight(i - 1),...
    rad2deg(theta_vector_straight(i - 1))];
table_2 = cat(1, head, table_2, last_row_straight);


% График траектории пули при прямом выстреле
figure('Name',"Прямой выстрел");
plot(x_vector_straight, y_vector_straight, "r-"); grid on;
ylim([0 1.002]);
xlim([0 65]);
hold on
plot([0 65], [b b], "k--");
hold off
title('Прямой выстрел');
xlabel('x, [м]');
ylabel('y, [м]');

%% Функции

% Парабола безопасности
function Parabola_Safe(final_x, V_0, g)
    x_vector = 0:(final_x / 1000): final_x;
    y_vector = zeros();
    i = 1;
    for x = x_vector
        y_vector(i) = V_0^2 / (2 * g) - (g * x^2) / (2 * V_0^2);
        i = i + 1;
    end
    figure('Name',"Парабола безопасности");
    plot(x_vector, y_vector, "r-"); grid on;
    title('Парабола безопасности');
    xlabel('x');
    ylabel('y');
end

% Сила лобового сопротивления
function [Xa] = X_a(V, S, C_xa, rho)
    Xa = C_xa * (rho * V^2) / 2 * S;
end

% Математическая модель
function [dxdt] = dx_dt(V, theta)
    dxdt = V * cos(theta);
end
function [dydt] = dy_dt(V, theta)
    dydt = V * sin(theta);
end
function [dVdt] = dV_dt(V, theta, g, S, C_xa, rho, m)
    dVdt = (-X_a(V, S, C_xa, rho) - m * g * cos(theta)) / m;
end
function [dthetadt] = dtheta_dt(V, theta, g)
    dthetadt = (-g * cos(theta)) / V;
end

% РК4
function [x, y, V, theta] = RK4(x, y, V, theta, dt, g, S, C_xa, rho, m)
    K1_x     = dt *     dx_dt(V, theta);
    K1_y     = dt *     dy_dt(V, theta);
    K1_V     = dt *     dV_dt(V, theta, g, S, C_xa, rho, m);
    K1_theta = dt * dtheta_dt(V, theta, g);

    K2_x     = dt *     dx_dt(V + K1_V / 2, theta + K1_theta / 2);
    K2_y     = dt *     dy_dt(V + K1_V / 2, theta + K1_theta / 2);
    K2_V     = dt *     dV_dt(V + K1_V / 2, theta + K1_theta / 2, g, S, C_xa, rho, m);
    K2_theta = dt * dtheta_dt(V + K1_V / 2, theta + K1_theta / 2, g);

    K3_x     = dt *     dx_dt(V + K2_V / 2, theta + K2_theta / 2);
    K3_y     = dt *     dy_dt(V + K2_V / 2, theta + K2_theta / 2);
    K3_V     = dt *     dV_dt(V + K2_V / 2, theta + K2_theta / 2, g, S, C_xa, rho, m);
    K3_theta = dt * dtheta_dt(V + K2_V / 2, theta + K2_theta / 2, g);

    K4_x     = dt *     dx_dt(V + K3_V, theta + K3_theta);
    K4_y     = dt *     dy_dt(V + K3_V, theta + K3_theta);
    K4_V     = dt *     dV_dt(V + K3_V, theta + K3_theta, g, S, C_xa, rho, m);
    K4_theta = dt * dtheta_dt(V + K3_V, theta + K3_theta, g);

    x = x + (K1_x + 2 * K2_x + 2 * K3_x + K4_x) / 6;
    y = y + (K1_y + 2 * K2_y + 2 * K3_y + K4_y) / 6;
    V = V + (K1_V + 2 * K2_V + 2 * K3_V + K4_V) / 6;
    theta = theta + (K1_theta + 2 * K2_theta + 2 * K3_theta + K4_theta) / 6;
end