FieldAbstractAnalyticalPropagator.java

/* 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.analytical;

import java.util.ArrayList;
import java.util.Collection;
import java.util.Collections;
import java.util.Comparator;
import java.util.List;
import java.util.PriorityQueue;
import java.util.Queue;

import org.hipparchus.CalculusFieldElement;
import org.hipparchus.Field;
import org.hipparchus.exception.MathRuntimeException;
import org.hipparchus.ode.events.Action;
import org.hipparchus.util.MathArrays;
import org.orekit.attitudes.AttitudeProvider;
import org.orekit.attitudes.FieldAttitude;
import org.orekit.errors.OrekitException;
import org.orekit.errors.OrekitInternalError;
import org.orekit.frames.Frame;
import org.orekit.orbits.FieldOrbit;
import org.orekit.propagation.BoundedPropagator;
import org.orekit.propagation.FieldAbstractPropagator;
import org.orekit.propagation.FieldAdditionalStateProvider;
import org.orekit.propagation.FieldBoundedPropagator;
import org.orekit.propagation.FieldEphemerisGenerator;
import org.orekit.propagation.FieldSpacecraftState;
import org.orekit.propagation.events.FieldEventDetector;
import org.orekit.propagation.events.FieldEventState;
import org.orekit.propagation.events.FieldEventState.EventOccurrence;
import org.orekit.propagation.sampling.FieldOrekitStepInterpolator;
import org.orekit.time.FieldAbsoluteDate;
import org.orekit.utils.FieldPVCoordinatesProvider;
import org.orekit.utils.ParameterDriver;
import org.orekit.utils.TimeStampedFieldPVCoordinates;

/** Common handling of {@link org.orekit.propagation.FieldPropagator} methods for analytical propagators.
 * <p>
 * This abstract class allows to provide easily the full set of {@link
 * org.orekit.propagation.FieldPropagator FieldPropagator} methods, including all propagation
 * modes support and discrete events support for any simple propagation method. Only
 * two methods must be implemented by derived classes: {@link #propagateOrbit(FieldAbsoluteDate, CalculusFieldElement[])}
 * and {@link #getMass(FieldAbsoluteDate)}. The first method should perform straightforward
 * propagation starting from some internally stored initial state up to the specified target date.
 * </p>
 * @author Luc Maisonobe
 */

public abstract class FieldAbstractAnalyticalPropagator<T extends CalculusFieldElement<T>> extends FieldAbstractPropagator<T> {

    /** Provider for attitude computation. */
    private FieldPVCoordinatesProvider<T> pvProvider;

    /** Start date of last propagation. */
    private FieldAbsoluteDate<T> lastPropagationStart;

    /** End date of last propagation. */
    private FieldAbsoluteDate<T> lastPropagationEnd;

    /** Initialization indicator of events states. */
    private boolean statesInitialized;

    /** Indicator for last step. */
    private boolean isLastStep;

    /** Event steps. */
    private final Collection<FieldEventState<?, T>> eventsStates;

    /** Build a new instance.
     * @param attitudeProvider provider for attitude computation
     * @param field field used as default
     */
    protected FieldAbstractAnalyticalPropagator(final Field<T> field, final AttitudeProvider attitudeProvider) {
        super(field);
        setAttitudeProvider(attitudeProvider);
        pvProvider           = new FieldLocalPVProvider();
        lastPropagationStart = FieldAbsoluteDate.getPastInfinity(field);
        lastPropagationEnd   = FieldAbsoluteDate.getFutureInfinity(field);
        statesInitialized    = false;
        eventsStates         = new ArrayList<>();
    }

    /** {@inheritDoc} */
    @Override
    public FieldEphemerisGenerator<T> getEphemerisGenerator() {
        return () -> new FieldBoundedPropagatorView(lastPropagationStart, lastPropagationEnd);
    }

