clc;
clear;
close all;
format long
warning('off','all')

% Constants
G = 6.67430e-20; % Gravitational constant in km^3/(kg * s^2)
m1 = 5.97219e24; % Mass of the Earth in kg
RE = 6378.12; % Earth's radius in km
mu = 398600.44189; % Earth's gravitational parameter in km^3/s^2
Cd = 2.2; % Drag coefficient
A = 2.120773439350248; % Cross-sectional area of the satellite in square meters

% % Initial conditions(curcle)
% r0 = [0 7000.000000000003 0]; % Initial position vector in km
% v0 = [-7.546053287267834 0 0]; % Initial velocity vector in km/s
% Y0 = [r0 v0]; % Initial state vector


% Initial conditions(sun)
r0 = [6302.808897237966 -2994.790829642488 -14.962698827009]; % Initial position vector in km
v0 = [-0.421864841013 -0.925270374649 7.489140887101]; % Initial velocity vector in km/s
Y0 = [r0 v0]; % Initial state vector

% % Initial conditions(test)
% r0 = [1112.99423149112 6487.00576850888  3800]; % Initial position vector in km
% v0 = [-6.334136715824456 -1.086766048584727 3.710451382204591]; % Initial velocity vector in km/s
% Y0 = [r0 v0]; % Initial state vector


% Time settings
t0 = 0; % Initial time in seconds
tf = 86400; % Final time in seconds (1 day)

% Propagate the orbit using 4th order Runge-Kutta method
[t, Y] = ode4(@relative_motion_drag_J2, t0, 1, tf, Y0);

% Extract position and velocity vectors
rvec = Y(:, 1:3);
vvec = Y(:, 4:6);

% Calculate altitude
rmag = vecnorm(rvec, 2, 2);
altitude = rmag - RE;

% Initialize arrays for orbital elements
eccentricity = zeros(length(t), 1);
semi_major_axis = zeros(length(t), 1);
inclination = zeros(length(t), 1);
RAAN = zeros(length(t), 1);
arg_perigee = zeros(length(t), 1);
true_anomaly = zeros(length(t), 1);

% Unit vectors
K = [0; 0; 1]; % Unit vector along the Z-axis
I_hat = [1; 0; 0]; % Unit vector along the X-axis
e_vec_array = zeros(length(t), 3);
for i = 1:length(t)
    r_i = rvec(i, :)';
    v_i = vvec(i, :)';
    
    % Eccentricity vector and magnitude
    h_vec = cross(r_i, v_i);
    e_vec = (cross(v_i, h_vec) / mu) - (r_i / norm(r_i));
    eccentricity(i) = norm(e_vec);
    % Save eccentricity vector into array
    e_vec_array(i, :) = e_vec';
    % Specific orbital energy and semi-major axis
    xi = (norm(v_i)^2 / 2) - (mu / norm(r_i));
    semi_major_axis(i) = -mu / (2 * xi);
    
    % Specific angular momentum and inclination
    h_vec = cross(r_i, v_i);
    inclination(i) = acos(dot(K, h_vec) / norm(h_vec));
    
    % Node vector and RAAN
    n_vec = cross(K, h_vec);
    raan_test(i) = (dot(n_vec',I_hat'));
    
    if norm(n_vec) ~= 0
        RAAN(i) = acos(dot(I_hat, n_vec) / norm(n_vec));
        if  n_vec(2) < 0
            RAAN(i) = 2*pi - RAAN(i);
        end
    else
        RAAN(i) = 0; % Edge case where orbit is equatorial
    end

    aop_test(i) = dot(e_vec',K');
    % Argument of Perigee
    if norm(n_vec) ~= 0 && eccentricity(i) - 0<=1e-10
        arg_perigee(i) = acos(dot(n_vec, e_vec) / (norm(n_vec) * eccentricity(i)));
        if e_vec(3) < 0
            arg_perigee(i) = 2*pi - arg_perigee(i);
        end
    else
        arg_perigee(i) = 0; % Undefined for circular orbits
    end
    
    % True Anomaly
    if eccentricity(i) > 0
        true_anomaly(i) = acos(dot(e_vec, r_i) / (eccentricity(i) * norm(r_i)));
        if dot(r_i, v_i) < 0
            true_anomaly(i) = 2*pi - true_anomaly(i);
        end
    
    end
end
% Extract the components of the eccentricity vectors
e_x = e_vec_array(:, 1);
e_y = e_vec_array(:, 2);
e_z = e_vec_array(:, 3);





% Convert to degrees
inclination = rad2deg(inclination);
RAAN = rad2deg(RAAN);
arg_perigee = rad2deg(arg_perigee);
true_anomaly = rad2deg(true_anomaly);

