AdditionalEquationsTest

/* Copyright 2002-2022 CS GROUP
 * Licensed to CS GROUP (CS) under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * CS licenses this file to You under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with
 * the License.  You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
package org.orekit.propagation.integration;

import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.ode.nonstiff.AdaptiveStepsizeIntegrator;
import org.hipparchus.ode.nonstiff.DormandPrince853Integrator;
import org.junit.After;
import org.junit.Assert;
import org.junit.Before;
import org.junit.Test;
import org.orekit.Utils;
import org.orekit.forces.gravity.potential.GravityFieldFactory;
import org.orekit.forces.gravity.potential.SHMFormatReader;
import org.orekit.forces.maneuvers.ImpulseManeuver;
import org.orekit.frames.FramesFactory;
import org.orekit.orbits.EquinoctialOrbit;
import org.orekit.orbits.Orbit;
import org.orekit.orbits.OrbitType;
import org.orekit.propagation.SpacecraftState;
import org.orekit.propagation.events.DateDetector;
import org.orekit.propagation.numerical.NumericalPropagator;
import org.orekit.propagation.semianalytical.dsst.DSSTPropagator;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.PVCoordinates;

@Deprecated
public class AdditionalEquationsTest {

    private double          mu;
    private AbsoluteDate    initDate;
    private SpacecraftState initialState;
    private double[][]      tolerance;

    /** Test for issue #401
     *  with a numerical propagator */
    @Test
    public void testInitNumerical() {

        // setup
        final double reference = 1.25;
        final double rate      = 1.5;
        final double dt        = 600.0;
        Linear linear = new Linear("linear", reference, rate);
        Assert.assertFalse(linear.wasCalled());

        // action
        AdaptiveStepsizeIntegrator integrator = new DormandPrince853Integrator(0.001, 200, tolerance[0], tolerance[1]);
        integrator.setInitialStepSize(60);
        NumericalPropagator propagatorNumerical = new NumericalPropagator(integrator);
        propagatorNumerical.setInitialState(initialState.addAdditionalState(linear.getName(), reference));
        propagatorNumerical.addAdditionalEquations(linear);
        SpacecraftState finalState = propagatorNumerical.propagate(initDate.shiftedBy(dt));

        // verify
        Assert.assertTrue(linear.wasCalled());
        Assert.assertEquals(reference + dt * rate, finalState.getAdditionalState(linear.getName())[0], 1.0e-10);

    }

    /** Test for issue #401
     *  with a DSST propagator */
    @Test
    public void testInitDSST() {

        // setup
        final double reference = 3.5;
        final double rate      = 1.5;
        final double dt        = 600.0;
        Linear linear = new Linear("linear", reference, rate);
        Assert.assertFalse(linear.wasCalled());

        // action
        AdaptiveStepsizeIntegrator integrator = new DormandPrince853Integrator(0.001, 200, tolerance[0], tolerance[1]);
        integrator.setInitialStepSize(60);
        DSSTPropagator propagatorDSST = new DSSTPropagator(integrator);
        propagatorDSST.setInitialState(initialState.addAdditionalState(linear.getName(), reference));
        propagatorDSST.addAdditionalEquations(linear);
        SpacecraftState finalState = propagatorDSST.propagate(initDate.shiftedBy(dt));

        // verify
        Assert.assertTrue(linear.wasCalled());
        Assert.assertEquals(reference + dt * rate, finalState.getAdditionalState(linear.getName())[0], 1.0e-10);

    }

    @Test
    public void testResetState() {

        // setup
        final double reference1 = 3.5;
        final double rate1      = 1.5;
        Linear linear1 = new Linear("linear-1", reference1, rate1);
        Assert.assertFalse(linear1.wasCalled());
        final double reference2 = 4.5;
        final double rate2      = 1.25;
        Linear linear2 = new Linear("linear-2", reference2, rate2);
        Assert.assertFalse(linear2.wasCalled());
        final double dt = 600;

        // action
        AdaptiveStepsizeIntegrator integrator = new DormandPrince853Integrator(0.001, 200, tolerance[0], tolerance[1]);
        integrator.setInitialStepSize(60);
        NumericalPropagator propagatorNumerical = new NumericalPropagator(integrator);
        propagatorNumerical.setInitialState(initialState.
                                            addAdditionalState(linear1.getName(), reference1).
                                            addAdditionalState(linear2.getName(), reference2));
        propagatorNumerical.addAdditionalEquations(linear1);
        propagatorNumerical.addAdditionalEquations(linear2);
        propagatorNumerical.addEventDetector(new ImpulseManeuver<>(new DateDetector(initDate.shiftedBy(dt / 2.0)),
                                                                   new Vector3D(0.1, 0.2, 0.3), 350.0));
        SpacecraftState finalState = propagatorNumerical.propagate(initDate.shiftedBy(dt));

        // verify
        Assert.assertTrue(linear1.wasCalled());
        Assert.assertTrue(linear2.wasCalled());
        Assert.assertEquals(reference1 + dt * rate1, finalState.getAdditionalState(linear1.getName())[0], 1.0e-10);
        Assert.assertEquals(reference2 + dt * rate2, finalState.getAdditionalState(linear2.getName())[0], 1.0e-10);

    }

    @Before
    public void setUp() {
        Utils.setDataRoot("regular-data:potential/shm-format");
        GravityFieldFactory.addPotentialCoefficientsReader(new SHMFormatReader("^eigen_cg03c_coef$", false));
        mu  = GravityFieldFactory.getUnnormalizedProvider(0, 0).getMu();
        final Vector3D position = new Vector3D(7.0e6, 1.0e6, 4.0e6);
        final Vector3D velocity = new Vector3D(-500.0, 8000.0, 1000.0);
        initDate = AbsoluteDate.J2000_EPOCH;
        final Orbit orbit = new EquinoctialOrbit(new PVCoordinates(position,  velocity),
                                                 FramesFactory.getEME2000(), initDate, mu);
        initialState = new SpacecraftState(orbit);
        tolerance = NumericalPropagator.tolerances(0.001, orbit, OrbitType.EQUINOCTIAL);
    }

    @After
    public void tearDown() {
        initDate     = null;
        initialState = null;
        tolerance    = null;
    }

    private static class Linear implements AdditionalEquations {

        private String  name;
        private double  expectedAtInit;
        private double  rate;
        private boolean called;

        public Linear(final String name, final double expectedAtInit, final double rate) {
            this.name           = name;
            this.expectedAtInit = expectedAtInit;
            this.rate           = rate;
            this.called         = false;
        }

        @Override
        public void init(SpacecraftState initiaState, AbsoluteDate target) {
            Assert.assertEquals(expectedAtInit, initiaState.getAdditionalState(getName())[0], 1.0e-15);
            called = true;
        }

        @Override
        public double[] computeDerivatives(SpacecraftState s, double[] pDot) {
            pDot[0] = rate;
            return null;
        }

        @Override
        public String getName() {
            return name;
        }

        public boolean wasCalled() {
            return called;
        }

    }

}