clear;
clc;

%% Parameters
params.Cd = 0.40;
params.A  = 1.2;
params.m  = 900;
params.LD = 2.5;
params.re = 6371000;

params.rhoFcn = @atmosphericDensity;
params.g = 9.80665;

% constant bank angle (or 0 for pure glide)
params.sigmaFcn = @(t,X) 0;

%% Initial conditions (2D model)
X = [
    6500;              % v
    deg2rad(-5);       % gamma
    0;                 % Psi
    80000              % h
];

%% Time
dt = 0.1;
tf = 2500;
N = floor(tf/dt);

time  = zeros(N,1);
state = zeros(N,4);

%% Integration
for i = 1:N

    t = (i-1)*dt;

    time(i) = t;
    state(i,:) = X';

    if X(4) <= 0
        time = time(1:i);
        state = state(1:i,:);
        break;
    end

    X = rk4Step(t, X, dt, params);

end

%% Extract
v = state(:,1);
gamma = rad2deg(state(:,2));
Psi = state(:,3);
h = state(:,4);

downrange = params.re * Psi / 1000;

%% Plot
figure;
plot(time, h/1000);
grid on;
xlabel('Time (s)');
ylabel('Altitude (km)');
title('Altitude');

figure;
plot(downrange, h/1000);
grid on;
xlabel('Downrange (km)');
ylabel('Altitude (km)');
title('Trajectory');