import java.io.FileWriter;
import java.io.IOException;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.ode.nonstiff.AdaptiveStepsizeIntegrator;
import org.hipparchus.ode.nonstiff.DormandPrince853Integrator;
import org.orekit.attitudes.AlignedAndConstrained;
import org.orekit.attitudes.AttitudesSequence;
import org.orekit.attitudes.PredefinedTarget;
import org.orekit.bodies.CelestialBodyFactory;
import org.orekit.bodies.GeodeticPoint;
import org.orekit.bodies.OneAxisEllipsoid;
import org.orekit.forces.ForceModel;
import org.orekit.forces.gravity.HolmesFeatherstoneAttractionModel;
import org.orekit.forces.gravity.potential.GravityFieldFactory;
import org.orekit.forces.gravity.potential.NormalizedSphericalHarmonicsProvider;
import org.orekit.frames.Frame;
import org.orekit.frames.FramesFactory;
import org.orekit.frames.StaticTransform;
import org.orekit.models.earth.ReferenceEllipsoid;
import org.orekit.orbits.KeplerianOrbit;
import org.orekit.orbits.Orbit;
import org.orekit.orbits.OrbitType;
import org.orekit.orbits.PositionAngleType;
import org.orekit.propagation.BoundedPropagator;
import org.orekit.propagation.EphemerisGenerator;
import org.orekit.propagation.SpacecraftState;
import org.orekit.propagation.events.DateDetector;
import org.orekit.propagation.events.EclipseDetector;
import org.orekit.propagation.events.handlers.ContinueOnEvent;
import org.orekit.propagation.events.handlers.RecordAndContinue;
import org.orekit.propagation.numerical.NumericalPropagator;
import org.orekit.time.AbsoluteDate;
import org.orekit.time.TimeScalesFactory;
import org.orekit.time.TimeStamped;
import org.orekit.utils.AngularDerivativesFilter;
import org.orekit.utils.Constants;
import org.orekit.utils.ExtendedPVCoordinatesProvider;
import org.orekit.utils.ExtendedPositionProvider;
import org.orekit.utils.IERSConventions;

public class TestCase12 {

    private TestCase12 () {
        // Empty
    }
    