    /** {@inheritDoc} */
    public <D extends FieldEventDetector<T>> void addEventDetector(final D detector) {
        eventsStates.add(new FieldEventState<>(detector));
    }

    /** {@inheritDoc} */
    @Override
    public Collection<FieldEventDetector<T>> getEventsDetectors() {
        final List<FieldEventDetector<T>> list = new ArrayList<>();
        for (final FieldEventState<?, T> state : eventsStates) {
            list.add(state.getEventDetector());
        }
        return Collections.unmodifiableCollection(list);
    }

    /** {@inheritDoc} */
    @Override
    public void clearEventsDetectors() {
        eventsStates.clear();
    }
    /** {@inheritDoc} */
    @Override
    public FieldSpacecraftState<T> propagate(final FieldAbsoluteDate<T> start, final FieldAbsoluteDate<T> target) {
        try {

            initializePropagation();

            lastPropagationStart = start;

            final boolean           isForward = target.compareTo(start) >= 0;
            FieldSpacecraftState<T> state   = updateAdditionalStates(basicPropagate(start));

            // initialize event detectors
            for (final FieldEventState<?, T> es : eventsStates) {
                es.init(state, target);
            }

            // initialize step handlers
            getMultiplexer().init(state, target);

            // iterate over the propagation range, need loop due to reset events
            statesInitialized = false;
            isLastStep = false;
            do {

                // attempt to advance to the target date
                final FieldSpacecraftState<T> previous = state;
                final FieldSpacecraftState<T> current = updateAdditionalStates(basicPropagate(target));
                final FieldBasicStepInterpolator interpolator =
                        new FieldBasicStepInterpolator(isForward, previous, current);

                // accept the step, trigger events and step handlers
                state = acceptStep(interpolator, target);

                // Update the potential changes in the spacecraft state due to the events
                // especially the potential attitude transition
                state = updateAdditionalStates(basicPropagate(state.getDate()));

            } while (!isLastStep);

            // return the last computed state
            lastPropagationEnd = state.getDate();
            setStartDate(state.getDate());
            return state;

        } catch (MathRuntimeException mrte) {
            throw OrekitException.unwrap(mrte);
        }
    }

