% orekitNumPropTest.m
%
% Last modified: 20 June 2019
%--------------------------------------------------------------------

close all
clear;
%clear java

% Constants
deg2Rd     = pi / 180.0;
mu_m3_sec2 =  3.986004415e+14;
twoPi      = 2.0 * pi;

%loaded = javaclasspath('-dynamic');
javaaddpath(fullfile('\hipparchus_1_7\hipparchus-core-1.7.jar'));
javaaddpath(fullfile('\hipparchus_1_7\hipparchus-filtering-1.7.jar'));
javaaddpath(fullfile('\hipparchus_1_7\hipparchus-fitting-1.7.jar'));
javaaddpath(fullfile('\hipparchus_1_7\hipparchus-geometry-1.7.jar'));
javaaddpath(fullfile('\hipparchus_1_7\hipparchus-ode-1.7.jar'));
javaaddpath(fullfile('\hipparchus_1_7\hipparchus-optim-1.7.jar'));
javaaddpath(fullfile('\hipparchus_1_7\hipparchus-stat-1.7.jar'));
%javaaddpath(fullfile('orekit-9.3.1.jar'));
%javaaddpath(fullfile('orekit-10.0.jar'));
javaaddpath(fullfile('orekit-10.1.jar'));

% From java
import java.lang.Math;
import java.lang.System;
import java.util.Calendar;
import java.util.Date;
import java.util.GregorianCalendar;
import java.util.TimeZone;
import java.io.File;

% From the Hipparchus Math Project
import org.hipparchus.geometry.euclidean.threed.*;
import org.hipparchus.util.*;
import org.hipparchus.ode.nonstiff.*;

% From the ORbit Extrapolation KIT
import org.orekit.attitudes.*;
import org.orekit.bodies.*;
import org.orekit.data.*;
import org.orekit.data.DataProvidersManager.*;
import org.orekit.data.ZipJarCrawler.*;
import org.orekit.errors.*;
import org.orekit.forces.*;
import org.orekit.forces.drag.*;
import org.orekit.forces.gravity.*;
import org.orekit.forces.gravity.potential.*;
import org.orekit.forces.maneuver.*;
import org.orekit.forces.maneuver.ImpulseManeuver.*;
import org.orekit.forces.radiation.*;
import org.orekit.frames.*;
import org.orekit.orbits.*;
import org.orekit.orbits.Orbit.*;
import org.orekit.orbits.OrbitType.*;
import org.orekit.orbits.PositionAngle.*;
import org.orekit.propagation.*;
import org.orekit.propagation.analytical.*;
import org.orekit.propagation.events.*;
import org.orekit.propagation.events.NodeDetector.*;
import org.orekit.propagation.numerical.*;
import org.orekit.time.*;
import org.orekit.tle.*;
import org.orekit.utils.*;

