% from Cartesian state vector to orbital elements and viceversa
clear all
clc
close all

%%%Data from
%%% http://spaceflight.nasa.gov/realdata/sightings/SSapplications/Post/JavaSSOP/orbit/ISS/SVPOST.html
%%%[retrieved Feb. 14, 2016]
% Coasting Arc #1 (Orbit 2542)
%     ---------------------------------------
%  
%     Vector Time (GMT): 2016/043/12:00:00.000
%     Vector Time (MET): N/A
%     Weight (LBS)     : 922504.5
%  
%               M50 Cartesian                         M50 Keplerian  
%     -----------------------------------       --------------------------------
%      X    =       -2360510.86                 A    =         6775827.48  meter
%      Y    =       -3518121.42  meter          E    =           .0021176
%      Z    =       -5306312.78                 I    =           51.41132
%      XDOT =       6265.592107                 Wp   =           94.61517
%      YDOT =      -4393.802513  meter/sec      RA   =          324.21494  deg
%      ZDOT =        124.637340                 TA   =          176.58585
%                                               MA   =          176.57137
%                                               Ha   =            222.485  n.mi
%                                               Hp   =            211.425

%constants of the problem START%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
G = 6.6742*10^(-20); %km^3/(kg*s^2) gravitational constant
m1 = 5.972*10^24; %kg mass of Earth
mu = G*m1; %km^3/s^2 gravitational parameter
%constants of the problem END%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%FROM STATE VECTOR TO ORBITAL ELEMENTS (START)

%1) ISS
r0 =  [-2360510.86;  -3518121.42; -5306312.78]*10^(-3); %km
v0 =  [6265.592107;  -4393.802513; 124.637340]*10^(-3); %km/s

%2)
% r0 = [8000; 0; 6000]; %km 
% v0 = [0; 8.5; 0]; %km/s sqrt(mu/norm(r0))

%3)
% r0 = [10000; 0; 0]; %km 
% v0 = [0; -sqrt(mu/norm(r0)); 0]; %km/s sqrt(mu/norm(r0))

%4)
% r0 = [0; 10000; 0]; %km 
% v0 = [0; 0; -sqrt(mu/norm(r0))]; %km/s sqrt(mu/norm(r0))

h = cross(r0, v0) %km^2/s angular momentum vector
hnorm = norm(h)
arg_acos = h(3)/norm(h);
if norm(arg_acos) >= 1
    arg_acos = sign(arg_acos);
end
inclination = acos(arg_acos) %inclination
inclination_deg = inclination*180/pi
if inclination == 0 || inclination == pi
    node_line = [1;0;0]; %picking x axis
    RAAN = 0;   
else
    node_line = cross([0;0;1],h)/norm(cross([0;0;1],h));
    arg_acos = node_line(1);
    if norm(arg_acos) >= 1
        arg_acos = sign(arg_acos);
    end
    RAAN = acos(arg_acos); %right ascension of the ascending node
end
if node_line(2) < 0
    RAAN = 2*pi - RAAN
else
    RAAN
end
RAAN_deg = RAAN*180/pi
e = cross(v0, h)/mu - r0/norm(r0) %[-] eccentricity vector
enorm = norm(e) %printing norm of e vector
if enorm < 10^(-3)
    arg_acos = dot(node_line, r0)/norm(r0);
    if norm(arg_acos) >= 1
        arg_acos = sign(arg_acos);
    end
    arg_per = acos(arg_acos); % argument of perigee
    if r0(3) < 0
        arg_per = 2*pi - arg_per;
    else
        arg_per
    end
else
    arg_acos = dot(node_line, e)/enorm;
    if norm(arg_acos) >= 1
        arg_acos = sign(arg_acos);
    end
    arg_per = acos(arg_acos); % argument of perigee
    if e(3) < 0
        arg_per = 2*pi - arg_per;
    else
        arg_per
    end
end
arg_per_deg = arg_per*180/pi
rp = (norm(h)^2/mu)*(1/(1+norm(e))); %km perigee
ra = (norm(h)^2/mu)*(1/(1-norm(e))); %km apogee
if ra < 0
    ra = -ra;
end
a = (rp+ra)/2 %km semi-major axis
T = (2*pi/sqrt(mu))*a^(3/2) %s period
if enorm < 10^(-3)
    true_anomaly = 0
else
    arg_acos = dot(e, r0)/(enorm*norm(r0));
    if norm(arg_acos) >= 1
        arg_acos = sign(arg_acos);
    end
    true_anomaly = acos(arg_acos) % true anomaly
end
ur = r0/norm(r0);
if dot(v0, ur) < 0
    true_anomaly = 2*pi - true_anomaly;
else
    true_anomaly
end
true_anomaly_deg = true_anomaly*180/pi
%%%%%FROM STATE VECTOR TO ORBITAL ELEMENTS (END)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%FROM ORBITAL ELEMENTS TO STATE VECTOR (START)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
R1_i = [1 0 0; 0 cos(inclination) sin(inclination); 0 -sin(inclination) cos(inclination)];
R3_Om = [cos(RAAN) sin(RAAN) 0; -sin(RAAN) cos(RAAN) 0; 0 0 1];
R3_om = [cos(arg_per) sin(arg_per) 0; -sin(arg_per) cos(arg_per) 0; 0 0 1];

support_var = R3_om*R1_i*R3_Om;
x = support_var(1,1:3);
y = support_var(2,1:3);
z = support_var(3,1:3);

e_orb = enorm*x
h_orb = hnorm*z

rnorm = ((hnorm^2)/mu)*(1/(1+enorm*cos(true_anomaly)));
r = (rnorm*cos(true_anomaly)*x + rnorm*sin(true_anomaly)*y)'
u_r = r/norm(r);
u_per = (cross(z, u_r)/norm(cross(z, u_r)))';
v = (mu/hnorm)*enorm*sin(true_anomaly)*u_r + (mu/hnorm)*(1+enorm*cos(true_anomaly))*u_per
%%%%%FROM ORBITAL ELEMENTS TO STATE VECTOR (END)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