    /** Accept a step, triggering events and step handlers.
     * @param interpolator interpolator for the current step
     * @param target final propagation time
     * @return state at the end of the step
     * @exception MathRuntimeException if an event cannot be located
     */
    protected FieldSpacecraftState<T> acceptStep(final FieldBasicStepInterpolator interpolator,
                                                 final FieldAbsoluteDate<T> target)
        throws MathRuntimeException {

        FieldSpacecraftState<T>       previous   = interpolator.getPreviousState();
        final FieldSpacecraftState<T> current    = interpolator.getCurrentState();
        FieldBasicStepInterpolator    restricted = interpolator;

        // initialize the events states if needed
        if (!statesInitialized) {

            if (!eventsStates.isEmpty()) {
                // initialize the events states
                for (final FieldEventState<?, T> state : eventsStates) {
                    state.reinitializeBegin(interpolator);
                }
            }

            statesInitialized = true;

        }

        // search for next events that may occur during the step
        final int orderingSign = interpolator.isForward() ? +1 : -1;
        final Queue<FieldEventState<?, T>> occurringEvents = new PriorityQueue<>(new Comparator<FieldEventState<?, T>>() {
            /** {@inheritDoc} */
            @Override
            public int compare(final FieldEventState<?, T> es0, final FieldEventState<?, T> es1) {
                return orderingSign * es0.getEventDate().compareTo(es1.getEventDate());
            }
        });

        boolean doneWithStep = false;
        resetEvents:
        do {

            // Evaluate all event detectors for events
            occurringEvents.clear();
            for (final FieldEventState<?, T> state : eventsStates) {
                if (state.evaluateStep(interpolator)) {
                    // the event occurs during the current step
                    occurringEvents.add(state);
                }
            }


            do {

                eventLoop:
                while (!occurringEvents.isEmpty()) {

                    // handle the chronologically first event
                    final FieldEventState<?, T> currentEvent = occurringEvents.poll();

                    // get state at event time
                    FieldSpacecraftState<T> eventState = restricted.getInterpolatedState(currentEvent.getEventDate());

                    // restrict the interpolator to the first part of the step, up to the event
                    restricted = restricted.restrictStep(previous, eventState);

                    // try to advance all event states to current time
                    for (final FieldEventState<?, T> state : eventsStates) {
                        if (state != currentEvent && state.tryAdvance(eventState, interpolator)) {
                            // we need to handle another event first
                            // remove event we just updated to prevent heap corruption
                            occurringEvents.remove(state);
                            // add it back to update its position in the heap
                            occurringEvents.add(state);
                            // re-queue the event we were processing
                            occurringEvents.add(currentEvent);
                            continue eventLoop;
                        }
                    }
                    // all event detectors agree we can advance to the current event time

                    // handle the first part of the step, up to the event
                    getMultiplexer().handleStep(restricted);

                    // acknowledge event occurrence
                    final EventOccurrence<T> occurrence = currentEvent.doEvent(eventState);
                    final Action action = occurrence.getAction();
                    isLastStep = action == Action.STOP;
                    if (isLastStep) {

                        // ensure the event is after the root if it is returned STOP
                        // this lets the user integrate to a STOP event and then restart
                        // integration from the same time.
                        final FieldSpacecraftState<T> savedState = eventState;
                        eventState = interpolator.getInterpolatedState(occurrence.getStopDate());
                        restricted = restricted.restrictStep(savedState, eventState);

                        // handle the almost zero size last part of the final step, at event time
                        getMultiplexer().handleStep(restricted);
                        getMultiplexer().finish(restricted.getCurrentState());

                    }

                    if (isLastStep) {
                        // the event asked to stop integration
                        return eventState;
                    }

                    if (action == Action.RESET_DERIVATIVES || action == Action.RESET_STATE) {
                        // some event handler has triggered changes that
                        // invalidate the derivatives, we need to recompute them
                        final FieldSpacecraftState<T> resetState = occurrence.getNewState();
                        resetIntermediateState(resetState, interpolator.isForward());
                        return resetState;
                    }
                    // at this point action == Action.CONTINUE or Action.RESET_EVENTS

                    // prepare handling of the remaining part of the step
                    previous = eventState;
                    restricted = new FieldBasicStepInterpolator(restricted.isForward(), eventState, current);

                    if (action == Action.RESET_EVENTS) {
                        continue resetEvents;
                    }

                    // at this pint action == Action.CONTINUE
                    // check if the same event occurs again in the remaining part of the step
                    if (currentEvent.evaluateStep(restricted)) {
                        // the event occurs during the current step
                        occurringEvents.add(currentEvent);
                    }

                }

                // last part of the step, after the last event. Advance all detectors to
                // the end of the step. Should only detect a new event here if an event
                // modified the g function of another detector. Detecting such events here
                // is unreliable and RESET_EVENTS should be used instead. Might as well
                // re-check here because we have to loop through all the detectors anyway
                // and the alternative is to throw an exception.
                for (final FieldEventState<?, T> state : eventsStates) {
                    if (state.tryAdvance(current, interpolator)) {
                        occurringEvents.add(state);
                    }
                }

            } while (!occurringEvents.isEmpty());

            doneWithStep = true;
        } while (!doneWithStep);

        isLastStep = target.equals(current.getDate());

        // handle the remaining part of the step, after all events if any
        getMultiplexer().handleStep(restricted);
        if (isLastStep) {
            getMultiplexer().finish(restricted.getCurrentState());
        }

        return current;

    }

    /** Get the mass.
     * @param date target date for the orbit
     * @return mass mass
     */
    protected abstract T getMass(FieldAbsoluteDate<T> date);

