% orekitLambertTest.m
%
% Last modified: 21 January 2021
%--------------------------------------------------------------------

close all
clear;

% Constants
deg2Rd = pi / 180.0;
mu     = 398600.4415;
twoPi  = 2.0 * pi;

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-10.2.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.estimation.iod.IodLambert.*;
import org.orekit.estimation.iod.IodLambert.estimate.*;
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)

inertialFrame = FramesFactory.getEME2000();
utc           = TimeScalesFactory.getUTC();
initialDate   = AbsoluteDate(2018, 11, 1, 0, 0, 0.0, utc);

% Case 1
pos1_km   = Vector3D(22592.145603, -1599.915239, -19783.950506);

fprintf('Initial Position:\n');
fprintf('       rECI (km): %14.6f %14.6f %14.6f\n',pos1_km.toArray);

pos2_km   = Vector3D(1922.067697, 4054.157051, -8925.727465);

fprintf('Final Position:\n');
fprintf('       rECI (km): %14.6f %14.6f %14.6f\n',pos2_km.toArray);

transferAngle = cmpang(pos1_km.toArray, pos2_km.toArray);

fprintf('Transfer Angle (deg): %14.6f\n',transferAngle/deg2Rd);

% Time of flight in seconds
tof_s     = 36000.0;
finalDate = AbsoluteDate(initialDate, tof_s, utc);

iod1        = org.orekit.estimation.iod.IodLambert(mu);
% Posigrade
posigrade   = true;
nRev        = 0;
kepOrbitOut = iod1.estimate(inertialFrame,posigrade,nRev,pos1_km, ...
                            initialDate,pos2_km,finalDate);

vel2_km_s   = kepOrbitOut.getPVCoordinates.getVelocity();

fprintf('NRev = 0; Posigrade; Final Velocity:\n');
fprintf('     vECI (km/s): %17.9f %17.9f %17.9f\n\n',vel2_km_s.toArray);

% Retrograde
posigrade   = false;
kepOrbitOut = iod1.estimate(inertialFrame,posigrade,nRev,pos1_km, ...
                            initialDate,pos2_km,finalDate);

vel2_km_s   = kepOrbitOut.getPVCoordinates.getVelocity();

fprintf('NRev = 0; Retrograde; Final Velocity:\n');
fprintf('     vECI (km/s): %17.9f %17.9f %17.9f\n\n',vel2_km_s.toArray);

% Posigrade
posigrade   = true;
nRev        = 1;
kepOrbitOut = iod1.estimate(inertialFrame,posigrade,nRev,pos1_km, ...
                            initialDate,pos2_km,finalDate);

vel2_km_s   = kepOrbitOut.getPVCoordinates.getVelocity();

fprintf('NRev = 1; Posigrade; Final Velocity:\n');
fprintf('     vECI (km/s): %17.9f %17.9f %17.9f\n\n',vel2_km_s.toArray);


% Posigrade
posigrade   = false;
kepOrbitOut = iod1.estimate(inertialFrame,posigrade,nRev,pos1_km, ...
                            initialDate,pos2_km,finalDate);

vel2_km_s   = kepOrbitOut.getPVCoordinates.getVelocity();

fprintf('NRev = 1; Retrograde; Final Velocity:\n');
fprintf('     vECI (km/s): %17.9f %17.9f %17.9f\n\n',vel2_km_s.toArray);

