%% Kepler to Cartesian Coordinate Change
% Author: Jack Naylor
% SID: 470354333

% Description:
% This function converts keplerian coordinates to cartesian coordinates for
% plotting in MATLAB.

% Inputs:
% a = semi-major axis (m)
% e = eccentricity
% i = inclination (deg)
% omega = argument (deg)
% bigOmega = argument of the perigee (deg)
% T = period (s)
% t = time (s)

% Outputs:
% initialCart = 6x1 vector of [x, xVelocity, y, yVelocity, z, zVelocity] in
% m and m s^-1 respectively.

function initialCart = keplerToCart(a,e,i,omega,bigOmega,T,t)
%% Setup
% Load global variables
global mu

% Define tolerance
tol = 1e-12;

%% Calculation
% Calculate mean anomaly
M = rad2deg(sqrt(mu/a^3)*(t-T));

% Iteratively calculate the eccentric anomaly
E = eccentricAnomaly(e,M,tol);

% Calculate true anomaly
v = 2*atand(sqrt((1+e)/(1-e))*tand(E/2));

% Calculate radius
r = a*(1-e*cosd(E));

% Calculate specific angular momentum
h = sqrt(mu*a*(1-e^2));

% p
p = a*(1-e^2);

% Calculate cartesian positions
x = r*(cosd(bigOmega)*cosd(omega+v)-sind(bigOmega)*sind(omega+v)*cosd(i));
y = r*(sind(bigOmega)*cosd(omega+v)+cosd(bigOmega)*sind(omega+v)*cosd(i));
z = r*(sind(i)*sind(omega+v));

% Calculate cartesian velocities
xdot = x*h*e/(r*p)*sind(v)-h/r*(cosd(bigOmega)*sind(omega+v)+sind(bigOmega)*cosd(omega+v)*cosd(i));
ydot = y*h*e/(r*p)*sind(v)-h/r*(sind(bigOmega)*sind(omega+v)+cosd(bigOmega)*cosd(omega+v)*cosd(i));
zdot = z*h*e/(r*p)*sind(v)+h/r*sind(i)*cosd(omega+v);

%% Output
% Output positions and velocities in a vector
initialCart = [x, xdot, y, ydot, z, zdot];
end