    /** Get PV coordinates provider.
     * @return PV coordinates provider
     */
    public FieldPVCoordinatesProvider<T> getPvProvider() {
        return pvProvider;
    }

    /** Reset an intermediate state.
     * @param state new intermediate state to consider
     * @param forward if true, the intermediate state is valid for
     * propagations after itself
     */
    protected abstract void resetIntermediateState(FieldSpacecraftState<T> state, boolean forward);

    /** Extrapolate an orbit up to a specific target date.
     * @param date target date for the orbit
     * @param parameters model parameters
     * @return extrapolated parameters
     */
    protected abstract FieldOrbit<T> propagateOrbit(FieldAbsoluteDate<T> date, T[] parameters);

    /** Get the parameters driver for propagation model.
     * @return drivers for propagation model
     */
    protected abstract List<ParameterDriver> getParametersDrivers();

    /** Get model parameters.
     * @param field field to which the elements belong
     * @return model parameters
     */
    public T[] getParameters(final Field<T> field) {
        final List<ParameterDriver> drivers = getParametersDrivers();
        final T[] parameters = MathArrays.buildArray(field, drivers.size());
        for (int i = 0; i < drivers.size(); ++i) {
            parameters[i] = field.getZero().add(drivers.get(i).getValue());
        }
        return parameters;
    }

    /** Propagate an orbit without any fancy features.
     * <p>This method is similar in spirit to the {@link #propagate} method,
     * except that it does <strong>not</strong> call any handler during
     * propagation, nor any discrete events, not additional states. It always
     * stop exactly at the specified date.</p>
     * @param date target date for propagation
     * @return state at specified date
     */
    protected FieldSpacecraftState<T> basicPropagate(final FieldAbsoluteDate<T> date) {
        try {

            // evaluate orbit
            final FieldOrbit<T> orbit = propagateOrbit(date, getParameters(date.getField()));

            // evaluate attitude
            final FieldAttitude<T> attitude =
                getAttitudeProvider().getAttitude(pvProvider, date, orbit.getFrame());

            // build raw state
            return new FieldSpacecraftState<>(orbit, attitude, getMass(date));

        } catch (OrekitException oe) {
            throw new OrekitException(oe);
        }
    }

    /** Internal FieldPVCoordinatesProvider<T> for attitude computation. */
    private class FieldLocalPVProvider implements FieldPVCoordinatesProvider<T> {

        /** {@inheritDoc} */
        @Override
        public TimeStampedFieldPVCoordinates<T> getPVCoordinates(final FieldAbsoluteDate<T> date, final Frame frame) {
            return propagateOrbit(date, getParameters(date.getField())).getPVCoordinates(frame);
        }

    }