    public static void run () throws IOException {
        // configure Orekit
        final File home       = new File(System.getProperty("user.home"));
        final File orekitData = new File(home, "orekit-data");
        if (!orekitData.exists()) {
            System.err.format(Locale.US, "Failed to find %s folder%n",
                                orekitData.getAbsolutePath());
            System.exit(1);
        }
        final DataProvidersManager manager = DataContext.getDefault().getDataProvidersManager();
        manager.addProvider(new DirectoryCrawler(orekitData));
        
        // Inertial Frame Definition and Date
        final Frame inertialFrame =  FramesFactory.getMOD(IERSConventions.IERS_2010);
        final AbsoluteDate initialDate = AbsoluteDate.createMJDDate(5325+51544, 0.00072917*Constants.JULIAN_DAY, TimeScalesFactory.getTT());

        // Defining Orbit using Keplerian Elements (Osculating)
        final double a = 6993.47003*1000;                  // Semi-major Axis
        final double e = 0.002490593;                      // Eccentricity
        final double i = Math.toRadians(20);        // Inclination
        final double omega = Math.toRadians(90);    // Argument of Perigee
        final double raan = Math.toRadians(180);    // Right Ascension of the Ascending Node
        final double ta = Math.toRadians(33.25922); // True Anomaly

        Orbit initialOrbit = new KeplerianOrbit(a, e, i, omega, raan, ta, PositionAngleType.TRUE, inertialFrame, initialDate, Constants.EGM96_EARTH_MU);

        // Numerical Propagator
        // Defining Integrator
        final double minStep = 0.001;
        final double maxStep = 100.0;
        final double initStep = 0.001*24*60*60; 
        final double tolerance = 0.001;         // https://forum.orekit.org/t/position-tolerance-min-max-step/3350/3 -> 0.001
        final double[][] tolerances = NumericalPropagator.tolerances(tolerance, initialOrbit, OrbitType.EQUINOCTIAL);
        AdaptiveStepsizeIntegrator integrator = new DormandPrince853Integrator(minStep, maxStep, tolerances[0], tolerances[1]);
        integrator.setInitialStepSize(initStep);

        // Defining Propagator
        final SpacecraftState initialState = new SpacecraftState(initialOrbit);
        final NumericalPropagator propagator = new NumericalPropagator(integrator);
        propagator.setOrbitType(OrbitType.CARTESIAN);
        propagator.setInitialState(initialState);

        // Adding Force Models
        // Gravity Model
        final NormalizedSphericalHarmonicsProvider provider = GravityFieldFactory.getConstantNormalizedProvider(2, 0, initialDate);
        final ForceModel forceGravity = new HolmesFeatherstoneAttractionModel(FramesFactory.getITRF(IERSConventions.IERS_2010, true), provider);
        propagator.addForceModel(forceGravity);


        // Earth and Sun
        final OneAxisEllipsoid earth = ReferenceEllipsoid.getIers2010(FramesFactory.getITRF(IERSConventions.IERS_2010, true)); // Use ReferenceEllipsoid for the eclipse detector, instead of a new OneAxisEllipsoid
        final ExtendedPositionProvider sun = CelestialBodyFactory.getSun();


        // Night Attitude
        final AlignedAndConstrained nightAttitude = new AlignedAndConstrained(Vector3D.PLUS_I, PredefinedTarget.NADIR, Vector3D.PLUS_J, PredefinedTarget.VELOCITY, sun, earth);

        // Day Attitude
        final AlignedAndConstrained dayAttitude = new AlignedAndConstrained(Vector3D.MINUS_K, PredefinedTarget.SUN, Vector3D.PLUS_I, PredefinedTarget.EARTH, sun, earth);

        // Eclipse Detector
        RecordAndContinue eclipseRecordHandler = new RecordAndContinue();
        final EclipseDetector eclipseDetector = new EclipseDetector((ExtendedPVCoordinatesProvider) sun, 696340000., earth).withHandler(eclipseRecordHandler); 

        final AttitudesSequence solarStandBy = new AttitudesSequence();

        solarStandBy.addSwitchingCondition(nightAttitude, dayAttitude, eclipseDetector, true, false, 10.0, AngularDerivativesFilter.USE_R, null);
        solarStandBy.addSwitchingCondition(dayAttitude, nightAttitude, eclipseDetector, false, true, 10.0, AngularDerivativesFilter.USE_R, null);
        
        // Initialize attitude
        if (eclipseDetector.g(initialState) >= 0) {
            solarStandBy.resetActiveProvider(dayAttitude);
        }
        else {
            solarStandBy.resetActiveProvider(nightAttitude);
        }

        // Observation Attitude
        AlignedAndConstrained observationAttitude = nightAttitude;

        // Set detector
        TimeStamped[] dates = {initialDate.shiftedBy(1300), initialDate.shiftedBy(1700)};
        DateDetector dateDetector = new DateDetector(dates).withHandler(new ContinueOnEvent());

        final AttitudesSequence attitudeSequence = new AttitudesSequence();
        attitudeSequence.addSwitchingCondition(solarStandBy, observationAttitude, dateDetector, true, false, 10.0, AngularDerivativesFilter.USE_R, null);
        attitudeSequence.addSwitchingCondition(observationAttitude, solarStandBy, dateDetector, false, true, 10.0, AngularDerivativesFilter.USE_R, null);

        // Set propagator attitudeProvider
        propagator.setAttitudeProvider(attitudeSequence);
        solarStandBy.registerSwitchEvents(propagator);
        attitudeSequence.registerSwitchEvents(propagator);

        // Run propagation 
        final double dt = 2 * initialOrbit.getKeplerianPeriod();
        final AbsoluteDate finalDate = initialDate.shiftedBy(dt);

        EphemerisGenerator ephgen = propagator.getEphemerisGenerator();
        propagator.propagate(finalDate);
        BoundedPropagator ephemeris = ephgen.getGeneratedEphemeris();

        System.out.println("Size of Eclipse Event Handler: " + eclipseRecordHandler.getEvents().size());

        // Write to file
        String outputFilePath = "./output/attitude.csv";

        final FileWriter fileWriter = new FileWriter(outputFilePath);

        double ephemerisStep = 1.;

        for (AbsoluteDate addDate = initialDate; addDate.isBeforeOrEqualTo(finalDate); addDate = addDate.shiftedBy(ephemerisStep)) {
            SpacecraftState addState = ephemeris.propagate(addDate);
            
            fileWriter.write(String.format("%.8f;%.5f;%.5f\n",
                                            addState.getDate().getMJD(TimeScalesFactory.getTT()),   // Date in MJD
                                            nadirAngle(addState, earth, Vector3D.PLUS_I),           // Angle between instrument direction (PLUS_I = +x) and Nadir, which is restricted to a max of offNadirAngle
                                            sunAngle(addState, sun, Vector3D.MINUS_K)               // Angle between the satellite's sun pointing vector (MINUS_J = -z) and the Sun direction
                                            ));
        }

        fileWriter.close();

    }

    private static double nadirAngle (SpacecraftState state, OneAxisEllipsoid earth, Vector3D pointingVector) {
        final StaticTransform inertToEarth = state.getFrame().getStaticTransformTo(earth.getBodyFrame(), state.getDate());
        GeodeticPoint geoPoint = earth.transform(inertToEarth.transformVector(state.getPosition()), earth.getFrame(), state.getDate());
        Vector3D nadirAtInertial = inertToEarth.getInverse().transformVector(geoPoint.getNadir());

        Vector3D pointingVectorAtInertial = state.getAttitude().getRotation().applyInverseTo(pointingVector);

        double angle = Vector3D.angle(nadirAtInertial, pointingVectorAtInertial);

        return angle;
    }

    private static double sunAngle (SpacecraftState s, ExtendedPositionProvider sun, Vector3D satellitePointingVector) {
        Vector3D sunToSatelliteInertialFrame = sun.getPosition(s.getDate(), s.getFrame()).subtract(s.getPosition());
        Vector3D sunPointingVectorInertialFrame = s.getAttitude().getRotation().applyInverseTo(satellitePointingVector);
        double angle = Vector3D.angle(sunToSatelliteInertialFrame, sunPointingVectorInertialFrame);
        
        return angle;
    }
}