/* Copyright 2022-2025 Romain Serra
    * 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.control.indirect.shooting;
  
  import org.hipparchus.CalculusFieldElement;
  import org.hipparchus.Field;
  import org.hipparchus.analysis.differentiation.Gradient;
  import org.hipparchus.analysis.differentiation.GradientField;
  import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  import org.hipparchus.linear.LUDecomposition;
  import org.hipparchus.linear.MatrixUtils;
  import org.hipparchus.linear.RealMatrix;
  import org.hipparchus.ode.FieldOrdinaryDifferentialEquation;
  import org.hipparchus.ode.nonstiff.FieldExplicitRungeKuttaIntegrator;
  import org.hipparchus.util.FastMath;
  import org.hipparchus.util.MathArrays;
  import org.orekit.attitudes.FieldAttitude;
  import org.orekit.control.indirect.adjoint.CartesianAdjointDerivativesProvider;
  import org.orekit.control.indirect.adjoint.FieldCartesianAdjointDerivativesProvider;
  import org.orekit.control.indirect.adjoint.cost.ControlSwitchDetector;
  import org.orekit.control.indirect.adjoint.cost.FieldControlSwitchDetector;
  import org.orekit.control.indirect.shooting.propagation.AdjointDynamicsProvider;
  import org.orekit.control.indirect.shooting.propagation.ShootingPropagationSettings;
  import org.orekit.forces.ForceModel;
  import org.orekit.frames.Frame;
  import org.orekit.orbits.FieldCartesianOrbit;
  import org.orekit.propagation.FieldSpacecraftState;
  import org.orekit.propagation.SpacecraftState;
  import org.orekit.propagation.events.EventsLogger;
  import org.orekit.propagation.events.FieldEventDetector;
  import org.orekit.propagation.integration.FieldAdditionalDerivativesProvider;
  import org.orekit.propagation.integration.FieldCombinedDerivatives;
  import org.orekit.propagation.numerical.NumericalPropagator;
  import org.orekit.propagation.sampling.PropagationStepRecorder;
  import org.orekit.time.AbsoluteDate;
  import org.orekit.time.FieldAbsoluteDate;
  import org.orekit.utils.FieldAbsolutePVCoordinates;
  import org.orekit.utils.FieldPVCoordinates;
  
  import java.util.Arrays;
  import java.util.List;
  import java.util.stream.Collectors;
  
  /**
   * Abstract class for indirect single shooting methods with fixed initial Cartesian state.
   * Inheritors must implement the iteration update, assuming derivatives are needed.
   *
   * @author Romain Serra
   * @since 13.0
   * @see CartesianAdjointDerivativesProvider
   * @see org.orekit.control.indirect.adjoint.FieldCartesianAdjointDerivativesProvider
   */
  public abstract class AbstractFixedInitialCartesianSingleShooting extends AbstractIndirectShooting {
  
      /** Template for initial state. Contains the initial Cartesian coordinates. */
      private final SpacecraftState initialSpacecraftStateTemplate;
  
      /**
       * Step handler to record propagation steps.
       */
      private final PropagationStepRecorder propagationStepRecorder;
  
      /** Event logger. */
      private final EventsLogger eventsLogger;
  
      /** Scales for automatic differentiation variables. */
      private double[] scales;
  
      /**
       * Constructor with boundary conditions as orbits.
       * @param propagationSettings propagation settings
       * @param initialSpacecraftStateTemplate template for initial propagation state
       */
      protected AbstractFixedInitialCartesianSingleShooting(final ShootingPropagationSettings propagationSettings,
                                                            final SpacecraftState initialSpacecraftStateTemplate) {
          super(propagationSettings);
          this.initialSpacecraftStateTemplate = initialSpacecraftStateTemplate;
          this.propagationStepRecorder = new PropagationStepRecorder();
          this.eventsLogger = new EventsLogger();
      }
  
      /**
       * Build unit scales.
       * @return scales
       */
     private double[] getUnitScales() {
         final double[] unitScales = new double[getPropagationSettings().getAdjointDynamicsProvider().getDimension()];
         Arrays.fill(unitScales, 1.);
         return unitScales;
     }
 
     /**
      * Protected getter for the differentiation scales.
      * @return scales for variable scales
      */
     protected double[] getScales() {
         return scales;
     }
 
     /**
      * Returns the maximum number of iterations.
      * @return maximum iterations
      */
     public abstract int getMaximumIterationCount();
 
     /** {@inheritDoc} */
     @Override
     public ShootingBoundaryOutput solve(final double initialMass, final double[] initialGuess) {
         return solve(initialMass, initialGuess, getUnitScales());
     }
 
     /**
      * Solve for the boundary conditions, given an initial mass and an initial guess for the adjoint variables.
      * Uses scales for automatic differentiation.
      * @param initialMass initial mass
      * @param initialGuess initial guess
      * @param userScales scales
      * @return boundary problem solution
      */
     public ShootingBoundaryOutput solve(final double initialMass, final double[] initialGuess,
                                         final double[] userScales) {
         scales = userScales.clone();
         final SpacecraftState initialState = createStateWithMassAndAdjoint(initialMass, initialGuess);
         final ShootingBoundaryOutput initialGuessSolution = computeCandidateSolution(initialState, 0);
         if (initialGuessSolution.isConverged()) {
             return initialGuessSolution;
         } else {
             return iterate(initialGuessSolution);
         }
     }
 
     /** {@inheritDoc} */
     @Override
     protected NumericalPropagator buildPropagator(final SpacecraftState initialState) {
         final NumericalPropagator propagator = super.buildPropagator(initialState);
         propagator.setStepHandler(propagationStepRecorder);
         final CartesianAdjointDerivativesProvider derivativesProvider = (CartesianAdjointDerivativesProvider)
             getPropagationSettings().getAdjointDynamicsProvider().buildAdditionalDerivativesProvider();
         eventsLogger.clearLoggedEvents();
         derivativesProvider.getCost().getEventDetectors()
                 .forEach(detector -> propagator.addEventDetector(eventsLogger.monitorDetector(detector)));
         return propagator;
     }
 
     /**
      * Form solution container with input initial state.
      * @param initialState initial state
      * @param iterationCount iteration count
      * @return candidate solution
      */
     public abstract ShootingBoundaryOutput computeCandidateSolution(SpacecraftState initialState, int iterationCount);
 
     /**
      * Iterate on initial guess to solve boundary problem.
      * @param initialGuess initial guess
      * @return solution (or null pointer if not converged)
      */
     private ShootingBoundaryOutput iterate(final ShootingBoundaryOutput initialGuess) {
         int iterationCount = 1;
         SpacecraftState initialState = initialGuess.getInitialState();
         ShootingBoundaryOutput candidateSolution = initialGuess;
         final String adjointName = getPropagationSettings().getAdjointDynamicsProvider().getAdjointName();
         double[] initialAdjoint = initialState.getAdditionalState(adjointName).clone();
         final double initialMass = initialState.getMass();
         while (iterationCount < getMaximumIterationCount()) {
             if (candidateSolution.isConverged()) {
                 return candidateSolution;
             }
             final FieldSpacecraftState<Gradient> fieldInitialState = createFieldInitialStateWithMassAndAdjoint(initialMass,
                 initialAdjoint);
             initialState = fieldInitialState.toSpacecraftState();
             final FieldSpacecraftState<Gradient> fieldTerminalState = propagateField(fieldInitialState);
             initialAdjoint = updateShootingVariables(initialAdjoint, fieldTerminalState);
             if (Double.isNaN(initialAdjoint[0])) {
                 return candidateSolution;
             } else {
                 candidateSolution = computeCandidateSolution(initialState, iterationCount);
             }
             iterationCount++;
         }
         return candidateSolution;
     }
 
     /**
      * Propagate in Field. Does not use a full propagator (only the integrator, moving step by step using the history of non-Field propagation).
      * It is faster as adaptive step and event detection are particularly slow with Field.
      * @param fieldInitialState initial state
      * @return terminal state
      */
     private FieldSpacecraftState<Gradient> propagateField(final FieldSpacecraftState<Gradient> fieldInitialState) {
         final FieldAbsoluteDate<Gradient> initialDate = fieldInitialState.getDate();
         final FieldExplicitRungeKuttaIntegrator<Gradient> fieldIntegrator = buildFieldIntegrator(fieldInitialState);
         final FieldOrdinaryDifferentialEquation<Gradient> fieldODE = buildFieldODE(fieldInitialState.getDate());
         final AdjointDynamicsProvider dynamicsProvider = getPropagationSettings().getAdjointDynamicsProvider();
         AbsoluteDate date = initialDate.toAbsoluteDate();
         Gradient[] integrationState = formatToArray(fieldInitialState, dynamicsProvider.getAdjointName());
         // step-by-step integration
         final List<EventsLogger.LoggedEvent> loggedEvents = eventsLogger.getLoggedEvents();
         final List<AbsoluteDate> stepDates = propagationStepRecorder.copyStates().stream().map(SpacecraftState::getDate)
                 .collect(Collectors.toList());
         for (final AbsoluteDate stepDate: stepDates) {
             final Gradient time = initialDate.durationFrom(date).negate();
             final Gradient nextTime = initialDate.durationFrom(stepDate).negate();
             integrationState = fieldIntegrator.singleStep(fieldODE, time, integrationState, nextTime);
             // deal with switch event if any
             if (!loggedEvents.isEmpty()) {
                 for (final EventsLogger.LoggedEvent loggedEvent: loggedEvents) {
                     final SpacecraftState loggedState = loggedEvent.getState();
                     if (loggedEvent.getEventDetector() instanceof ControlSwitchDetector && loggedState.getDate().isEqualTo(stepDate)) {
                         final ControlSwitchDetector switchDetector = (ControlSwitchDetector) loggedEvent.getEventDetector();
                         integrationState = findSwitchEventAndUpdateState(date, integrationState,
                                 initialDate.toAbsoluteDate(), switchDetector, dynamicsProvider);
                     }
                 }
             }
             date = new AbsoluteDate(stepDate);
         }
         return formatFromArray(date, integrationState);
     }
 
     /**
      * Create initial state with input mass.
      * @param initialMass initial mass
      * @return state
      */
     protected SpacecraftState createInitialStateWithMass(final double initialMass) {
         return initialSpacecraftStateTemplate.withMass(initialMass);
     }
 
     /**
      * Create initial state with input mass and adjoint vector.
      * @param initialMass initial mass
      * @param initialAdjoint initial adjoint variables
      * @return state
      */
     private SpacecraftState createStateWithMassAndAdjoint(final double initialMass, final double[] initialAdjoint) {
         final String adjointName = getPropagationSettings().getAdjointDynamicsProvider().getAdjointName();
         return createInitialStateWithMass(initialMass).addAdditionalData(adjointName, initialAdjoint);
     }
 
     /**
      * Create initial Gradient state with input mass and adjoint vector, the latter being the independent variables.
      * @param initialMass initial mass
      * @param initialAdjoint initial adjoint variables
      * @return state
      */
     protected FieldSpacecraftState<Gradient> createFieldInitialStateWithMassAndAdjoint(final double initialMass,
                                                                                        final double[] initialAdjoint) {
         final int parametersNumber = initialAdjoint.length;
         final GradientField field = GradientField.getField(parametersNumber);
         final FieldSpacecraftState<Gradient> stateWithoutAdjoint = new FieldSpacecraftState<>(field,
                 createInitialStateWithMass(initialMass));
         final Gradient[] fieldInitialAdjoint = MathArrays.buildArray(field, initialAdjoint.length);
         for (int i = 0; i < parametersNumber; i++) {
             fieldInitialAdjoint[i] = Gradient.variable(parametersNumber, i, 0.).multiply(getScales()[i]).add(initialAdjoint[i]);
         }
         final String adjointName = getPropagationSettings().getAdjointDynamicsProvider().getAdjointName();
         return stateWithoutAdjoint.addAdditionalData(adjointName, fieldInitialAdjoint);
     }
 
     /**
      * Create state.
      * @param date epoch
      * @param cartesian Cartesian variables
      * @param mass mass
      * @param adjoint adjoint variables
      * @param <T> field type
      * @return state
      */
     protected <T extends CalculusFieldElement<T>> FieldSpacecraftState<T> createFieldState(final FieldAbsoluteDate<T> date,
                                                                                            final T[] cartesian,
                                                                                            final T mass,
                                                                                            final T[] adjoint) {
         final Field<T> field = mass.getField();
         final Frame frame = getPropagationSettings().getPropagationFrame();
         final FieldVector3D<T> position = new FieldVector3D<>(Arrays.copyOfRange(cartesian, 0, 3));
         final FieldVector3D<T> velocity = new FieldVector3D<>(Arrays.copyOfRange(cartesian, 3, 6));
         final FieldPVCoordinates<T> pvCoordinates = new FieldPVCoordinates<>(position, velocity);
         final FieldSpacecraftState<T> stateWithoutAdjoint;
         if (initialSpacecraftStateTemplate.isOrbitDefined()) {
             final T mu = field.getZero().newInstance(initialSpacecraftStateTemplate.getOrbit().getMu());
             final FieldCartesianOrbit<T> orbit = new FieldCartesianOrbit<>(pvCoordinates, frame, date, mu);
             final FieldAttitude<T> attitude = getPropagationSettings().getAttitudeProvider().getAttitude(orbit,
                     orbit.getDate(), orbit.getFrame());
             stateWithoutAdjoint = new FieldSpacecraftState<>(orbit, attitude).withMass(mass);
         } else {
             final FieldAbsolutePVCoordinates<T> absolutePVCoordinates = new FieldAbsolutePVCoordinates<>(frame, date,
                     pvCoordinates);
             final FieldAttitude<T> attitude = getPropagationSettings().getAttitudeProvider().getAttitude(absolutePVCoordinates,
                     absolutePVCoordinates.getDate(), absolutePVCoordinates.getFrame());
             stateWithoutAdjoint = new FieldSpacecraftState<>(absolutePVCoordinates, attitude).withMass(mass);
         }
         final String adjointName = getPropagationSettings().getAdjointDynamicsProvider().getAdjointName();
         return stateWithoutAdjoint.addAdditionalData(adjointName, adjoint);
     }
 
     /**
      * Update shooting variables.
      * @param originalShootingVariables original shooting variables (before update)
      * @param fieldTerminalState final state of gradient propagation
      * @return updated shooting variables
      */
     protected abstract double[] updateShootingVariables(double[] originalShootingVariables,
                                                         FieldSpacecraftState<Gradient> fieldTerminalState);
 
     /**
      * Build Ordinary Differential Equation for Field.
      * @param referenceDate date defining the origin of times
      * @param <T> field type
      * @return Field ODE
      * @since 13.0
      */
     protected <T extends CalculusFieldElement<T>> FieldOrdinaryDifferentialEquation<T> buildFieldODE(final FieldAbsoluteDate<T> referenceDate) {
         final Field<T> field = referenceDate.getField();
         final AdjointDynamicsProvider adjointDynamicsProvider = getPropagationSettings().getAdjointDynamicsProvider();
         final FieldAdditionalDerivativesProvider<T> derivativesProvider = adjointDynamicsProvider
                 .buildFieldAdditionalDerivativesProvider(field);
 
         return new FieldOrdinaryDifferentialEquation<T>() {
             @Override
             public int getDimension() {
                 return 7 + adjointDynamicsProvider.getDimension();
             }
 
             @Override
             public T[] computeDerivatives(final T t, final T[] y) {
                 // build state
                 final T[] cartesian = Arrays.copyOfRange(y, 0, 6);
                 final FieldAbsoluteDate<T> date = referenceDate.shiftedBy(t);
                 final Field<T> field = date.getField();
                 final T zero = field.getZero();
                 final T[] adjoint = Arrays.copyOfRange(y, 7, y.length);
                 final FieldSpacecraftState<T> state = createFieldState(date, cartesian, y[6], adjoint);
                 // compute derivatives
                 final T[] yDot = MathArrays.buildArray(field, getDimension());
                 yDot[0] = zero.add(y[3]);
                 yDot[1] = zero.add(y[4]);
                 yDot[2] = zero.add(y[5]);
                 FieldVector3D<T> totalAcceleration = FieldVector3D.getZero(field);
                 for (final ForceModel forceModel: getPropagationSettings().getForceModels()) {
                     final FieldVector3D<T> acceleration = forceModel.acceleration(state, forceModel.getParameters(field));
                     totalAcceleration = totalAcceleration.add(acceleration);
                 }
                 yDot[3] = totalAcceleration.getX();
                 yDot[4] = totalAcceleration.getY();
                 yDot[5] = totalAcceleration.getZ();
                 final FieldCombinedDerivatives<T> combinedDerivatives = derivativesProvider.combinedDerivatives(state);
                 final T[] cartesianDotContribution = combinedDerivatives.getMainStateDerivativesIncrements();
                 for (int i = 0; i < cartesianDotContribution.length; i++) {
                     yDot[i] = yDot[i].add(cartesianDotContribution[i]);
                 }
                 final T[] adjointDot = combinedDerivatives.getAdditionalDerivatives();
                 System.arraycopy(adjointDot, 0, yDot, yDot.length - adjointDot.length, adjointDot.length);
                 return yDot;
             }
         };
     }
 
     /**
      * Form array from Orekit object.
      * @param fieldState state
      * @param adjointName adjoint name
      * @return propagation state as array
      */
     private Gradient[] formatToArray(final FieldSpacecraftState<Gradient> fieldState,
                                      final String adjointName) {
         final Gradient[] adjoint = fieldState.getAdditionalState(adjointName);
         final int adjointDimension = adjoint.length;
         final Gradient[] integrationState = MathArrays.buildArray(fieldState.getMass().getField(), 7 + adjointDimension);
         final FieldPVCoordinates<Gradient> pvCoordinates = fieldState.getPVCoordinates();
         System.arraycopy(pvCoordinates.getPosition().toArray(), 0, integrationState, 0, 3);
         System.arraycopy(pvCoordinates.getVelocity().toArray(), 0, integrationState, 3, 3);
         integrationState[6] = fieldState.getMass();
         System.arraycopy(adjoint, 0, integrationState, 7, adjointDimension);
         return integrationState;
     }
 
     /**
      * Form Orekit object from array.
      * @param date date
      * @param integrationState propagation state as array
      * @return Orekit state
      */
     private FieldSpacecraftState<Gradient> formatFromArray(final AbsoluteDate date, final Gradient[] integrationState) {
         final Gradient[] terminalCartesian = Arrays.copyOfRange(integrationState, 0, 6);
         final Gradient[] terminalAdjoint = Arrays.copyOfRange(integrationState, 7, integrationState.length);
         final Field<Gradient> field = terminalCartesian[0].getField();
         return createFieldState(new FieldAbsoluteDate<>(field, date), terminalCartesian, integrationState[6],
                 terminalAdjoint);
     }
 
     /**
      * Iterate over field switch detectors and find the one that was triggered in the non-Field propagation.
      * Then, use it to update the gradient.
      * @param date date
      * @param integrationState integration variables
      * @param referenceDate date at start of propagation
      * @param switchDetector switch detector
      * @param dynamicsProvider adjoint dynamics provider
      * @return update integration state
      */
     private Gradient[] findSwitchEventAndUpdateState(final AbsoluteDate date, final Gradient[] integrationState,
                                                      final AbsoluteDate referenceDate,
                                                      final ControlSwitchDetector switchDetector,
                                                      final AdjointDynamicsProvider dynamicsProvider) {
         final FieldSpacecraftState<Gradient> fieldState = formatFromArray(date, integrationState);
         final double expectedG = switchDetector.g(fieldState.toSpacecraftState());
         final int shootingVariables = dynamicsProvider.getDimension();
         final int increasedVariables = shootingVariables + 1;
         final GradientField increasedVariablesField = GradientField.getField(increasedVariables);
         final FieldCartesianAdjointDerivativesProvider<Gradient> fieldDerivativesProvider =
                 (FieldCartesianAdjointDerivativesProvider<Gradient>) dynamicsProvider.buildFieldAdditionalDerivativesProvider(increasedVariablesField);
         final List<FieldEventDetector<Gradient>> fieldEventDetectors = fieldDerivativesProvider.getCost()
                 .getFieldEventDetectors(increasedVariablesField).collect(Collectors.toList());
         for (final FieldEventDetector<Gradient> fieldEventDetector : fieldEventDetectors) {
             if (fieldEventDetector instanceof FieldControlSwitchDetector) {
                 final double actualG = fieldEventDetector.g(fieldState).getReal();
                 if (FastMath.abs(actualG - expectedG) < 1e-10) {
                     return updateStateWithTaylorMapInversion(date, integrationState, referenceDate, fieldEventDetector);
                 }
             }
         }
         return integrationState;
     }
 
     /**
      * Update the differential information by taking into account that a state-dependent event was triggered.
      * @param date date
      * @param integrationState integration variables
      * @param initialDate date at start of propagation
      * @param fieldDetector switch detector
      * @return updated integration state
      */
     private Gradient[] updateStateWithTaylorMapInversion(final AbsoluteDate date, final Gradient[] integrationState,
                                                          final AbsoluteDate initialDate,
                                                          final FieldEventDetector<Gradient> fieldDetector) {
         // form array with increased gradient size
         final Gradient threshold = fieldDetector.getThreshold();
         final int increasedVariables = threshold.getFreeParameters();
         final GradientField increasedVariablesField = GradientField.getField(increasedVariables);
         final Gradient[] increasedVariablesArray = MathArrays.buildArray(increasedVariablesField,
                 integrationState.length);
         for (int i = 0; i < integrationState.length; i++) {
             increasedVariablesArray[i] = integrationState[i].stackVariable();
         }
         // compute rates at switch
         final GradientField field = increasedVariablesArray[0].getField();
         final FieldOrdinaryDifferentialEquation<Gradient> fieldODE = buildFieldODE(new FieldAbsoluteDate<>(field, initialDate));
         final double duration = date.durationFrom(initialDate);
         final Gradient fieldDuration = field.getZero().newInstance(duration);
         final Gradient[] derivatives = fieldODE.computeDerivatives(fieldDuration, increasedVariablesArray);
         // compute differences in rates
         final double[] deltaRates = computeDeltaRates(date, increasedVariablesArray,
                 fieldDetector.getThreshold().getValue(), initialDate, fieldODE, derivatives);
         // evaluate event function in Taylor algebra
         final double[] derivativesValues = new double[derivatives.length];
         for (int i = 0; i < derivativesValues.length; i++) {
             derivativesValues[i] = derivatives[i].getValue();
         }
         final Gradient g = evaluateG(date, increasedVariablesArray, initialDate, fieldDetector, derivativesValues);
         // perform last step involving variables swap
         return invertTaylorMap(increasedVariablesArray, deltaRates, g);
     }
 
     /**
      * Form differences in rates before and after switch.
      * @param date epoch
      * @param integrationState state variables
      * @param threshold event detection threshold
      * @param initialDate initial date
      * @param fieldODE ODE model
      * @param derivatives rates at switch
      * @return rates difference
      */
     private double[] computeDeltaRates(final AbsoluteDate date, final Gradient[] integrationState,
                                        final double threshold, final AbsoluteDate initialDate,
                                        final FieldOrdinaryDifferentialEquation<Gradient> fieldODE,
                                        final Gradient[] derivatives) {
         final double duration = date.durationFrom(initialDate);
         final Gradient fieldDuration = integrationState[0].getField().getZero().newInstance(duration);
         final double tinyStep = threshold * 2.;
         final boolean isForward = date.isAfter(initialDate);
         final double timeShift = isForward ? tinyStep : -tinyStep;
         final Gradient[] derivativesBefore = shiftAndComputeDerivatives(fieldDuration, integrationState, derivatives,
                 fieldODE, -timeShift);
         final Gradient[] derivativesAfter = shiftAndComputeDerivatives(fieldDuration, integrationState, derivatives,
                 fieldODE, timeShift);
         final double[] deltaRates = new double[integrationState.length];
         for (int i = 0; i < integrationState.length; i++) {
             deltaRates[i] = derivativesBefore[i].getValue() - derivativesAfter[i].getValue();
         }
         return deltaRates;
     }
 
     /**
      * Shift state variables in time and compute rates.
      * @param fieldDuration duration
      * @param integrationState state variables
      * @param derivatives rates before shift
      * @param fieldODE ODE model
      * @param timeShift shift
      * @return rates
      */
     private Gradient[] shiftAndComputeDerivatives(final Gradient fieldDuration, final Gradient[] integrationState,
                                                   final Gradient[] derivatives,
                                                   final FieldOrdinaryDifferentialEquation<Gradient> fieldODE,
                                                   final double timeShift) {
         final Gradient[] state = integrationState.clone();
         for (int i = 0; i < state.length; i++) {
             state[i] = state[i].add(derivatives[i].multiply(timeShift));
         }
         return fieldODE.computeDerivatives(fieldDuration, state);
     }
 
     /**
      * Evaluate event function in proper Taylor algebra.
      * @param date date
      * @param increasedVariablesArray integration variables with increased gradient size
      * @param initialDate date at start of propagation
      * @param fieldDetector switch detector
      * @param derivatives rates
      * @return g
      */
     private Gradient evaluateG(final AbsoluteDate date, final Gradient[] increasedVariablesArray,
                                final AbsoluteDate initialDate, final FieldEventDetector<Gradient> fieldDetector,
                                final double[] derivatives) {
         final Field<Gradient> field = increasedVariablesArray[0].getField();
         final int increasedVariables = field.getZero().getFreeParameters();
         final Gradient lastVariable = Gradient.variable(increasedVariables, increasedVariables - 1, 1);
         final boolean isForward = date.isAfterOrEqualTo(initialDate);
         final Gradient dt = isForward ? lastVariable : lastVariable.negate();
         final Gradient[] shiftedVariables = increasedVariablesArray.clone();
         for (int i = 0; i < shiftedVariables.length; i++) {
             shiftedVariables[i] = shiftedVariables[i].add(dt.multiply(derivatives[i]));
         }
         final FieldSpacecraftState<Gradient> fieldState = formatFromArray(date, shiftedVariables);
         return fieldDetector.g(fieldState);
     }
 
     /**
      * Invert so-called Taylor map to make the event function value an independent variable.
      * Then nullify its variation to keep only the derivatives of interest.
      * @param state integration variables with dt as last gradient variable
      * @param deltaRates differences in rates from dynamics switch
      * @param g event function evaluated with dt as last gradient variable
      * @return update integration variables
      */
     private static Gradient[] invertTaylorMap(final Gradient[] state, final double[] deltaRates, final Gradient g) {
         // swap dt and g as variables of algebra
         final int increasedGradientDimension = g.getFreeParameters();
         final RealMatrix rightMatrix = MatrixUtils.createRealIdentityMatrix(increasedGradientDimension);
         rightMatrix.setRow(rightMatrix.getRowDimension() - 1, g.getGradient());
         final LUDecomposition luDecomposition = new LUDecomposition(rightMatrix, 0.);
         final RealMatrix inverted = luDecomposition.getSolver().getInverse();
         final double[][] leftMatrixCoefficients = new double[state.length][];
         for (int i = 0; i < state.length; i++) {
             leftMatrixCoefficients[i] = state[i].getGradient();
             leftMatrixCoefficients[i][leftMatrixCoefficients[i].length - 1] = deltaRates[i];
         }
         final RealMatrix product = MatrixUtils.createRealMatrix(leftMatrixCoefficients).multiply(inverted);
         // pack into array with original gradient dimension
         final int gradientDimension = increasedGradientDimension - 1;
         final GradientField field = GradientField.getField(gradientDimension);
         final Gradient[] outputState = MathArrays.buildArray(field, state.length);
         for (int i = 0; i < outputState.length; i++) {
             final double[] gradient = new double[gradientDimension];
             System.arraycopy(product.getRow(i), 0, gradient, 0, gradientDimension);
             outputState[i] = new Gradient(state[i].getValue(), gradient);
         }
         return outputState;
     }
 
 }