fprintf('First Orbit Eccentricity: %.12f\n', eccentricity(1));
fprintf('First Semi-Major Axis: %.12f km\n', semi_major_axis(1));
fprintf('First Inclination: %.12f degrees\n', inclination(1));
fprintf('First RAAN: %.12f degrees\n', RAAN(1));
fprintf('First Argument of Perigee: %.12f degrees\n', arg_perigee(1));
fprintf('First True Anomaly: %.12f degrees\n', true_anomaly(1));

% Plot altitude over time
figure;


% Plot eccentricity over time
subplot(6,1,1);
plot(t / 86400, eccentricity);
xlabel('Time (days)');
ylabel('Eccentricity');
title('Eccentricity over Time');
grid on;

% Plot semi-major axis over time
subplot(6,1,2);
plot(t / 86400, semi_major_axis);
xlabel('Time (days)');
ylabel('Semi-Major Axis (km)');
title('Semi-Major Axis over Time');
grid on;

% Plot inclination over time
subplot(6,1,3);
plot(t / 86400, inclination);
xlabel('Time (days)');
ylabel('Inclination (degrees)');
title('Inclination over Time');
grid on;

% Plot RAAN over time
subplot(6,1,4);
plot(t / 86400, RAAN);
xlabel('Time (days)');
ylabel('RAAN (degrees)');
title('RAAN over Time');
grid on;

% Plot Argument of Perigee over time
subplot(6,1,5);
plot(t / 86400, arg_perigee);
xlabel('Time (days)');
ylabel('Argument of Perigee (degrees)');
title('Argument of Perigee over Time');
grid on;

% Plot True Anomaly over time
subplot(6,1,6);
plot(t / 86400, true_anomaly);
xlabel('Time (days)');
ylabel('True Anomaly (degrees)');
title('True Anomaly over Time');
grid on;

% % Create a 3D plot of the orbit
figure;
[xx, yy, zz] = sphere(200);
surf(RE * xx, RE * yy, RE * zz)
colormap(light_blue)
clim([-RE / 100 RE / 100])
shading interp

% Draw and label the X, Y, and Z axes
line([0 2 * RE], [0 0], [0 0]);
text(2 * RE + RE / 10, 0, 0, 'x')
line([0 0], [0 2 * RE], [0 0]);
text(0, 2 * RE + RE / 10, 0, 'y')
line([0 0], [0 0], [0 2 * RE]);
text(0, 0, 2 * RE + RE / 10, 'z')

hold on
plot3(rvec(:, 1), rvec(:, 2), rvec(:, 3), 'b')
plot3(rvec(1, 1), rvec(1, 2), rvec(1, 3), 'go'); % Starting point
plot3(rvec(end, 1), rvec(end, 2), rvec(end, 3), 'ro'); % Ending point
% Create a 3D plot

scale_factor = 1e13; % Adjust this factor as needed to visualize the vectors clearly
quiver3(zeros(length(e_x),1), zeros(length(e_y),1), zeros(length(e_z),1), e_x*scale_factor, e_y*scale_factor, e_z*scale_factor, 'r');



% Optionally, plot the orbit vectors for context
plot3(rvec(:,1), rvec(:,2), rvec(:,3), 'b');
% Set graph properties
grid on
axis equal
xlabel('km')
ylabel('km')
zlabel('km')
title('3D Orbit');
legend;

% Custom colormap function
function map = light_blue
    r = 0.8;
    g = r;
    b = 1.0;
    map = [r g b; 0 0 0; r g b];
end

% Function to compute the derivatives of the state vector
function Ydot = relative_motion_drag_J2(~, Y)
    % Earth's J2 coefficient and gravitational parameter
    J2 = 0;
    mu = 398600.44189; % km^3/s^2
    RE = 6378.12; % Earth's radius in km
    
    % Extract position and velocity vectors
    rvector = Y(1:3);
    vvector = Y(4:6);
    
    % J2 perturbation calculation
    r = norm(rvector);  
    Z = rvector(3);
    Z2 = Z^2;
    r2 = r^2;
    ax_J2 = (-mu * rvector(1) / r^3) * (1 - (3 / 2) * J2 * (RE^2 / r2) * (5 * Z2 / r2 - 1));
    ay_J2 = (-mu * rvector(2) / r^3) * (1 - (3 / 2) * J2 * (RE^2 / r2) * (5 * Z2 / r2 - 1));
    az_J2 = (-mu * rvector(3) / r^3) * (1 - (3 / 2) * J2 * (RE^2 / r2) * (5 * Z2 / r2 - 3));
    a_J2 = [ax_J2; ay_J2; az_J2];
    
    % Total acceleration
    a_total = a_J2;
    
    % Output derivative of state vector
    Ydot = [vvector a_total'];
end