clear all; clc;close all;
mu = 398600;               %[km^3/s^2] Earth’s gravitational parameter

R1 = [5887 -3520 -1204];   %[km]
R2 = [5572 -3457 -2376];   %[km]
R3 = [5088 -3289 -3480];   %[km]

r1 = norm(R1); r2 = norm(R2); r3 = norm(R3);
% Vector cross products
R12 = cross(R1,R2);
R23 = cross(R2,R3);
R31 = cross(R3,R1);

Nv  = r1*R23 + r2*R31 + r3*R12;
Dv  = R23 + R31 + R12;
Sv  = (r2-r3)*R1 + (r3-r1)*R2 + (r1-r2)*R3;

N   = norm(Nv);
D   = norm(Dv);
% Velocity vector
V1 = (mu/(N*D))^0.5*(cross(Dv,R1)/r1 + Sv);
[ h i RAAN e omega theta ] = State2Orbital(R1,V1);
OE = [h i RAAN e omega theta];

fprintf('h [km^2/s]    i [deg]     RAAN [deg]  e    omega[deg]   theta [deg] \n');
fprintf('%4.2f     %4.2f       %4.2f     %4.4f    %4.2f    %4.2f  \n',OE);
% Check, Only true anomaly
V2 = (mu/(N*D))^0.5*(cross(Dv,R2)/r2 + Sv);
[ h i RAAN e omega theta ] = State2Orbital(R2,V2);
OE = [h i RAAN e omega theta];

fprintf('h [km^2/s]    i [deg]     RAAN [deg]  e    omega[deg]   theta [deg] \n');
fprintf('%4.2f     %4.2f       %4.2f     %4.4f    %4.2f    %4.2f  \n',OE);

h [km^2/s]    i [deg]     RAAN [deg]  e    omega[deg]   theta [deg] 
52948.88     95.03       150.01     0.0127    151.69    38.30  
h [km^2/s]    i [deg]     RAAN [deg]  e    omega[deg]   theta [deg] 
52948.88     95.01       150.00     0.0127    151.69    48.31

EarthTopographicMap(1,820,420);
[Rv, alfa,delta ] = J2PropagR( h, i, RAAN, e,omega,theta) ;
scatter(alfa,delta,'.r');
text(270,-80,'smallsats.org','Color',[1 1 1], 'VerticalAlignment','middle',...
	'HorizontalAlignment','left','FontSize',14 );
title('Satellite ground track');
% Earth 3D Plot
Earth3DPlot(2);
scatter3(Rv(:,1),Rv(:,2),Rv(:,3),'.r');
% Plot position vectors
% Direction Cosines
l = R1/r1; m = R2/r2; n = R3/r3;
lv =(0:1:r1)'*l; mv =(0:1:r2)'*m;nv =(0:1:r3)'*n;
figure(3);
scatter3(Rv(:,1),Rv(:,2),Rv(:,3),'.k');
hold on;
scatter3(lv(:,1),lv(:,2),lv(:,3),'.r');
scatter3(mv(:,1),mv(:,2),mv(:,3),'.g');
scatter3(nv(:,1),nv(:,2),nv(:,3),'.b');
legend('Orbit','R1','R2','R3');
title('Satellite Orbit and R1,R2,R3 successive position vectors');
zoom(2)
% Equations from the book
% Orbital Mechanics for Engineering Students, 2nd Edition, Aerospace Engineering