DM      = org.orekit.data.DataProvidersManager.getInstance();
crawler = org.orekit.data.DirectoryCrawler(File([cd ,'\OrekitData\']));

DM.clearProviders()
DM.addProvider(crawler)

mu1 = CelestialBodyFactory.getEarth().getGM();
mu2 = Constants.WGS84_EARTH_MU;

longitude_r = FastMath.toRadians(45.0);
latitude_r  = FastMath.toRadians(25.0);
altitude_m  = 0.0;
station     = GeodeticPoint(latitude_r, longitude_r, altitude_m);

lat_deg = station.getLatitude / deg2Rd;
lon_deg = station.getLongitude / deg2Rd;
alt_m   = station.getAltitude;
fprintf('   Latitude (deg): %15.8f\n',lat_deg);
fprintf('  Longitude (deg): %15.8f\n',lon_deg);
fprintf('Altitude (meters): %15.8f\n\n',alt_m);

earthFrame    = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
earth         = OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS, ...
                                 Constants.WGS84_EARTH_FLATTENING,earthFrame);
staF          = TopocentricFrame(earth, station, "station");

maxCheck  = 60.0;
threshold = 0.001;
elevation = FastMath.toRadians(5.0);
%staFVis   = ElevationDetector(maxCheck, threshold, staF); %.withConstantElevation(elevation).withHandler(VisibilityHandler());

utc           = TimeScalesFactory.getUTC();

position_m    = Vector3D(6199504.105, 2918683.471, -11054.708);
velocity_m_s  = Vector3D(426.864, -878.040, 7564.256);
pvCoordinates = PVCoordinates(position_m, velocity_m_s);
initialDate   = AbsoluteDate(2018,10,17,0,0,0.000, utc);
inertialFrame = FramesFactory.getEME2000();

initialOrbit = KeplerianOrbit(pvCoordinates,inertialFrame,initialDate,mu_m3_sec2);
initialState = SpacecraftState(initialOrbit);
initPer_sec  = initialState.getKeplerianPeriod();

pv         = initialState.getPVCoordinates();
pos_m      = pv.getPosition();
posM_m     = pos_m.toArray;
posMag_m   = pos_m.getNorm();
vel_m_s    = pv.getVelocity();
velM_m_s   = vel_m_s.toArray;
velMag_m_s = vel_m_s.getNorm();

fprintf('Initial State at: %s\n',initialState.getDate());
fprintf('        P (km): %14.6f %14.6f %14.6f\n',posM_m/1000.0);
fprintf('    P Mag (km): %14.6f\n',posMag_m/1000.0);
fprintf('    V (km/sec): %14.8f %14.8f %14.8f\n',velM_m_s/1000.0);
fprintf('V Mag (km/sec): %14.8f\n\n',velMag_m_s/1000.0);

sma_m     = initialState.getA;
ecc       = initialState.getE;
inc_r     = initialState.getI;
hX        = initialState.getHx;
hY        = initialState.getHy;
raan_r    = atan2(hY,hX);
raan_r    = mod(raan_r,twoPi);
eccX      = initialState.getEquinoctialEx;
eccY      = initialState.getEquinoctialEy;
omega_r   = atan2(eccY,eccX) - raan_r;
omega_r   = mod(omega_r,twoPi);
trueLat_r = initialState.getLv;
trueLat_r = mod(trueLat_r,twoPi);
nu_r      = trueLat_r - (omega_r + raan_r);
nu_r      = mod(nu_r,twoPi);
meanLat_r = initialState.getLM;
meanLat_r = mod(meanLat_r,twoPi);
MA_r      = meanLat_r - (omega_r + raan_r);
MA_r      = mod(MA_r,twoPi);
fprintf('Initial J2000 Kepler elements:\n');
fprintf('        Semi-major axis (km): %15.8f\n',sma_m/1000.0);
fprintf('                Eccentricity: %15.8f\n',ecc);
fprintf('           Inclination (deg): %15.8f\n',inc_r/deg2Rd);
fprintf('       Right Ascension (deg): %15.8f\n',raan_r/deg2Rd);
fprintf('   Argument of Perigee (deg): %15.8f\n',omega_r/deg2Rd);
%fprintf('          True Anomaly (deg): %15.8f\n',nu/deg2Rd);
fprintf('          Mean Anomaly (deg): %15.8f\n\n',MA_r/deg2Rd);

kepler = KeplerianPropagator(initialOrbit);

kepler.setSlaveMode();

periodMin  = periodKm(sma_m/1000.0);
%duration   = 600.0;     % seconds
duration   = floor(periodMin*60);
finalDate  = initialDate.shiftedBy(duration);
stepT      = 60.0;      % seconds
cpt        = 1;
extrapDate = initialDate;
delT       = finalDate.durationFrom(initialDate);
P          = zeros(3,95);
fprintf('\n');
while (extrapDate.compareTo(finalDate) <= 0)
      currentState = kepler.propagate(extrapDate);
      fprintf('step %0d: time %s %s\n', cpt, char(currentState.getDate()), ...
              char(currentState.getOrbit()));
      coord      = currentState.getPVCoordinates.getPosition;
      P(:,cpt)   = [coord.getX coord.getY coord.getZ]';
      extrapDate = AbsoluteDate(extrapDate, stepT, utc);
      cpt        = cpt + 1;
end

figure;
plot3(P(1,:),P(2,:),P(3,:));
grid on;

%kepler.addEventDetector(staFVis.withConstantElevation(elevation));

%**************************************************************************
% Using numerical propagator

fprintf('Initial State at %s\n',initialState.getDate);
fprintf('    P (km): %14.5f %14.5f %14.5f\n',posM_m/1000.0);
fprintf('V (km/sec): %14.5f %14.5f %14.5f\n',velM_m_s/1000.0);

% Adaptive step integrator
%  with a minimum step of 0.001 and a maximum step of 1000
minStep = 0.001;
maxStep = 500.0;
positionTolerance = 1.0;
propagationType   = OrbitType.KEPLERIAN;
tolerances   = NumericalPropagator.tolerances(positionTolerance, ...
                                             initialOrbit,propagationType);
absTolerance = tolerances(1,1);
relTolerance = tolerances(2,1);
% Variable Step Dormand Prince Integration
integrator = DormandPrince853Integrator(minStep,maxStep,absTolerance, ...
                                        relTolerance);
propagator = NumericalPropagator(integrator);
propagator.setOrbitType(propagationType);

provider           = GravityFieldFactory.getNormalizedProvider(10, 10);
holmesFeatherstone = HolmesFeatherstoneAttractionModel(FramesFactory.getITRF(IERSConventions.IERS_2010, ...
                                                       true), provider);

propagator.addForceModel(holmesFeatherstone);

propagator.setEphemerisMode();

propagator.setInitialState(initialState);

%finalState    = propagator.propagate(AbsoluteDate(initialDate, 14400.0));
finalState    = propagator.propagate(initialDate.shiftedBy(14400.0));
ephemerisOut  = propagator.getGeneratedEphemeris();
orbitFinal    = finalState.getOrbit;
argP_d        = FastMath.toDegrees(orbitFinal.getPerigeeArgument());
argP_d        = mod(argP_d,360.0);
trueAnomaly_d = FastMath.toDegrees(orbitFinal.getTrueAnomaly());
trueAnomaly_d = mod(trueAnomaly_d,360);
fprintf('%s %12.3f %10.8f %10.6f %10.6f %10.6f %10.6f\n', ...
        finalState.getDate(),orbitFinal.getA(),orbitFinal.getE(), ...
        FastMath.toDegrees(orbitFinal.getI()), ...
        argP_d, ...
        FastMath.toDegrees(orbitFinal.getRightAscensionOfAscendingNode()), ...
        trueAnomaly_d);

pv       = finalState.getPVCoordinates();
pos_m    = pv.getPosition();
posM_m   = pos_m.toArray;
vel_m_s  = pv.getVelocity();
velM_m_s = vel_m_s.toArray;

fprintf('\nFinal State at %s\n',finalState.getDate);
fprintf('    P (km): %14.5f %14.5f %14.5f\n',posM_m/1000.0);
fprintf('V (km/sec): %14.5f %14.5f %14.5f\n',velM_m_s/1000.0);

smaF_m     = finalState.getA;
eccF       = finalState.getE;
incF_r     = finalState.getI;
hXF        = finalState.getHx;
hYF        = finalState.getHy;
raanF_r    = atan2(hYF,hXF);
raanF_r    = mod(raanF_r,twoPi);
eccXF      = finalState.getEquinoctialEx;
eccYF      = finalState.getEquinoctialEy;
omegaF_r   = atan2(eccYF,eccXF) - raanF_r;
omegaF_r   = mod(omegaF_r,twoPi);
trueLatF_r = finalState.getLv;
trueLatF_r = mod(trueLatF_r,twoPi);
nuF_r      = trueLatF_r - (omegaF_r + raanF_r);
nuF_r      = mod(nuF_r,twoPi);
meanLatF_r = finalState.getLM;
meanLatF_r = mod(meanLatF_r,twoPi);
MAF_r      = meanLatF_r - (omegaF_r + raanF_r);
MAF_r      = mod(MAF_r,twoPi);
 
fprintf('\nFinal J2000 Kepler elements:\n');
fprintf('        Semi-major axis (km): %15.8f\n',smaF_m/1000.0);
fprintf('                Eccentricity: %15.8f\n',eccF);
fprintf('           Inclination (deg): %15.8f\n',incF_r/deg2Rd);
fprintf('       Right Ascension (deg): %15.8f\n',raanF_r/deg2Rd);
fprintf('   Argument of Perigee (deg): %15.8f\n',omegaF_r/deg2Rd);
fprintf('          True Anomaly (deg): %15.8f\n',nuF_r/deg2Rd);
fprintf('          Mean Anomaly (deg): %15.8f\n\n',MAF_r/deg2Rd);

intermediateDate  = initialDate.shiftedBy(3600);
intermediateState = ephemerisOut.propagate(intermediateDate);
pvI       = intermediateState.getPVCoordinates();
posI_m    = pvI.getPosition();
posIM_m   = pos_m.toArray;
velI_m_s  = pvI.getVelocity();
velIM_m_s = vel_m_s.toArray;

fprintf('\nintermediate State at %s\n',intermediateState.getDate);
fprintf('    P (km): %14.5f %14.5f %14.5f\n',posIM_m/1000.0);
fprintf('V (km/sec): %14.5f %14.5f %14.5f\n',velIM_m_s/1000.0);
