1   /* Copyright 2002-2022 CS GROUP
2    * Licensed to CS GROUP (CS) under one or more
3    * contributor license agreements.  See the NOTICE file distributed with
4    * this work for additional information regarding copyright ownership.
5    * CS licenses this file to You under the Apache License, Version 2.0
6    * (the "License"); you may not use this file except in compliance with
7    * the License.  You may obtain a copy of the License at
8    *
9    *   http://www.apache.org/licenses/LICENSE-2.0
10   *
11   * Unless required by applicable law or agreed to in writing, software
12   * distributed under the License is distributed on an "AS IS" BASIS,
13   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14   * See the License for the specific language governing permissions and
15   * limitations under the License.
16   */
17  package org.orekit.propagation.integration;
18  
19  import java.util.ArrayList;
20  import java.util.Arrays;
21  import java.util.Collection;
22  import java.util.Collections;
23  import java.util.HashMap;
24  import java.util.LinkedList;
25  import java.util.List;
26  import java.util.Map;
27  import java.util.Queue;
28  
29  import org.hipparchus.exception.MathRuntimeException;
30  import org.hipparchus.ode.DenseOutputModel;
31  import org.hipparchus.ode.ExpandableODE;
32  import org.hipparchus.ode.ODEIntegrator;
33  import org.hipparchus.ode.ODEState;
34  import org.hipparchus.ode.ODEStateAndDerivative;
35  import org.hipparchus.ode.OrdinaryDifferentialEquation;
36  import org.hipparchus.ode.SecondaryODE;
37  import org.hipparchus.ode.events.Action;
38  import org.hipparchus.ode.events.EventHandlerConfiguration;
39  import org.hipparchus.ode.events.ODEEventHandler;
40  import org.hipparchus.ode.sampling.AbstractODEStateInterpolator;
41  import org.hipparchus.ode.sampling.ODEStateInterpolator;
42  import org.hipparchus.ode.sampling.ODEStepHandler;
43  import org.hipparchus.util.Precision;
44  import org.orekit.attitudes.AttitudeProvider;
45  import org.orekit.errors.OrekitException;
46  import org.orekit.errors.OrekitInternalError;
47  import org.orekit.errors.OrekitMessages;
48  import org.orekit.frames.Frame;
49  import org.orekit.orbits.OrbitType;
50  import org.orekit.orbits.PositionAngle;
51  import org.orekit.propagation.AbstractPropagator;
52  import org.orekit.propagation.BoundedPropagator;
53  import org.orekit.propagation.EphemerisGenerator;
54  import org.orekit.propagation.PropagationType;
55  import org.orekit.propagation.SpacecraftState;
56  import org.orekit.propagation.events.EventDetector;
57  import org.orekit.propagation.sampling.OrekitStepHandler;
58  import org.orekit.propagation.sampling.OrekitStepInterpolator;
59  import org.orekit.time.AbsoluteDate;
60  import org.orekit.utils.DoubleArrayDictionary;
61  
62  
63  /** Common handling of {@link org.orekit.propagation.Propagator Propagator}
64   *  methods for both numerical and semi-analytical propagators.
65   *  @author Luc Maisonobe
66   */
67  public abstract class AbstractIntegratedPropagator extends AbstractPropagator {
68  
69      /** Internal name used for complete secondary state dimension.
70       * @since 11.1
71       */
72      private static final String SECONDARY_DIMENSION = "Orekit-secondary-dimension";
73  
74      /** Event detectors not related to force models. */
75      private final List<EventDetector> detectors;
76  
77      /** Step handlers dedicated to ephemeris generation. */
78      private final List<StoringStepHandler> ephemerisGenerators;
79  
80      /** Integrator selected by the user for the orbital extrapolation process. */
81      private final ODEIntegrator integrator;
82  
83      /** Offsets of secondary states managed by {@link AdditionalEquations}.
84       * @since 11.1
85       */
86      private final Map<String, Integer> secondaryOffsets;
87  
88      /** Additional derivatives providers.
89       * @since 11.1
90       */
91      private List<AdditionalDerivativesProvider> additionalDerivativesProviders;
92  
93      /** Map of secondary equation offset in main
94      /** Counter for differential equations calls. */
95      private int calls;
96  
97      /** Mapper between raw double components and space flight dynamics objects. */
98      private StateMapper stateMapper;
99  
100     /** Flag for resetting the state at end of propagation. */
101     private boolean resetAtEnd;
102 
103     /** Type of orbit to output (mean or osculating) <br/>
104      * <p>
105      * This is used only in the case of semianalitical propagators where there is a clear separation between
106      * mean and short periodic elements. It is ignored by the Numerical propagator.
107      * </p>
108      */
109     private PropagationType propagationType;
110 
111     /** Build a new instance.
112      * @param integrator numerical integrator to use for propagation.
113      * @param propagationType type of orbit to output (mean or osculating).
114      */
115     protected AbstractIntegratedPropagator(final ODEIntegrator integrator, final PropagationType propagationType) {
116         detectors                      = new ArrayList<>();
117         ephemerisGenerators            = new ArrayList<>();
118         additionalDerivativesProviders = new ArrayList<>();
119         this.secondaryOffsets          = new HashMap<>();
120         this.integrator                = integrator;
121         this.propagationType           = propagationType;
122         this.resetAtEnd                = true;
123     }
124 
125     /** Allow/disallow resetting the initial state at end of propagation.
126      * <p>
127      * By default, at the end of the propagation, the propagator resets the initial state
128      * to the final state, thus allowing a new propagation to be started from there without
129      * recomputing the part already performed. Calling this method with {@code resetAtEnd} set
130      * to false changes prevents such reset.
131      * </p>
132      * @param resetAtEnd if true, at end of each propagation, the {@link
133      * #getInitialState() initial state} will be reset to the final state of
134      * the propagation, otherwise the initial state will be preserved
135      * @since 9.0
136      */
137     public void setResetAtEnd(final boolean resetAtEnd) {
138         this.resetAtEnd = resetAtEnd;
139     }
140 
141     /** Initialize the mapper. */
142     protected void initMapper() {
143         stateMapper = createMapper(null, Double.NaN, null, null, null, null);
144     }
145 
146     /**  {@inheritDoc} */
147     public void setAttitudeProvider(final AttitudeProvider attitudeProvider) {
148         super.setAttitudeProvider(attitudeProvider);
149         stateMapper = createMapper(stateMapper.getReferenceDate(), stateMapper.getMu(),
150                                    stateMapper.getOrbitType(), stateMapper.getPositionAngleType(),
151                                    attitudeProvider, stateMapper.getFrame());
152     }
153 
154     /** Set propagation orbit type.
155      * @param orbitType orbit type to use for propagation, null for
156      * propagating using {@link org.orekit.utils.AbsolutePVCoordinates AbsolutePVCoordinates}
157      * rather than {@link org.orekit.orbits Orbit}
158      */
159     protected void setOrbitType(final OrbitType orbitType) {
160         stateMapper = createMapper(stateMapper.getReferenceDate(), stateMapper.getMu(),
161                                    orbitType, stateMapper.getPositionAngleType(),
162                                    stateMapper.getAttitudeProvider(), stateMapper.getFrame());
163     }
164 
165     /** Get propagation parameter type.
166      * @return orbit type used for propagation, null for
167      * propagating using {@link org.orekit.utils.AbsolutePVCoordinates AbsolutePVCoordinates}
168      * rather than {@link org.orekit.orbits Orbit}
169      */
170     protected OrbitType getOrbitType() {
171         return stateMapper.getOrbitType();
172     }
173 
174     /** Check if only the mean elements should be used in a semianalitical propagation.
175      * @return {@link PropagationType MEAN} if only mean elements have to be used or
176      *         {@link PropagationType OSCULATING} if osculating elements have to be also used.
177      * @deprecated as of 11.1, replaced by {@link #getPropagationType()}
178      */
179     @Deprecated
180     protected PropagationType isMeanOrbit() {
181         return getPropagationType();
182     }
183 
184     /** Get the propagation type.
185      * @return propagation type.
186      * @since 11.1
187      */
188     public PropagationType getPropagationType() {
189         return propagationType;
190     }
191 
192     /** Set position angle type.
193      * <p>
194      * The position parameter type is meaningful only if {@link
195      * #getOrbitType() propagation orbit type}
196      * support it. As an example, it is not meaningful for propagation
197      * in {@link OrbitType#CARTESIAN Cartesian} parameters.
198      * </p>
199      * @param positionAngleType angle type to use for propagation
200      */
201     protected void setPositionAngleType(final PositionAngle positionAngleType) {
202         stateMapper = createMapper(stateMapper.getReferenceDate(), stateMapper.getMu(),
203                                    stateMapper.getOrbitType(), positionAngleType,
204                                    stateMapper.getAttitudeProvider(), stateMapper.getFrame());
205     }
206 
207     /** Get propagation parameter type.
208      * @return angle type to use for propagation
209      */
210     protected PositionAngle getPositionAngleType() {
211         return stateMapper.getPositionAngleType();
212     }
213 
214     /** Set the central attraction coefficient μ.
215      * @param mu central attraction coefficient (m³/s²)
216      */
217     public void setMu(final double mu) {
218         stateMapper = createMapper(stateMapper.getReferenceDate(), mu,
219                                    stateMapper.getOrbitType(), stateMapper.getPositionAngleType(),
220                                    stateMapper.getAttitudeProvider(), stateMapper.getFrame());
221     }
222 
223     /** Get the central attraction coefficient μ.
224      * @return mu central attraction coefficient (m³/s²)
225      * @see #setMu(double)
226      */
227     public double getMu() {
228         return stateMapper.getMu();
229     }
230 
231     /** Get the number of calls to the differential equations computation method.
232      * <p>The number of calls is reset each time the {@link #propagate(AbsoluteDate)}
233      * method is called.</p>
234      * @return number of calls to the differential equations computation method
235      */
236     public int getCalls() {
237         return calls;
238     }
239 
240     /** {@inheritDoc} */
241     @Override
242     public boolean isAdditionalStateManaged(final String name) {
243 
244         // first look at already integrated states
245         if (super.isAdditionalStateManaged(name)) {
246             return true;
247         }
248 
249         // then look at states we integrate ourselves
250         for (final AdditionalDerivativesProvider provider : additionalDerivativesProviders) {
251             if (provider.getName().equals(name)) {
252                 return true;
253             }
254         }
255 
256         return false;
257     }
258 
259     /** {@inheritDoc} */
260     @Override
261     public String[] getManagedAdditionalStates() {
262         final String[] alreadyIntegrated = super.getManagedAdditionalStates();
263         final String[] managed = new String[alreadyIntegrated.length + additionalDerivativesProviders.size()];
264         System.arraycopy(alreadyIntegrated, 0, managed, 0, alreadyIntegrated.length);
265         for (int i = 0; i < additionalDerivativesProviders.size(); ++i) {
266             managed[i + alreadyIntegrated.length] = additionalDerivativesProviders.get(i).getName();
267         }
268         return managed;
269     }
270 
271     /** Add a set of user-specified equations to be integrated along with the orbit propagation.
272      * @param additional additional equations
273      * @deprecated as of 11.1, replaced by {@link #addAdditionalDerivativesProvider(AdditionalDerivativesProvider)}
274      */
275     @Deprecated
276     public void addAdditionalEquations(final AdditionalEquations additional) {
277         addAdditionalDerivativesProvider(new AdditionalEquationsAdapter(additional, this::getInitialState));
278     }
279 
280     /** Add a provider for user-specified state derivatives to be integrated along with the orbit propagation.
281      * @param provider provider for additional derivatives
282      * @see #addAdditionalStateProvider(org.orekit.propagation.AdditionalStateProvider)
283      * @since 11.1
284      */
285     public void addAdditionalDerivativesProvider(final AdditionalDerivativesProvider provider) {
286 
287         // check if the name is already used
288         if (isAdditionalStateManaged(provider.getName())) {
289             // these derivatives are already registered, complain
290             throw new OrekitException(OrekitMessages.ADDITIONAL_STATE_NAME_ALREADY_IN_USE,
291                                       provider.getName());
292         }
293 
294         // this is really a new set of derivatives, add it
295         additionalDerivativesProviders.add(provider);
296 
297         secondaryOffsets.clear();
298 
299     }
300 
301     /** Get an unmodifiable list of providers for additional derivatives.
302      * @return providers for the additional derivatives
303      * @since 11.1
304      */
305     public List<AdditionalDerivativesProvider> getAdditionalDerivativesProviders() {
306         return Collections.unmodifiableList(additionalDerivativesProviders);
307     }
308 
309     /** {@inheritDoc} */
310     public void addEventDetector(final EventDetector detector) {
311         detectors.add(detector);
312     }
313 
314     /** {@inheritDoc} */
315     public Collection<EventDetector> getEventsDetectors() {
316         return Collections.unmodifiableCollection(detectors);
317     }
318 
319     /** {@inheritDoc} */
320     public void clearEventsDetectors() {
321         detectors.clear();
322     }
323 
324     /** Set up all user defined event detectors.
325      */
326     protected void setUpUserEventDetectors() {
327         for (final EventDetector detector : detectors) {
328             setUpEventDetector(integrator, detector);
329         }
330     }
331 
332     /** Wrap an Orekit event detector and register it to the integrator.
333      * @param integ integrator into which event detector should be registered
334      * @param detector event detector to wrap
335      */
336     protected void setUpEventDetector(final ODEIntegrator integ, final EventDetector detector) {
337         integ.addEventHandler(new AdaptedEventDetector(detector),
338                               detector.getMaxCheckInterval(),
339                               detector.getThreshold(),
340                               detector.getMaxIterationCount());
341     }
342 
343     /** {@inheritDoc} */
344     @Override
345     public EphemerisGenerator getEphemerisGenerator() {
346         final StoringStepHandler storingHandler = new StoringStepHandler();
347         ephemerisGenerators.add(storingHandler);
348         return storingHandler;
349     }
350 
351     /** Create a mapper between raw double components and spacecraft state.
352     /** Simple constructor.
353      * <p>
354      * The position parameter type is meaningful only if {@link
355      * #getOrbitType() propagation orbit type}
356      * support it. As an example, it is not meaningful for propagation
357      * in {@link OrbitType#CARTESIAN Cartesian} parameters.
358      * </p>
359      * @param referenceDate reference date
360      * @param mu central attraction coefficient (m³/s²)
361      * @param orbitType orbit type to use for mapping
362      * @param positionAngleType angle type to use for propagation
363      * @param attitudeProvider attitude provider
364      * @param frame inertial frame
365      * @return new mapper
366      */
367     protected abstract StateMapper createMapper(AbsoluteDate referenceDate, double mu,
368                                                 OrbitType orbitType, PositionAngle positionAngleType,
369                                                 AttitudeProvider attitudeProvider, Frame frame);
370 
371     /** Get the differential equations to integrate (for main state only).
372      * @param integ numerical integrator to use for propagation.
373      * @return differential equations for main state
374      */
375     protected abstract MainStateEquations getMainStateEquations(ODEIntegrator integ);
376 
377     /** {@inheritDoc} */
378     public SpacecraftState propagate(final AbsoluteDate target) {
379         if (getStartDate() == null) {
380             if (getInitialState() == null) {
381                 throw new OrekitException(OrekitMessages.INITIAL_STATE_NOT_SPECIFIED_FOR_ORBIT_PROPAGATION);
382             }
383             setStartDate(getInitialState().getDate());
384         }
385         return propagate(getStartDate(), target);
386     }
387 
388     /** {@inheritDoc} */
389     public SpacecraftState propagate(final AbsoluteDate tStart, final AbsoluteDate tEnd) {
390 
391         if (getInitialState() == null) {
392             throw new OrekitException(OrekitMessages.INITIAL_STATE_NOT_SPECIFIED_FOR_ORBIT_PROPAGATION);
393         }
394 
395         // make sure the integrator will be reset properly even if we change its events handlers and step handlers
396         try (IntegratorResetter resetter = new IntegratorResetter(integrator)) {
397 
398             // prepare handling of STM and Jacobian matrices
399             setUpStmAndJacobianGenerators();
400 
401             if (!tStart.equals(getInitialState().getDate())) {
402                 // if propagation start date is not initial date,
403                 // propagate from initial to start date without event detection
404                 try (IntegratorResetter startResetter = new IntegratorResetter(integrator)) {
405                     integrateDynamics(tStart);
406                 }
407             }
408 
409             // set up events added by user
410             setUpUserEventDetectors();
411 
412             // set up step handlers
413             for (final OrekitStepHandler handler : getMultiplexer().getHandlers()) {
414                 integrator.addStepHandler(new AdaptedStepHandler(handler));
415             }
416             for (final StoringStepHandler generator : ephemerisGenerators) {
417                 generator.setEndDate(tEnd);
418                 integrator.addStepHandler(generator);
419             }
420 
421             // propagate from start date to end date with event detection
422             final SpacecraftState finalState = integrateDynamics(tEnd);
423 
424             return finalState;
425 
426         }
427 
428     }
429 
430     /** Set up State Transition Matrix and Jacobian matrix handling.
431      * @since 11.1
432      */
433     protected void setUpStmAndJacobianGenerators() {
434         // nothing to do by default
435     }
436 
437     /** Propagation with or without event detection.
438      * @param tEnd target date to which orbit should be propagated
439      * @return state at end of propagation
440      */
441     private SpacecraftState integrateDynamics(final AbsoluteDate tEnd) {
442         try {
443 
444             initializePropagation();
445 
446             if (getInitialState().getDate().equals(tEnd)) {
447                 // don't extrapolate
448                 return getInitialState();
449             }
450 
451             // space dynamics view
452             stateMapper = createMapper(getInitialState().getDate(), stateMapper.getMu(),
453                                        stateMapper.getOrbitType(), stateMapper.getPositionAngleType(),
454                                        stateMapper.getAttitudeProvider(), getInitialState().getFrame());
455 
456 
457             if (Double.isNaN(getMu())) {
458                 setMu(getInitialState().getMu());
459             }
460 
461             if (getInitialState().getMass() <= 0.0) {
462                 throw new OrekitException(OrekitMessages.SPACECRAFT_MASS_BECOMES_NEGATIVE,
463                                           getInitialState().getMass());
464             }
465 
466             // convert space flight dynamics API to math API
467             final SpacecraftState initialIntegrationState = getInitialIntegrationState();
468             final ODEState mathInitialState = createInitialState(initialIntegrationState);
469             final ExpandableODE mathODE = createODE(integrator, mathInitialState);
470 
471             // mathematical integration
472             final ODEStateAndDerivative mathFinalState;
473             beforeIntegration(initialIntegrationState, tEnd);
474             mathFinalState = integrator.integrate(mathODE, mathInitialState,
475                                                   tEnd.durationFrom(getInitialState().getDate()));
476             afterIntegration();
477 
478             // get final state
479             SpacecraftState finalState =
480                             stateMapper.mapArrayToState(stateMapper.mapDoubleToDate(mathFinalState.getTime(),
481                                                                                     tEnd),
482                                                         mathFinalState.getPrimaryState(),
483                                                         mathFinalState.getPrimaryDerivative(),
484                                                         propagationType);
485 
486             if (!additionalDerivativesProviders.isEmpty()) {
487                 final double[] secondary            = mathFinalState.getSecondaryState(1);
488                 final double[] secondaryDerivatives = mathFinalState.getSecondaryDerivative(1);
489                 for (AdditionalDerivativesProvider provider : additionalDerivativesProviders) {
490                     final String   name        = provider.getName();
491                     final int      offset      = secondaryOffsets.get(name);
492                     final int      dimension   = provider.getDimension();
493                     finalState = finalState.
494                                  addAdditionalState(name, Arrays.copyOfRange(secondary, offset, offset + dimension)).
495                                  addAdditionalStateDerivative(name, Arrays.copyOfRange(secondaryDerivatives, offset, offset + dimension));
496                 }
497             }
498             finalState = updateAdditionalStates(finalState);
499 
500             if (resetAtEnd) {
501                 resetInitialState(finalState);
502                 setStartDate(finalState.getDate());
503             }
504 
505             return finalState;
506 
507         } catch (MathRuntimeException mre) {
508             throw OrekitException.unwrap(mre);
509         }
510     }
511 
512     /** Get the initial state for integration.
513      * @return initial state for integration
514      */
515     protected SpacecraftState getInitialIntegrationState() {
516         return getInitialState();
517     }
518 
519     /** Create an initial state.
520      * @param initialState initial state in flight dynamics world
521      * @return initial state in mathematics world
522      */
523     private ODEState createInitialState(final SpacecraftState initialState) {
524 
525         // retrieve initial state
526         final double[] primary = new double[getBasicDimension()];
527         stateMapper.mapStateToArray(initialState, primary, null);
528 
529         if (secondaryOffsets.isEmpty()) {
530             // compute dimension of the secondary state
531             int offset = 0;
532             for (final AdditionalDerivativesProvider provider : additionalDerivativesProviders) {
533                 secondaryOffsets.put(provider.getName(), offset);
534                 offset += provider.getDimension();
535             }
536             secondaryOffsets.put(SECONDARY_DIMENSION, offset);
537         }
538 
539         return new ODEState(0.0, primary, secondary(initialState));
540 
541     }
542 
543     /** Create secondary state.
544      * @param state spacecraft state
545      * @return secondary state
546      * @since 11.1
547      */
548     private double[][] secondary(final SpacecraftState state) {
549 
550         if (secondaryOffsets.isEmpty()) {
551             return null;
552         }
553 
554         final double[][] secondary = new double[1][secondaryOffsets.get(SECONDARY_DIMENSION)];
555         for (final AdditionalDerivativesProvider provider : additionalDerivativesProviders) {
556             final String   name       = provider.getName();
557             final int      offset     = secondaryOffsets.get(name);
558             final double[] additional = state.getAdditionalState(name);
559             System.arraycopy(additional, 0, secondary[0], offset, additional.length);
560         }
561 
562         return secondary;
563 
564     }
565 
566     /** Create secondary state derivative.
567      * @param state spacecraft state
568      * @return secondary state derivative
569      * @since 11.1
570      */
571     private double[][] secondaryDerivative(final SpacecraftState state) {
572 
573         if (secondaryOffsets.isEmpty()) {
574             return null;
575         }
576 
577         final double[][] secondaryDerivative = new double[1][secondaryOffsets.get(SECONDARY_DIMENSION)];
578         for (final AdditionalDerivativesProvider provider : additionalDerivativesProviders) {
579             final String   name       = provider.getName();
580             final int      offset     = secondaryOffsets.get(name);
581             final double[] additionalDerivative = state.getAdditionalStateDerivative(name);
582             System.arraycopy(additionalDerivative, 0, secondaryDerivative[0], offset, additionalDerivative.length);
583         }
584 
585         return secondaryDerivative;
586 
587     }
588 
589     /** Create an ODE with all equations.
590      * @param integ numerical integrator to use for propagation.
591      * @param mathInitialState initial state
592      * @return a new ode
593      */
594     private ExpandableODE createODE(final ODEIntegrator integ,
595                                     final ODEState mathInitialState) {
596 
597         final ExpandableODE ode =
598                 new ExpandableODE(new ConvertedMainStateEquations(getMainStateEquations(integ)));
599 
600         // secondary part of the ODE
601         if (!additionalDerivativesProviders.isEmpty()) {
602             ode.addSecondaryEquations(new ConvertedSecondaryStateEquations());
603         }
604 
605         return ode;
606 
607     }
608 
609     /** Method called just before integration.
610      * <p>
611      * The default implementation does nothing, it may be specialized in subclasses.
612      * </p>
613      * @param initialState initial state
614      * @param tEnd target date at which state should be propagated
615      */
616     protected void beforeIntegration(final SpacecraftState initialState,
617                                      final AbsoluteDate tEnd) {
618         // do nothing by default
619     }
620 
621     /** Method called just after integration.
622      * <p>
623      * The default implementation does nothing, it may be specialized in subclasses.
624      * </p>
625      */
626     protected void afterIntegration() {
627         // do nothing by default
628     }
629 
630     /** Get state vector dimension without additional parameters.
631      * @return state vector dimension without additional parameters.
632      */
633     public int getBasicDimension() {
634         return 7;
635     }
636 
637     /** Get the integrator used by the propagator.
638      * @return the integrator.
639      */
640     protected ODEIntegrator getIntegrator() {
641         return integrator;
642     }
643 
644     /** Convert a state from mathematical world to space flight dynamics world.
645      * @param os mathematical state
646      * @return space flight dynamics state
647      */
648     private SpacecraftState convert(final ODEStateAndDerivative os) {
649 
650         SpacecraftState s = stateMapper.mapArrayToState(os.getTime(),
651                                                         os.getPrimaryState(),
652                                                         os.getPrimaryDerivative(),
653                                                         propagationType);
654         if (os.getNumberOfSecondaryStates() > 0) {
655             final double[] secondary           = os.getSecondaryState(1);
656             final double[] secondaryDerivative = os.getSecondaryDerivative(1);
657             for (final AdditionalDerivativesProvider provider : additionalDerivativesProviders) {
658                 final String name      = provider.getName();
659                 final int    offset    = secondaryOffsets.get(name);
660                 final int    dimension = provider.getDimension();
661                 s = s.addAdditionalState(name, Arrays.copyOfRange(secondary, offset, offset + dimension));
662                 s = s.addAdditionalStateDerivative(name, Arrays.copyOfRange(secondaryDerivative, offset, offset + dimension));
663             }
664         }
665         s = updateAdditionalStates(s);
666 
667         return s;
668 
669     }
670 
671     /** Convert a state from space flight dynamics world to mathematical world.
672      * @param state space flight dynamics state
673      * @return mathematical state
674      */
675     private ODEStateAndDerivative convert(final SpacecraftState state) {
676 
677         // retrieve initial state
678         final double[] primary    = new double[getBasicDimension()];
679         final double[] primaryDot = new double[getBasicDimension()];
680         stateMapper.mapStateToArray(state, primary, primaryDot);
681 
682         // secondary part of the ODE
683         final double[][] secondary           = secondary(state);
684         final double[][] secondaryDerivative = secondaryDerivative(state);
685 
686         return new ODEStateAndDerivative(stateMapper.mapDateToDouble(state.getDate()),
687                                          primary, primaryDot,
688                                          secondary, secondaryDerivative);
689 
690     }
691 
692     /** Differential equations for the main state (orbit, attitude and mass). */
693     public interface MainStateEquations {
694 
695         /**
696          * Initialize the equations at the start of propagation. This method will be
697          * called before any calls to {@link #computeDerivatives(SpacecraftState)}.
698          *
699          * <p> The default implementation of this method does nothing.
700          *
701          * @param initialState initial state information at the start of propagation.
702          * @param target       date of propagation. Not equal to {@code
703          *                     initialState.getDate()}.
704          */
705         default void init(final SpacecraftState initialState, final AbsoluteDate target) {
706         }
707 
708         /** Compute differential equations for main state.
709          * @param state current state
710          * @return derivatives of main state
711          */
712         double[] computeDerivatives(SpacecraftState state);
713 
714     }
715 
716     /** Differential equations for the main state (orbit, attitude and mass), with converted API. */
717     private class ConvertedMainStateEquations implements OrdinaryDifferentialEquation {
718 
719         /** Main state equations. */
720         private final MainStateEquations main;
721 
722         /** Simple constructor.
723          * @param main main state equations
724          */
725         ConvertedMainStateEquations(final MainStateEquations main) {
726             this.main = main;
727             calls = 0;
728         }
729 
730         /** {@inheritDoc} */
731         public int getDimension() {
732             return getBasicDimension();
733         }
734 
735         @Override
736         public void init(final double t0, final double[] y0, final double finalTime) {
737             // update space dynamics view
738             SpacecraftState initialState = stateMapper.mapArrayToState(t0, y0, null, PropagationType.MEAN);
739             initialState = updateAdditionalStates(initialState);
740             final AbsoluteDate target = stateMapper.mapDoubleToDate(finalTime);
741             main.init(initialState, target);
742         }
743 
744         /** {@inheritDoc} */
745         public double[] computeDerivatives(final double t, final double[] y) {
746 
747             // increment calls counter
748             ++calls;
749 
750             // update space dynamics view
751             SpacecraftState currentState = stateMapper.mapArrayToState(t, y, null, PropagationType.MEAN);
752             currentState = updateAdditionalStates(currentState);
753 
754             // compute main state differentials
755             return main.computeDerivatives(currentState);
756 
757         }
758 
759     }
760 
761     /** Differential equations for the secondary state (Jacobians, user variables ...), with converted API. */
762     private class ConvertedSecondaryStateEquations implements SecondaryODE {
763 
764         /** Dimension of the combined additional states. */
765         private final int combinedDimension;
766 
767         /** Simple constructor.
768           */
769         ConvertedSecondaryStateEquations() {
770             this.combinedDimension = secondaryOffsets.get(SECONDARY_DIMENSION);
771         }
772 
773         /** {@inheritDoc} */
774         @Override
775         public int getDimension() {
776             return combinedDimension;
777         }
778 
779         /** {@inheritDoc} */
780         @Override
781         public void init(final double t0, final double[] primary0,
782                          final double[] secondary0, final double finalTime) {
783             // update space dynamics view
784             final SpacecraftState initialState = convert(t0, primary0, null, secondary0);
785 
786             final AbsoluteDate target = stateMapper.mapDoubleToDate(finalTime);
787             for (final AdditionalDerivativesProvider provider : additionalDerivativesProviders) {
788                 provider.init(initialState, target);
789             }
790 
791         }
792 
793         /** {@inheritDoc} */
794         @Override
795         public double[] computeDerivatives(final double t, final double[] primary,
796                                            final double[] primaryDot, final double[] secondary) {
797 
798             // update space dynamics view
799             // the integrable generators generate method will be called here,
800             // according to the generators yield order
801             SpacecraftState updated = convert(t, primary, primaryDot, secondary);
802 
803             // set up queue for equations
804             final Queue<AdditionalDerivativesProvider> pending = new LinkedList<>(additionalDerivativesProviders);
805 
806             // gather the derivatives from all additional equations, taking care of dependencies
807             final double[] secondaryDot = new double[combinedDimension];
808             int yieldCount = 0;
809             while (!pending.isEmpty()) {
810                 final AdditionalDerivativesProvider provider = pending.remove();
811                 if (provider.yield(updated)) {
812                     // this provider has to wait for another one,
813                     // we put it again in the pending queue
814                     pending.add(provider);
815                     if (++yieldCount >= pending.size()) {
816                         // all pending providers yielded!, they probably need data not yet initialized
817                         // we let the propagation proceed, if these data are really needed right now
818                         // an appropriate exception will be triggered when caller tries to access them
819                         break;
820                     }
821                 } else {
822                     // we can use these equations right now
823                     final String   name        = provider.getName();
824                     final int      offset      = secondaryOffsets.get(name);
825                     final int      dimension   = provider.getDimension();
826                     final double[] derivatives = provider.derivatives(updated);
827                     System.arraycopy(derivatives, 0, secondaryDot, offset, dimension);
828                     updated = updated.addAdditionalStateDerivative(name, derivatives);
829                     yieldCount = 0;
830                 }
831             }
832 
833             return secondaryDot;
834 
835         }
836 
837         /** Convert mathematical view to space view.
838          * @param t current value of the independent <I>time</I> variable
839          * @param primary array containing the current value of the primary state vector
840          * @param primaryDot array containing the derivative of the primary state vector
841          * @param secondary array containing the current value of the secondary state vector
842          * @return space view of the state
843          */
844         private SpacecraftState convert(final double t, final double[] primary,
845                                         final double[] primaryDot, final double[] secondary) {
846 
847             SpacecraftState initialState = stateMapper.mapArrayToState(t, primary, primaryDot, PropagationType.MEAN);
848 
849             for (final AdditionalDerivativesProvider provider : additionalDerivativesProviders) {
850                 final String name      = provider.getName();
851                 final int    offset    = secondaryOffsets.get(name);
852                 final int    dimension = provider.getDimension();
853                 initialState = initialState.addAdditionalState(name, Arrays.copyOfRange(secondary, offset, offset + dimension));
854             }
855 
856             return updateAdditionalStates(initialState);
857 
858         }
859 
860     }
861 
862     /** Adapt an {@link org.orekit.propagation.events.EventDetector}
863      * to Hipparchus {@link org.hipparchus.ode.events.ODEEventHandler} interface.
864      * @author Fabien Maussion
865      */
866     private class AdaptedEventDetector implements ODEEventHandler {
867 
868         /** Underlying event detector. */
869         private final EventDetector detector;
870 
871         /** Time of the previous call to g. */
872         private double lastT;
873 
874         /** Value from the previous call to g. */
875         private double lastG;
876 
877         /** Build a wrapped event detector.
878          * @param detector event detector to wrap
879         */
880         AdaptedEventDetector(final EventDetector detector) {
881             this.detector = detector;
882             this.lastT    = Double.NaN;
883             this.lastG    = Double.NaN;
884         }
885 
886         /** {@inheritDoc} */
887         public void init(final ODEStateAndDerivative s0, final double t) {
888             detector.init(convert(s0), stateMapper.mapDoubleToDate(t));
889             this.lastT = Double.NaN;
890             this.lastG = Double.NaN;
891         }
892 
893         /** {@inheritDoc} */
894         public double g(final ODEStateAndDerivative s) {
895             if (!Precision.equals(lastT, s.getTime(), 0)) {
896                 lastT = s.getTime();
897                 lastG = detector.g(convert(s));
898             }
899             return lastG;
900         }
901 
902         /** {@inheritDoc} */
903         public Action eventOccurred(final ODEStateAndDerivative s, final boolean increasing) {
904             return detector.eventOccurred(convert(s), increasing);
905         }
906 
907         /** {@inheritDoc} */
908         public ODEState resetState(final ODEStateAndDerivative s) {
909 
910             final SpacecraftState oldState = convert(s);
911             final SpacecraftState newState = detector.resetState(oldState);
912             stateChanged(newState);
913 
914             // main part
915             final double[] primary    = new double[s.getPrimaryStateDimension()];
916             stateMapper.mapStateToArray(newState, primary, null);
917 
918             // secondary part
919             final double[][] secondary = new double[1][secondaryOffsets.get(SECONDARY_DIMENSION)];
920             for (final AdditionalDerivativesProvider provider : additionalDerivativesProviders) {
921                 final String name      = provider.getName();
922                 final int    offset    = secondaryOffsets.get(name);
923                 final int    dimension = provider.getDimension();
924                 System.arraycopy(newState.getAdditionalState(name), 0, secondary[0], offset, dimension);
925             }
926 
927             return new ODEState(newState.getDate().durationFrom(getStartDate()),
928                                 primary, secondary);
929 
930         }
931 
932     }
933 
934     /** Adapt an {@link org.orekit.propagation.sampling.OrekitStepHandler}
935      * to Hipparchus {@link ODEStepHandler} interface.
936      * @author Luc Maisonobe
937      */
938     private class AdaptedStepHandler implements ODEStepHandler {
939 
940         /** Underlying handler. */
941         private final OrekitStepHandler handler;
942 
943         /** Build an instance.
944          * @param handler underlying handler to wrap
945          */
946         AdaptedStepHandler(final OrekitStepHandler handler) {
947             this.handler = handler;
948         }
949 
950         /** {@inheritDoc} */
951         public void init(final ODEStateAndDerivative s0, final double t) {
952             handler.init(convert(s0), stateMapper.mapDoubleToDate(t));
953         }
954 
955         /** {@inheritDoc} */
956         @Override
957         public void handleStep(final ODEStateInterpolator interpolator) {
958             handler.handleStep(new AdaptedStepInterpolator(interpolator));
959         }
960 
961         /** {@inheritDoc} */
962         @Override
963         public void finish(final ODEStateAndDerivative finalState) {
964             handler.finish(convert(finalState));
965         }
966 
967     }
968 
969     /** Adapt an Hipparchus {@link ODEStateInterpolator}
970      * to an orekit {@link OrekitStepInterpolator} interface.
971      * @author Luc Maisonobe
972      */
973     private class AdaptedStepInterpolator implements OrekitStepInterpolator {
974 
975         /** Underlying raw rawInterpolator. */
976         private final ODEStateInterpolator mathInterpolator;
977 
978         /** Simple constructor.
979          * @param mathInterpolator underlying raw interpolator
980          */
981         AdaptedStepInterpolator(final ODEStateInterpolator mathInterpolator) {
982             this.mathInterpolator = mathInterpolator;
983         }
984 
985         /** {@inheritDoc}} */
986         @Override
987         public SpacecraftState getPreviousState() {
988             return convert(mathInterpolator.getPreviousState());
989         }
990 
991         /** {@inheritDoc}} */
992         @Override
993         public boolean isPreviousStateInterpolated() {
994             return mathInterpolator.isPreviousStateInterpolated();
995         }
996 
997         /** {@inheritDoc}} */
998         @Override
999         public SpacecraftState getCurrentState() {
1000             return convert(mathInterpolator.getCurrentState());
1001         }
1002 
1003         /** {@inheritDoc}} */
1004         @Override
1005         public boolean isCurrentStateInterpolated() {
1006             return mathInterpolator.isCurrentStateInterpolated();
1007         }
1008 
1009         /** {@inheritDoc}} */
1010         @Override
1011         public SpacecraftState getInterpolatedState(final AbsoluteDate date) {
1012             return convert(mathInterpolator.getInterpolatedState(date.durationFrom(stateMapper.getReferenceDate())));
1013         }
1014 
1015         /** {@inheritDoc}} */
1016         @Override
1017         public boolean isForward() {
1018             return mathInterpolator.isForward();
1019         }
1020 
1021         /** {@inheritDoc}} */
1022         @Override
1023         public AdaptedStepInterpolator restrictStep(final SpacecraftState newPreviousState,
1024                                                     final SpacecraftState newCurrentState) {
1025             try {
1026                 final AbstractODEStateInterpolator aosi = (AbstractODEStateInterpolator) mathInterpolator;
1027                 return new AdaptedStepInterpolator(aosi.restrictStep(convert(newPreviousState),
1028                                                                      convert(newCurrentState)));
1029             } catch (ClassCastException cce) {
1030                 // this should never happen
1031                 throw new OrekitInternalError(cce);
1032             }
1033         }
1034 
1035     }
1036 
1037     /** Specialized step handler storing interpolators for ephemeris generation.
1038      * @since 11.0
1039      */
1040     private class StoringStepHandler implements ODEStepHandler, EphemerisGenerator {
1041 
1042         /** Underlying raw mathematical model. */
1043         private DenseOutputModel model;
1044 
1045         /** the user supplied end date. Propagation may not end on this date. */
1046         private AbsoluteDate endDate;
1047 
1048         /** Generated ephemeris. */
1049         private BoundedPropagator ephemeris;
1050 
1051         /** Variable used to store the last interpolator handled by the object.*/
1052         private  ODEStateInterpolator lastInterpolator;
1053 
1054         /** Set the end date.
1055          * @param endDate end date
1056          */
1057         public void setEndDate(final AbsoluteDate endDate) {
1058             this.endDate = endDate;
1059         }
1060 
1061         /** {@inheritDoc} */
1062         @Override
1063         public void init(final ODEStateAndDerivative s0, final double t) {
1064 
1065             this.model = new DenseOutputModel();
1066             model.init(s0, t);
1067 
1068             // ephemeris will be generated when last step is processed
1069             this.ephemeris = null;
1070 
1071             this.lastInterpolator = null;
1072 
1073         }
1074 
1075         /** {@inheritDoc} */
1076         @Override
1077         public BoundedPropagator getGeneratedEphemeris() {
1078             // Each time we try to get the ephemeris, rebuild it using the last data.
1079             buildEphemeris();
1080             return ephemeris;
1081         }
1082 
1083         /** {@inheritDoc} */
1084         @Override
1085         public void handleStep(final ODEStateInterpolator interpolator) {
1086             model.handleStep(interpolator);
1087             lastInterpolator = interpolator;
1088         }
1089 
1090         /** {@inheritDoc} */
1091         @Override
1092         public void finish(final ODEStateAndDerivative finalState) {
1093             buildEphemeris();
1094         }
1095 
1096         /** Method used to produce ephemeris at a given time.
1097          * Can be used at multiple times, updating the ephemeris to
1098          * its last state.
1099          */
1100         private void buildEphemeris() {
1101             // buildEphemeris was built in order to allow access to what was previously the finish method.
1102             // This now allows to call it through getGeneratedEphemeris, therefore through an external call,
1103             // which was not previously the case.
1104 
1105             // Update the model's finalTime with the last interpolator.
1106             model.finish(lastInterpolator.getCurrentState());
1107             // set up the boundary dates
1108             final double tI = model.getInitialTime();
1109             final double tF = model.getFinalTime();
1110             // tI is almost? always zero
1111             final AbsoluteDate startDate =
1112                             stateMapper.mapDoubleToDate(tI);
1113             final AbsoluteDate finalDate =
1114                             stateMapper.mapDoubleToDate(tF, this.endDate);
1115             final AbsoluteDate minDate;
1116             final AbsoluteDate maxDate;
1117             if (tF < tI) {
1118                 minDate = finalDate;
1119                 maxDate = startDate;
1120             } else {
1121                 minDate = startDate;
1122                 maxDate = finalDate;
1123             }
1124 
1125             // get the initial additional states that are not managed
1126             final DoubleArrayDictionary unmanaged = new DoubleArrayDictionary();
1127             for (final DoubleArrayDictionary.Entry initial : getInitialState().getAdditionalStatesValues().getData()) {
1128                 if (!isAdditionalStateManaged(initial.getKey())) {
1129                     // this additional state was in the initial state, but is unknown to the propagator
1130                     // we simply copy its initial value as is
1131                     unmanaged.put(initial.getKey(), initial.getValue());
1132                 }
1133             }
1134 
1135             // get the names of additional states managed by differential equations
1136             final String[] names      = new String[additionalDerivativesProviders.size()];
1137             final int[]    dimensions = new int[additionalDerivativesProviders.size()];
1138             for (int i = 0; i < names.length; ++i) {
1139                 names[i] = additionalDerivativesProviders.get(i).getName();
1140                 dimensions[i] = additionalDerivativesProviders.get(i).getDimension();
1141             }
1142 
1143             // create the ephemeris
1144             ephemeris = new IntegratedEphemeris(startDate, minDate, maxDate,
1145                                                 stateMapper, propagationType, model,
1146                                                 unmanaged, getAdditionalStateProviders(),
1147                                                 names, dimensions);
1148 
1149         }
1150 
1151     }
1152 
1153     /** Wrapper for resetting an integrator handlers.
1154      * <p>
1155      * This class is intended to be used in a try-with-resource statement.
1156      * If propagator-specific event handlers and step handlers are added to
1157      * the integrator in the try block, they will be removed automatically
1158      * when leaving the block, so the integrator only keeps its own handlers
1159      * between calls to {@link AbstractIntegratedPropagator#propagate(AbsoluteDate, AbsoluteDate).
1160      * </p>
1161      * @since 11.0
1162      */
1163     private static class IntegratorResetter implements AutoCloseable {
1164 
1165         /** Wrapped integrator. */
1166         private final ODEIntegrator integrator;
1167 
1168         /** Initial event handlers list. */
1169         private final List<EventHandlerConfiguration> eventHandlersConfigurations;
1170 
1171         /** Initial step handlers list. */
1172         private final List<ODEStepHandler> stepHandlers;
1173 
1174         /** Simple constructor.
1175          * @param integrator wrapped integrator
1176          */
1177         IntegratorResetter(final ODEIntegrator integrator) {
1178             this.integrator                  = integrator;
1179             this.eventHandlersConfigurations = new ArrayList<>(integrator.getEventHandlersConfigurations());
1180             this.stepHandlers                = new ArrayList<>(integrator.getStepHandlers());
1181         }
1182 
1183         /** {@inheritDoc}
1184          * <p>
1185          * Reset event handlers and step handlers back to the initial list
1186          * </p>
1187          */
1188         @Override
1189         public void close() {
1190 
1191             // reset event handlers
1192             integrator.clearEventHandlers();
1193             eventHandlersConfigurations.forEach(c -> integrator.addEventHandler(c.getEventHandler(),
1194                                                                                 c.getMaxCheckInterval(),
1195                                                                                 c.getConvergence(),
1196                                                                                 c.getMaxIterationCount(),
1197                                                                                 c.getSolver()));
1198 
1199             // reset step handlers
1200             integrator.clearStepHandlers();
1201             stepHandlers.forEach(stepHandler -> integrator.addStepHandler(stepHandler));
1202 
1203         }
1204 
1205     }
1206 
1207 }