import org.hipparchus.analysis.differentiation.Gradient;
import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
import org.hipparchus.util.FastMath;
import org.orekit.attitudes.AttitudeProvider;
import org.orekit.attitudes.LofOffset;
import org.orekit.forces.maneuvers.FieldImpulseManeuver;
import org.orekit.frames.FramesFactory;
import org.orekit.frames.LOFType;
import org.orekit.orbits.FieldKeplerianOrbit;
import org.orekit.orbits.PositionAngleType;
import org.orekit.propagation.FieldSpacecraftState;
import org.orekit.propagation.analytical.FieldKeplerianPropagator;
import org.orekit.propagation.events.FieldDateDetector;
import org.orekit.propagation.events.FieldEventDetector;
import org.orekit.time.AbsoluteDate;
import org.orekit.time.FieldAbsoluteDate;
import org.orekit.utils.Constants;

public class TestAD2 {
    
    public static void main(String[] args) {
        
        // load orekit-data here
        // ...

        final double MU = Constants.IERS2010_EARTH_MU;
        Gradient muDS = Gradient.constant(3, MU);

        // define initial orbit
        AbsoluteDate initialDate = AbsoluteDate.ARBITRARY_EPOCH;
        Gradient t0DS = Gradient.constant(3, 0.0);

        FieldAbsoluteDate<Gradient> initialDateDS = new FieldAbsoluteDate<>(initialDate, t0DS);

        double r0 = Constants.IERS2010_EARTH_EQUATORIAL_RADIUS + 500e3;
        Gradient r0DS = Gradient.constant(3, r0); // =: a

        Gradient eDS = Gradient.constant(3, 0.0);
        Gradient iDS = Gradient.constant(3, 0.0);
        Gradient paDS = Gradient.constant(3, 0.0);
        Gradient raanDS = Gradient.constant(3, 0.0);
        Gradient tanDS = Gradient.constant(3, 0.0);

        FieldKeplerianOrbit<Gradient> kep0DS = new FieldKeplerianOrbit<Gradient>(r0DS, eDS, iDS,
                paDS, raanDS, tanDS, PositionAngleType.TRUE, FramesFactory.getEME2000(), initialDateDS, muDS);

        // define final orbit (radius)
        double rf = Constants.IERS2010_EARTH_EQUATORIAL_RADIUS + 1000e3;

        // define theoretical initial delta-v (according to Hohmann transfer theory)
        double dv0 = FastMath.sqrt(2 * MU * ((1 / r0) - (1 / (r0 + rf)))) - FastMath.sqrt(MU / r0);

        // define simple (Keplerian) propagator
        FieldKeplerianPropagator<Gradient> propagator = new FieldKeplerianPropagator<>(kep0DS);

        double dt = 1.0;
        propagator.propagate(initialDateDS.shiftedBy(-dt));

        // configure propagator with initial maneuver
        FieldEventDetector<Gradient> triggerDS = new FieldDateDetector<>(initialDateDS);
        AttitudeProvider attitudeProvider = new LofOffset(FramesFactory.getEME2000(), LOFType.NTW);

        Gradient dvXDS = Gradient.variable(3, 0, 0.0);
        Gradient dvYDS = Gradient.variable(3, 1, dv0);
        Gradient dvZDS = Gradient.variable(3, 2, 0.0);

        FieldVector3D<Gradient> deltaVSatDS = new FieldVector3D<Gradient>(dvXDS, dvYDS, dvZDS);
        
        Gradient ispDS = Gradient.constant(3, 300.0);

        FieldImpulseManeuver<Gradient> impulse0 = new FieldImpulseManeuver<Gradient>(triggerDS,
                attitudeProvider, deltaVSatDS, ispDS);

        propagator.addEventDetector(impulse0);

        // propagate to final date
        double aT = 0.5*(r0 + rf);
        Gradient timeF = Gradient
                .constant(3, 0.5 * (2 * FastMath.PI / FastMath.sqrt(MU)) * FastMath.pow(aT, 1.5));
        FieldAbsoluteDate<Gradient> finalDate = new FieldAbsoluteDate<>(initialDateDS, timeF);

        FieldSpacecraftState<Gradient> finalState = propagator.propagate(finalDate);

        // compute final position components
        Gradient xF = finalState.getPVCoordinates().getPosition().getX();
        Gradient yF = finalState.getPVCoordinates().getPosition().getY();
        Gradient zF = finalState.getPVCoordinates().getPosition().getZ();

        // check that Hohmann maneuver was performed (it should be h(tf) = 1000 km)...
        System.out.println("h(tf) = " + (finalState.getPVCoordinates().getPosition().getNorm().getValue()
                - Constants.IERS2010_EARTH_EQUATORIAL_RADIUS));

        // compute gradient of final position components w.r.t. free parameters (i.e. the maneuver delta-v components)
        double dxFdDVx = xF.getGradient()[0];
        double dxFdDVy = xF.getGradient()[1];
        double dxFdDVz = xF.getGradient()[2];

        double dyFdDVx = yF.getGradient()[0];
        double dyFdDVy = yF.getGradient()[1];
        double dyFdDVz = yF.getGradient()[2];

        double dzFdDVx = zF.getGradient()[0];
        double dzFdDVy = zF.getGradient()[1];
        double dzFdDVz = zF.getGradient()[2];

        System.out.println("dxFdDVx = " + dxFdDVx);
        System.out.println("dxFdDVy = " + dxFdDVy);
        System.out.println("dxFdDVz = " + dxFdDVz);

        System.out.println("dyFdDVx = " + dyFdDVx);
        System.out.println("dyFdDVy = " + dyFdDVy);
        System.out.println("dyFdDVz = " + dyFdDVz);

        System.out.println("dzFdDVx = " + dzFdDVx);
        System.out.println("dzFdDVy = " + dzFdDVy);
        System.out.println("dzFdDVz = " + dzFdDVz);
    }
}