    /** {@link BoundedPropagator} view of the instance. */
    private class FieldBoundedPropagatorView extends FieldAbstractAnalyticalPropagator<T>
        implements FieldBoundedPropagator<T> {

        /** Min date. */
        private final FieldAbsoluteDate<T> minDate;

        /** Max date. */
        private final FieldAbsoluteDate<T> maxDate;

        /** Simple constructor.
         * @param startDate start date of the propagation
         * @param endDate end date of the propagation
         */
        FieldBoundedPropagatorView(final FieldAbsoluteDate<T> startDate, final FieldAbsoluteDate<T> endDate) {
            super(startDate.durationFrom(endDate).getField(), FieldAbstractAnalyticalPropagator.this.getAttitudeProvider());
            super.resetInitialState(FieldAbstractAnalyticalPropagator.this.getInitialState());
            if (startDate.compareTo(endDate) <= 0) {
                minDate = startDate;
                maxDate = endDate;
            } else {
                minDate = endDate;
                maxDate = startDate;
            }

            try {
                // copy the same additional state providers as the original propagator
                for (FieldAdditionalStateProvider<T> provider : FieldAbstractAnalyticalPropagator.this.getAdditionalStateProviders()) {
                    addAdditionalStateProvider(provider);
                }
            } catch (OrekitException oe) {
                // as the providers are already compatible with each other,
                // this should never happen
                throw new OrekitInternalError(null);
            }

        }

        /** {@inheritDoc} */
        @Override
        public FieldAbsoluteDate<T> getMinDate() {
            return minDate;
        }

        /** {@inheritDoc} */
        @Override
        public FieldAbsoluteDate<T> getMaxDate() {
            return maxDate;
        }

        /** {@inheritDoc} */
        @Override
        protected FieldOrbit<T> propagateOrbit(final FieldAbsoluteDate<T> target, final T[] parameters) {
            return FieldAbstractAnalyticalPropagator.this.propagateOrbit(target, parameters);
        }

        /** {@inheritDoc} */
        @Override
        public T getMass(final FieldAbsoluteDate<T> date) {
            return FieldAbstractAnalyticalPropagator.this.getMass(date);
        }

        /** {@inheritDoc} */
        @Override
        public TimeStampedFieldPVCoordinates<T> getPVCoordinates(final FieldAbsoluteDate<T> date, final Frame frame) {
            return propagate(date).getPVCoordinates(frame);
        }

        /** {@inheritDoc} */
        @Override
        public void resetInitialState(final FieldSpacecraftState<T> state) {
            super.resetInitialState(state);
            FieldAbstractAnalyticalPropagator.this.resetInitialState(state);
        }

        /** {@inheritDoc} */
        @Override
        protected void resetIntermediateState(final FieldSpacecraftState<T> state, final boolean forward) {
            FieldAbstractAnalyticalPropagator.this.resetIntermediateState(state, forward);
        }

        /** {@inheritDoc} */
        @Override
        public FieldSpacecraftState<T> getInitialState() {
            return FieldAbstractAnalyticalPropagator.this.getInitialState();
        }

        /** {@inheritDoc} */
        @Override
        public Frame getFrame() {
            return FieldAbstractAnalyticalPropagator.this.getFrame();
        }

        @Override
        protected List<ParameterDriver> getParametersDrivers() {
            return FieldAbstractAnalyticalPropagator.this.getParametersDrivers();
        }
    }


    /** Internal class for local propagation. */
    private class FieldBasicStepInterpolator implements FieldOrekitStepInterpolator<T> {

        /** Previous state. */
        private final FieldSpacecraftState<T> previousState;

        /** Current state. */
        private final FieldSpacecraftState<T> currentState;

        /** Forward propagation indicator. */
        private final boolean forward;

        /** Simple constructor.
         * @param isForward integration direction indicator
         * @param previousState start of the step
         * @param currentState end of the step
         */
        FieldBasicStepInterpolator(final boolean isForward,
                                   final FieldSpacecraftState<T> previousState,
                                   final FieldSpacecraftState<T> currentState) {
            this.forward             = isForward;
            this.previousState   = previousState;
            this.currentState    = currentState;
        }

        /** {@inheritDoc} */
        @Override
        public FieldSpacecraftState<T> getPreviousState() {
            return previousState;
        }

        /** {@inheritDoc} */
        @Override
        public FieldSpacecraftState<T> getCurrentState() {
            return currentState;
        }

        /** {@inheritDoc} */
        @Override
        public FieldSpacecraftState<T> getInterpolatedState(final FieldAbsoluteDate<T> date) {

            // compute the basic spacecraft state
            final FieldSpacecraftState<T> basicState = basicPropagate(date);

            // add the additional states
            return updateAdditionalStates(basicState);

        }

        /** {@inheritDoc} */
        @Override
        public boolean isForward() {
            return forward;
        }

        /** {@inheritDoc} */
        @Override
        public FieldBasicStepInterpolator restrictStep(final FieldSpacecraftState<T> newPreviousState,
                                                       final FieldSpacecraftState<T> newCurrentState) {
            return new FieldBasicStepInterpolator(forward, newPreviousState, newCurrentState);
        }

    }

}