%% orbit determination
clear all
rad = pi/180;
R_E = 6371.2;

% gravity field coefficients up to degree 2
sh_coeff =[1
    -1.418260617806000e-09
    -4.841692151273000e-04
    -1.267187171440000e-09
    5.423985447263000e-10
    -2.349664289448000e-10
    1.467056166745000e-09
    2.439338908279000e-06
    -1.400275900305000e-06];

orbit.R = 6.378136600000000e+06; % reference radius of Earth in m
orbit.GM = 3.986004415000000e+14; % gravitational constant times mass of Earth in km^3/s^2

% parameters defining the orbit
orbit.ra = orbit.R + 5000e3; % radius of apogee in m 
orbit.rp = orbit.R + 200e3; % radius of perigee in m

% orbit.ra = orbit.R + 140e3; % radius of apogee in m 
% orbit.rp = orbit.R + 1500e3; % radius of perigee in m

orbit.i = 88; % orbit inclination in degrees
orbit.ta = md2000(2020,1,1,0,0,0); % start time
orbit.tb = md2000(2023,1,1,0,0,0); % end time

orbit.omega = 90; % argument of perigee in degrees at ta
orbit.Omega = 0; % right ascension of ascending node in degrees at ta
orbit.M = 0; % mean anomaly in radians at ta

orbit.dt = 30; % sampling rate in sec
t = [orbit.ta:orbit.dt/86400:orbit.tb]; % time instants
orbit.N = length(t); % number of data points

orbit.e = (orbit.ra-orbit.rp)/(orbit.ra+orbit.rp); % orbit eccentricity
orbit.a = orbit.ra/(1+orbit.e); % semi-major axis in m
orbit.n = sqrt(orbit.GM/orbit.a^3); % mean motion

% simple rotation matrix (from celestial to Earth-fixed frame)
% md2000 = t;
theta = mod(2*pi*(0.7790572732640 + mod(t - .5,1) + mod(0.00273781191135448 * (t - .5),1)), 2*pi);

rot = [cos(theta)
    -sin(theta)
    zeros(size(theta))
    sin(theta)
    cos(theta)
    zeros(size(theta))
    zeros(size(theta))
    zeros(size(theta))
    ones(size(theta))];

% integrate orbit - using ICRF->ITRF rotation matrix rot
integration_order = 12; % order of the Gauss-Jackson integrator (12 appears to work well)

[r_ICRF,v] = orbit_integrator(orbit.a, orbit.e, orbit.i*pi/180, orbit.omega*pi/180, orbit.Omega*pi/180, orbit.M*pi/180, ...
    (orbit.ta - md2000(1980, 1, 1))*86400, orbit.dt, orbit.N, orbit.GM, orbit.R, 2, sh_coeff, integration_order, rot);

r_ITRF = [[rot(1,:).*r_ICRF(1,:) + rot(2,:).*r_ICRF(2,:)]
    [rot(4,:).*r_ICRF(1,:) + rot(5,:).*r_ICRF(2,:)]
    r_ICRF(3,:)];

[phi, lat, r] = cart2sph(r_ITRF(1,:), r_ITRF(2,:), r_ITRF(3,:));
phi = phi/rad; theta = 90-lat/rad; r = r/1e3;

R = orbit.R/1e3; % reference radius in km

% orbital period T_orb (in minutes)
idx = (90-theta(2:end)).*(90-theta(1:end-1)) < 0;
T_orb = mean(gradient(t(idx)))*24*60*2
T_orb = 2*pi/orbit.n/60

%% 3D plot of selected data points 
idx = 200000 + [1:10000]';
plot3(r_ITRF(1,idx)/1e3, r_ITRF(2,idx)/1e3, r_ITRF(3,idx)/1e3)
axis equal
hold on
[X,Y,Z] = sphere(36);
surf(X*R,Y*R,Z*R)
hold off

%% long-lat plot of selected data points
idx =  + [1:5000]';
plot(phi(idx), 90-theta(idx), '.')
load coast
hold on
plot(long, lat, '-k')
hold off
set(gca, 'XTick', -180:60:180, 'YTick', -90:30:90)
axis([-180 180 -90 90])

%% animation, %% long-lat plot of selected data points
for k = 1:1000
plot(phi(1:k), 90-theta(1:k), '.b')
load coast
hold on
plot(long, lat, '-k')
set(gca, 'XTick', -180:60:180, 'YTick', -90:30:90)
axis([-180 180 -90 90])
pause(0)      
end
hold off


%% long/lat distribution of low altitude data
idx = r - R < 250;
histogram2(phi(idx), theta(idx), 'XBinEdges', [-180:2:180], 'YBinEdges', [0:2:180])
plot(t(idx)/365.25+2000, theta(idx), '.')

