clc;
clear;

% Define constants
mu = 398600.44189; % Earth's gravitational parameter in km^3/s^2
delta_t = 50 * 60; % Time in seconds (50 minutes)

% Initial position (r0) and velocity (v0) vectors in km and km/s
r0 = [-5102; -8228; -2105]; % in km
v0 = [-4.348; 3.478; -2.846]; % in km/s

% Calculate the magnitude of r0 and v0
r0_mag = norm(r0);
v0_mag = norm(v0);

% Calculate specific angular momentum vector (h = r0 x v0)
h = cross(r0, v0);
h_mag = norm(h);

% Calculate the eccentricity vector (e_vec)
e_vec = (cross(v0, h) / mu) - (r0 / r0_mag);
e = norm(e_vec);

% Calculate the semi-major axis (a)
energy = (v0_mag^2 / 2) - (mu / r0_mag);
a = -mu / (2 * energy);

% Calculate the mean motion (n)
n = sqrt(mu / a^3);

% Calculate the initial eccentric anomaly (E0)
E0 = atan2(dot(r0, v0) / sqrt(mu * a), 1 - r0_mag / a);

%tp
t_p = 0 - (1/n)*(E0-e*sin(E0));
% Calculate the mean anomaly at time t0 (M0)
M0 = E0 - e * sin(E0);

% Propagate the mean anomaly forward in time
M = M0 + n * delta_t;

% Solve Kepler's Equation for eccentric anomaly (E) using Newton's method
% Define the function for Kepler's equation
kepler_eq = @(E) E - e * sin(E) - M;

% Use fsolve to solve for E
options = optimoptions('fsolve', 'Display', 'none', 'TolFun', 1e-16); % Set options for fsolve
E = fsolve(kepler_eq, E0, options); % Initial guess is M

% Calculate the true anomaly (theta)
theta = 2 * atan2(sqrt(1 + e) * sin(E / 2), sqrt(1 - e) * cos(E / 2));

% Calculate the unit vectors P_hat and Q_hat
% First, calculate the eccentricity vector and normalize it
P_hat = e_vec / norm(e_vec); % P_hat is along the eccentricity vector

% Q_hat is perpendicular to P_hat and lies in the orbital plane
Q_hat = cross(h, P_hat);
Q_hat = Q_hat / norm(Q_hat); % Normalize Q_hat

% Calculate the new position vector (r) in the orbital plane
r = a * (cos(E) - e) * [1; 0; 0] + a * sqrt(1 - e^2) * sin(E) * [0; 1; 0];

% Rotate the position vector to the correct frame using P_hat and Q_hat vectors
r_new = r(1) * P_hat + r(2) * Q_hat;

% Calculate the new velocity vector (v) in the orbital plane
v_r = sqrt(mu / a) / (1 - e * cos(E)) * [-sin(E); sqrt(1 - e^2) * cos(E)];

% Rotate the velocity vector to the correct frame
v_new = v_r(1) * P_hat + v_r(2) * Q_hat;

% Output the new position and velocity
fprintf('New position vector at t0 + 50 min: [%.16f, %.16f, %.16f] km\n', r_new(1), r_new(2), r_new(3));
fprintf('New velocity vector at t0 + 50 min: [%.16f, %.16f, %.16f] km/s\n', v_new(1), v_new(2), v_new(3));

New position vector at t0 + 50 min: [-4198.4050237011451827, 7856.0510382690445113, -3199.1762864211327724] km
New velocity vector at t0 + 50 min: [4.9516901680966505, 3.4821245761681716, 2.4946563526752157] km/s