/*
    * Licensed to the Apache Software Foundation (ASF) under one or more
    * contributor license agreements.  See the NOTICE file distributed with
    * this work for additional information regarding copyright ownership.
    * The ASF 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
    *
    *      https://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.
   */
  
  /*
   * This is not the original file distributed by the Apache Software Foundation
   * It has been modified by the Hipparchus project
   */
  
  package org.hipparchus.ode;
  
  import java.util.ArrayList;
  import java.util.Collections;
  import java.util.Comparator;
  import java.util.List;
  import java.util.PriorityQueue;
  import java.util.Queue;
  import java.util.stream.Collectors;
  import java.util.stream.Stream;
  
  import org.hipparchus.CalculusFieldElement;
  import org.hipparchus.Field;
  import org.hipparchus.exception.MathIllegalArgumentException;
  import org.hipparchus.exception.MathIllegalStateException;
  import org.hipparchus.ode.events.Action;
  import org.hipparchus.ode.events.FieldDetectorBasedEventState;
  import org.hipparchus.ode.events.FieldEventOccurrence;
  import org.hipparchus.ode.events.FieldEventState;
  import org.hipparchus.ode.events.FieldODEEventDetector;
  import org.hipparchus.ode.events.FieldODEStepEndHandler;
  import org.hipparchus.ode.events.FieldStepEndEventState;
  import org.hipparchus.ode.sampling.AbstractFieldODEStateInterpolator;
  import org.hipparchus.ode.sampling.FieldODEStateInterpolator;
  import org.hipparchus.ode.sampling.FieldODEStepHandler;
  import org.hipparchus.util.FastMath;
  import org.hipparchus.util.Incrementor;
  
  /**
   * Base class managing common boilerplate for all integrators.
   * @param <T> the type of the field elements
   */
  public abstract class AbstractFieldIntegrator<T extends CalculusFieldElement<T>> implements FieldODEIntegrator<T> {
  
      /** Step handler. */
      private final List<FieldODEStepHandler<T>> stepHandlers;
  
      /** Current step start. */
      private FieldODEStateAndDerivative<T> stepStart;
  
      /** Current stepsize. */
      private T stepSize;
  
      /** Indicator for last step. */
      private boolean isLastStep;
  
      /** Indicator that a state or derivative reset was triggered by some event. */
      private boolean resetOccurred;
  
      /** Field to which the time and state vector elements belong. */
      private final Field<T> field;
  
      /** Events states. */
      private final List<FieldDetectorBasedEventState<T>> detectorBasedEventsStates;
  
      /** Events states related to step end. */
      private final List<FieldStepEndEventState<T>> stepEndEventsStates;
  
      /** Initialization indicator of events states. */
      private boolean statesInitialized;
  
      /** Name of the method. */
      private final String name;
  
      /** Counter for number of evaluations. */
      private Incrementor evaluations;
  
      /** Differential equations to integrate. */
      private transient FieldExpandableODE<T> equations;
  
      /** Build an instance.
       * @param field field to which the time and state vector elements belong
       * @param name name of the method
       */
      protected AbstractFieldIntegrator(final Field<T> field, final String name) {
          this.field                = field;
          this.name                 = name;
         stepHandlers              = new ArrayList<>();
         stepStart                 = null;
         stepSize                  = null;
         detectorBasedEventsStates = new ArrayList<>();
         stepEndEventsStates       = new ArrayList<>();
         statesInitialized         = false;
         evaluations               = new Incrementor();
     }
 
     /** Get the field to which state vector elements belong.
      * @return field to which state vector elements belong
      */
     public Field<T> getField() {
         return field;
     }
 
     /** {@inheritDoc} */
     @Override
     public String getName() {
         return name;
     }
 
     /** {@inheritDoc} */
     @Override
     public void addStepHandler(final FieldODEStepHandler<T> handler) {
         stepHandlers.add(handler);
     }
 
     /** {@inheritDoc} */
     @Override
     public List<FieldODEStepHandler<T>> getStepHandlers() {
         return Collections.unmodifiableList(stepHandlers);
     }
 
     /** {@inheritDoc} */
     @Override
     public void clearStepHandlers() {
         stepHandlers.clear();
     }
 
     /** {@inheritDoc} */
     @Override
     public void addEventDetector(final FieldODEEventDetector<T> detector) {
         detectorBasedEventsStates.add(new FieldDetectorBasedEventState<>(detector));
     }
 
     /** {@inheritDoc} */
     @Override
     public List<FieldODEEventDetector<T>> getEventDetectors() {
         return detectorBasedEventsStates.stream().map(FieldDetectorBasedEventState::getEventDetector).collect(Collectors.toList());
     }
 
     /** {@inheritDoc} */
     @Override
     public void clearEventDetectors() {
         detectorBasedEventsStates.clear();
     }
 
     /** {@inheritDoc} */
     @Override
     public void addStepEndHandler(FieldODEStepEndHandler<T> handler) {
         stepEndEventsStates.add(new FieldStepEndEventState<>(handler));
     }
 
     /** {@inheritDoc} */
     @Override
     public List<FieldODEStepEndHandler<T>> getStepEndHandlers() {
         return stepEndEventsStates.stream().map(FieldStepEndEventState::getHandler).collect(Collectors.toList());
     }
 
     /** {@inheritDoc} */
     @Override
     public void clearStepEndHandlers() {
         stepEndEventsStates.clear();
     }
 
     /** {@inheritDoc} */
     @Override
     public T getCurrentSignedStepsize() {
         return stepSize;
     }
 
     /** {@inheritDoc} */
     @Override
     public void setMaxEvaluations(int maxEvaluations) {
         evaluations = evaluations.withMaximalCount((maxEvaluations < 0) ? Integer.MAX_VALUE : maxEvaluations);
     }
 
     /** {@inheritDoc} */
     @Override
     public int getMaxEvaluations() {
         return evaluations.getMaximalCount();
     }
 
     /** {@inheritDoc} */
     @Override
     public int getEvaluations() {
         return evaluations.getCount();
     }
 
     /** Prepare the start of an integration.
      * @param eqn equations to integrate
      * @param s0 initial state vector
      * @param t target time for the integration
      * @return initial state with derivatives added
      */
     protected FieldODEStateAndDerivative<T> initIntegration(final FieldExpandableODE<T> eqn,
                                                             final FieldODEState<T> s0, final T t) {
 
         this.equations = eqn;
         evaluations    = evaluations.withCount(0);
 
         // initialize ODE
         eqn.init(s0, t);
 
         // set up derivatives of initial state (including primary and secondary components)
         final T   t0    = s0.getTime();
         final T[] y0    = s0.getCompleteState();
         final T[] y0Dot = computeDerivatives(t0, y0);
 
         // built the state
         final FieldODEStateAndDerivative<T> s0WithDerivatives =
                         eqn.getMapper().mapStateAndDerivative(t0, y0, y0Dot);
 
         // initialize detector based event states (both  and step end based)
         detectorBasedEventsStates.forEach(s -> {
             s.init(s0WithDerivatives, t);
             s.getEventDetector().getHandler().init(s0WithDerivatives, t, s.getEventDetector());
         });
 
         // initialize step end based event states
         stepEndEventsStates.forEach(s -> {
             s.init(s0WithDerivatives, t);
             s.getHandler().init(s0WithDerivatives, t);
         });
 
         // initialize step handlers
         for (FieldODEStepHandler<T> handler : stepHandlers) {
             handler.init(s0WithDerivatives, t);
         }
 
         setStateInitialized(false);
 
         return s0WithDerivatives;
 
     }
 
     /** Get the differential equations to integrate.
      * @return differential equations to integrate
      */
     protected FieldExpandableODE<T> getEquations() {
         return equations;
     }
 
     /** Get the evaluations counter.
      * @return evaluations counter
      */
     protected Incrementor getEvaluationsCounter() {
         return evaluations;
     }
 
     /** Compute the derivatives and check the number of evaluations.
      * @param t current value of the independent <I>time</I> variable
      * @param y array containing the current value of the state vector
      * @return state completed with derivatives
      * @exception MathIllegalArgumentException if arrays dimensions do not match equations settings
      * @exception MathIllegalStateException if the number of functions evaluations is exceeded
      * @exception NullPointerException if the ODE equations have not been set (i.e. if this method
      * is called outside of a call to {@link #integrate(FieldExpandableODE, FieldODEState,
      * CalculusFieldElement) integrate}
      */
     public T[] computeDerivatives(final T t, final T[] y)
         throws MathIllegalArgumentException, MathIllegalStateException, NullPointerException {
         evaluations.increment();
         return equations.computeDerivatives(t, y);
     }
 
     /** Increment evaluations of derivatives.
      *
      * @param nTimes number of evaluations to increment
      */
     protected void incrementEvaluations(final int nTimes) {
         evaluations.increment(nTimes);
     }
 
     /** Set the stateInitialized flag.
      * <p>This method must be called by integrators with the value
      * {@code false} before they start integration, so a proper lazy
      * initialization is done automatically on the first step.</p>
      * @param stateInitialized new value for the flag
      */
     protected void setStateInitialized(final boolean stateInitialized) {
         this.statesInitialized = stateInitialized;
     }
 
     /**
      * Accept a step, triggering events and step handlers.
      *
      * @param interpolator step interpolator
      * @param tEnd         final integration time
      * @return state at end of step
      * @throws MathIllegalStateException    if the interpolator throws one because the
      *                                      number of functions evaluations is exceeded
      * @throws MathIllegalArgumentException if the location of an event cannot be
      *                                      bracketed
      * @throws MathIllegalArgumentException if arrays dimensions do not match equations
      *                                      settings
      */
     protected FieldODEStateAndDerivative<T> acceptStep(final AbstractFieldODEStateInterpolator<T> interpolator,
                                                        final T tEnd)
             throws MathIllegalArgumentException, MathIllegalStateException {
 
         FieldODEStateAndDerivative<T> previousState = interpolator.getGlobalPreviousState();
         final FieldODEStateAndDerivative<T> currentState = interpolator.getGlobalCurrentState();
         FieldODEStateInterpolator<T> restricted = interpolator;
 
         // initialize the events states if needed
         if (!statesInitialized) {
             // initialize event states
             detectorBasedEventsStates.forEach(s -> s.reinitializeBegin(interpolator));
             statesInitialized = true;
         }
 
         // set end of step
         stepEndEventsStates.forEach(s -> s.setStepEnd(currentState.getTime()));
 
         // 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(FieldEventState<T> es0, FieldEventState<T> es1) {
                 return orderingSign * Double.compare(es0.getEventTime().getReal(), es1.getEventTime().getReal());
             }
         });
 
         resetOccurred = false;
         boolean doneWithStep = false;
         resetEvents:
         do {
 
             // Evaluate all event detectors and end steps for events
             occurringEvents.clear();
             final FieldODEStateInterpolator<T> finalRestricted = restricted;
             Stream.concat(detectorBasedEventsStates.stream(), stepEndEventsStates.stream()).
             forEach(s -> { if (s.evaluateStep(finalRestricted)) {
                     // the event occurs during the current step
                     occurringEvents.add(s);
                 }
             });
 
 
             do {
 
                 eventLoop:
                 while (!occurringEvents.isEmpty()) {
 
                     // handle the chronologically first event
                     final FieldEventState<T> currentEvent = occurringEvents.poll();
 
                     // get state at event time
                     FieldODEStateAndDerivative<T> eventState =
                             restricted.getInterpolatedState(currentEvent.getEventTime());
 
                     // restrict the interpolator to the first part of the step, up to the event
                     restricted = restricted.restrictStep(previousState, eventState);
 
                     // try to advance all event states related to detectors to current time
                     for (final FieldDetectorBasedEventState<T> state : detectorBasedEventsStates) {
                         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
                     for (final FieldODEStepHandler<T> handler : stepHandlers) {
                         handler.handleStep(restricted);
                     }
 
                     // acknowledge event occurrence
                     final FieldEventOccurrence<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 FieldODEStateAndDerivative<T> savedState = eventState;
                         eventState = interpolator.getInterpolatedState(occurrence.getStopTime());
                         restricted = interpolator.restrictStep(savedState, eventState);
 
                         // handle the almost zero size last part of the final step, at event time
                         for (final FieldODEStepHandler<T> handler : stepHandlers) {
                             handler.handleStep(restricted);
                             handler.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 FieldODEState<T> newState = occurrence.getNewState();
                         final T[] y = newState.getCompleteState();
                         final T[] yDot = computeDerivatives(newState.getTime(), y);
                         resetOccurred = true;
                         final FieldODEStateAndDerivative<T> newStateAndDerivative = equations.getMapper().mapStateAndDerivative(newState.getTime(),
                                 y, yDot);
                         detectorBasedEventsStates.forEach(e -> e.getEventDetector().reset(newStateAndDerivative, tEnd));
                         return newStateAndDerivative;
                     }
                     // at this point action == Action.CONTINUE or Action.RESET_EVENTS
 
                     // prepare handling of the remaining part of the step
                     previousState = eventState;
                     restricted = restricted.restrictStep(eventState, currentState);
 
                     if (action == Action.RESET_EVENTS) {
                         continue resetEvents;
                     }
 
                     // at this point 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 event
                 // detectors to the end of the step. Should never find new events unless
                 // a previous event modified the g function of another event detector when
                 // it returned Action.CONTINUE. Detecting such events here is unreliable
                 // and RESET_EVENTS should be used instead. Other option is to replace
                 // tryAdvance(...) with a doAdvance(...) that throws an exception when
                 // the g function sign is not as expected.
                 for (final FieldDetectorBasedEventState<T> state : detectorBasedEventsStates) {
                     if (state.tryAdvance(currentState, interpolator)) {
                         occurringEvents.add(state);
                     }
                 }
 
             } while (!occurringEvents.isEmpty());
 
             doneWithStep = true;
         } while (!doneWithStep);
 
 
         isLastStep = isLastStep || currentState.getTime().subtract(tEnd).norm() < FastMath.ulp(tEnd.getReal());
 
         // handle the remaining part of the step, after all events if any
         for (FieldODEStepHandler<T> handler : stepHandlers) {
             handler.handleStep(restricted);
             if (isLastStep) {
                 handler.finish(restricted.getCurrentState());
             }
         }
 
         return currentState;
 
     }
 
     /** Check the integration span.
      * @param initialState initial state
      * @param t target time for the integration
      * @exception MathIllegalArgumentException if integration span is too small
      * @exception MathIllegalArgumentException if adaptive step size integrators
      * tolerance arrays dimensions are not compatible with equations settings
      */
     protected void sanityChecks(final FieldODEState<T> initialState, final T t)
         throws MathIllegalArgumentException {
 
         final double threshold = 1000 * FastMath.ulp(FastMath.max(FastMath.abs(initialState.getTime().getReal()),
                                                                   FastMath.abs(t.getReal())));
         final double dt = initialState.getTime().subtract(t).norm();
         if (dt < threshold) {
             throw new MathIllegalArgumentException(LocalizedODEFormats.TOO_SMALL_INTEGRATION_INTERVAL,
                                                    dt, threshold, false);
         }
 
     }
 
     /** Check if a reset occurred while last step was accepted.
      * @return true if a reset occurred while last step was accepted
      */
     protected boolean resetOccurred() {
         return resetOccurred;
     }
 
     /** Set the current step size.
      * @param stepSize step size to set
      */
     protected void setStepSize(final T stepSize) {
         this.stepSize = stepSize;
     }
 
     /** Get the current step size.
      * @return current step size
      */
     protected T getStepSize() {
         return stepSize;
     }
 
     /** Set current step start.
      * @param stepStart step start
      */
     protected void setStepStart(final FieldODEStateAndDerivative<T> stepStart) {
         this.stepStart = stepStart;
     }
 
     /**  {@inheritDoc} */
     @Override
     public FieldODEStateAndDerivative<T> getStepStart() {
         return stepStart;
     }
 
     /** Set the last state flag.
      * @param isLastStep if true, this step is the last one
      */
     protected void setIsLastStep(final boolean isLastStep) {
         this.isLastStep = isLastStep;
     }
 
     /** Check if this step is the last one.
      * @return true if this step is the last one
      */
     protected boolean isLastStep() {
         return isLastStep;
     }
 
 }