/*
 * 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
 *
 *      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.apache.commons.math.ode.nonstiff;

import org.apache.commons.math.ode.DerivativeException;
import org.apache.commons.math.ode.FirstOrderDifferentialEquations;
import org.apache.commons.math.ode.IntegratorException;
import org.apache.commons.math.ode.events.EventHandler;
import org.apache.commons.math.ode.sampling.AbstractStepInterpolator;
import org.apache.commons.math.ode.sampling.DummyStepInterpolator;
import org.apache.commons.math.ode.sampling.StepHandler;
import org.apache.commons.math.util.FastMath;

/**
 * This class implements a Gragg-Bulirsch-Stoer integrator for
 * Ordinary Differential Equations.
 *
 * <p>The Gragg-Bulirsch-Stoer algorithm is one of the most efficient
 * ones currently available for smooth problems. It uses Richardson
 * extrapolation to estimate what would be the solution if the step
 * size could be decreased down to zero.</p>
 *
 * <p>
 * This method changes both the step size and the order during
 * integration, in order to minimize computation cost. It is
 * particularly well suited when a very high precision is needed. The
 * limit where this method becomes more efficient than high-order
 * embedded Runge-Kutta methods like {@link DormandPrince853Integrator
 * Dormand-Prince 8(5,3)} depends on the problem. Results given in the
 * Hairer, Norsett and Wanner book show for example that this limit
 * occurs for accuracy around 1e-6 when integrating Saltzam-Lorenz
 * equations (the authors note this problem is <i>extremely sensitive
 * to the errors in the first integration steps</i>), and around 1e-11
 * for a two dimensional celestial mechanics problems with seven
 * bodies (pleiades problem, involving quasi-collisions for which
 * <i>automatic step size control is essential</i>).
 * </p>
 *
 * <p>
 * This implementation is basically a reimplementation in Java of the
 * <a
 * href="http://www.unige.ch/math/folks/hairer/prog/nonstiff/odex.f">odex</a>
 * fortran code by E. Hairer and G. Wanner. The redistribution policy
 * for this code is available <a
 * href="http://www.unige.ch/~hairer/prog/licence.txt">here</a>, for
 * convenience, it is reproduced below.</p>
 * </p>
 *
 * <table border="0" width="80%" cellpadding="10" align="center" bgcolor="#E0E0E0">
 * <tr><td>Copyright (c) 2004, Ernst Hairer</td></tr>
 *
 * <tr><td>Redistribution and use in source and binary forms, with or
 * without modification, are permitted provided that the following
 * conditions are met:
 * <ul>
 *  <li>Redistributions of source code must retain the above copyright
 *      notice, this list of conditions and the following disclaimer.</li>
 *  <li>Redistributions in binary form must reproduce the above copyright
 *      notice, this list of conditions and the following disclaimer in the
 *      documentation and/or other materials provided with the distribution.</li>
 * </ul></td></tr>
 *
 * <tr><td><strong>THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
 * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A  PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.</strong></td></tr>
 * </table>
 *
 * @version $Revision: 1073158 $ $Date: 2011-02-21 22:46:52 +0100 (lun. 21 févr. 2011) $
 * @since 1.2
 */

public class GraggBulirschStoerIntegrator extends AdaptiveStepsizeIntegrator {

    /** Integrator method name. */
    private static final String METHOD_NAME = "Gragg-Bulirsch-Stoer";

    /** maximal order. */
    private int maxOrder;

    /** step size sequence. */
    private int[] sequence;

    /** overall cost of applying step reduction up to iteration k+1, in number of calls. */
    private int[] costPerStep;

    /** cost per unit step. */
    private double[] costPerTimeUnit;

    /** optimal steps for each order. */
    private double[] optimalStep;

    /** extrapolation coefficients. */
    private double[][] coeff;

    /** stability check enabling parameter. */
    private boolean performTest;

    /** maximal number of checks for each iteration. */
    private int maxChecks;

    /** maximal number of iterations for which checks are performed. */
    private int maxIter;

    /** stepsize reduction factor in case of stability check failure. */
    private double stabilityReduction;

    /** first stepsize control factor. */
    private double stepControl1;

    /** second stepsize control factor. */
    private double stepControl2;

    /** third stepsize control factor. */
    private double stepControl3;

    /** fourth stepsize control factor. */
    private double stepControl4;

    /** first order control factor. */
    private double orderControl1;

    /** second order control factor. */
    private double orderControl2;

    /** use interpolation error in stepsize control. */
    private boolean useInterpolationError;

    /** interpolation order control parameter. */
    private int mudif;

  /** Simple constructor.
   * Build a Gragg-Bulirsch-Stoer integrator with the given step
   * bounds. All tuning parameters are set to their default
   * values. The default step handler does nothing.
   * @param minStep minimal step (must be positive even for backward
   * integration), the last step can be smaller than this
   * @param maxStep maximal step (must be positive even for backward
   * integration)
   * @param scalAbsoluteTolerance allowed absolute error
   * @param scalRelativeTolerance allowed relative error
   */
  public GraggBulirschStoerIntegrator(final double minStep, final double maxStep,
                                      final double scalAbsoluteTolerance,
                                      final double scalRelativeTolerance) {
    super(METHOD_NAME, minStep, maxStep,
          scalAbsoluteTolerance, scalRelativeTolerance);
    setStabilityCheck(true, -1, -1, -1);
    setStepsizeControl(-1, -1, -1, -1);
    setOrderControl(-1, -1, -1);
    setInterpolationControl(true, -1);
  }

  /** Simple constructor.
   * Build a Gragg-Bulirsch-Stoer integrator with the given step
   * bounds. All tuning parameters are set to their default
   * values. The default step handler does nothing.
   * @param minStep minimal step (must be positive even for backward
   * integration), the last step can be smaller than this
   * @param maxStep maximal step (must be positive even for backward
   * integration)
   * @param vecAbsoluteTolerance allowed absolute error
   * @param vecRelativeTolerance allowed relative error
   */
  public GraggBulirschStoerIntegrator(final double minStep, final double maxStep,
                                      final double[] vecAbsoluteTolerance,
                                      final double[] vecRelativeTolerance) {
    super(METHOD_NAME, minStep, maxStep,
          vecAbsoluteTolerance, vecRelativeTolerance);
    setStabilityCheck(true, -1, -1, -1);
    setStepsizeControl(-1, -1, -1, -1);
    setOrderControl(-1, -1, -1);
    setInterpolationControl(true, -1);
  }

  /** Set the stability check controls.
   * <p>The stability check is performed on the first few iterations of
   * the extrapolation scheme. If this test fails, the step is rejected
   * and the stepsize is reduced.</p>
   * <p>By default, the test is performed, at most during two
   * iterations at each step, and at most once for each of these
   * iterations. The default stepsize reduction factor is 0.5.</p>
   * @param performStabilityCheck if true, stability check will be performed,
     if false, the check will be skipped
   * @param maxNumIter maximal number of iterations for which checks are
   * performed (the number of iterations is reset to default if negative
   * or null)
   * @param maxNumChecks maximal number of checks for each iteration
   * (the number of checks is reset to default if negative or null)
   * @param stepsizeReductionFactor stepsize reduction factor in case of
   * failure (the factor is reset to default if lower than 0.0001 or
   * greater than 0.9999)
   */
  public void setStabilityCheck(final boolean performStabilityCheck,
                                final int maxNumIter, final int maxNumChecks,
                                final double stepsizeReductionFactor) {

    this.performTest = performStabilityCheck;
    this.maxIter     = (maxNumIter   <= 0) ? 2 : maxNumIter;
    this.maxChecks   = (maxNumChecks <= 0) ? 1 : maxNumChecks;

    if ((stepsizeReductionFactor < 0.0001) || (stepsizeReductionFactor > 0.9999)) {
      this.stabilityReduction = 0.5;
    } else {
      this.stabilityReduction = stepsizeReductionFactor;
    }

  }

  /** Set the step size control factors.

   * <p>The new step size hNew is computed from the old one h by:
   * <pre>
   * hNew = h * stepControl2 / (err/stepControl1)^(1/(2k+1))
   * </pre>
   * where err is the scaled error and k the iteration number of the
   * extrapolation scheme (counting from 0). The default values are
   * 0.65 for stepControl1 and 0.94 for stepControl2.</p>
   * <p>The step size is subject to the restriction:
   * <pre>
   * stepControl3^(1/(2k+1))/stepControl4 <= hNew/h <= 1/stepControl3^(1/(2k+1))
   * </pre>
   * The default values are 0.02 for stepControl3 and 4.0 for
   * stepControl4.</p>
   * @param control1 first stepsize control factor (the factor is
   * reset to default if lower than 0.0001 or greater than 0.9999)
   * @param control2 second stepsize control factor (the factor
   * is reset to default if lower than 0.0001 or greater than 0.9999)
   * @param control3 third stepsize control factor (the factor is
   * reset to default if lower than 0.0001 or greater than 0.9999)
   * @param control4 fourth stepsize control factor (the factor
   * is reset to default if lower than 1.0001 or greater than 999.9)
   */
  public void setStepsizeControl(final double control1, final double control2,
                                 final double control3, final double control4) {

    if ((control1 < 0.0001) || (control1 > 0.9999)) {
      this.stepControl1 = 0.65;
    } else {
      this.stepControl1 = control1;
    }

    if ((control2 < 0.0001) || (control2 > 0.9999)) {
      this.stepControl2 = 0.94;
    } else {
      this.stepControl2 = control2;
    }

    if ((control3 < 0.0001) || (control3 > 0.9999)) {
      this.stepControl3 = 0.02;
    } else {
      this.stepControl3 = control3;
    }

    if ((control4 < 1.0001) || (control4 > 999.9)) {
      this.stepControl4 = 4.0;
    } else {
      this.stepControl4 = control4;
    }

  }

  /** Set the order control parameters.
   * <p>The Gragg-Bulirsch-Stoer method changes both the step size and
   * the order during integration, in order to minimize computation
   * cost. Each extrapolation step increases the order by 2, so the
   * maximal order that will be used is always even, it is twice the
   * maximal number of columns in the extrapolation table.</p>
   * <pre>
   * order is decreased if w(k-1) <= w(k)   * orderControl1
   * order is increased if w(k)   <= w(k-1) * orderControl2
   * </pre>
   * <p>where w is the table of work per unit step for each order
   * (number of function calls divided by the step length), and k is
   * the current order.</p>
   * <p>The default maximal order after construction is 18 (i.e. the
   * maximal number of columns is 9). The default values are 0.8 for
   * orderControl1 and 0.9 for orderControl2.</p>
   * @param maximalOrder maximal order in the extrapolation table (the
   * maximal order is reset to default if order <= 6 or odd)
   * @param control1 first order control factor (the factor is
   * reset to default if lower than 0.0001 or greater than 0.9999)
   * @param control2 second order control factor (the factor
   * is reset to default if lower than 0.0001 or greater than 0.9999)
   */
  public void setOrderControl(final int maximalOrder,
                              final double control1, final double control2) {

    if ((maximalOrder <= 6) || (maximalOrder % 2 != 0)) {
      this.maxOrder = 18;
    }

    if ((control1 < 0.0001) || (control1 > 0.9999)) {
      this.orderControl1 = 0.8;
    } else {
      this.orderControl1 = control1;
    }

    if ((control2 < 0.0001) || (control2 > 0.9999)) {
      this.orderControl2 = 0.9;
    } else {
      this.orderControl2 = control2;
    }

    // reinitialize the arrays
    initializeArrays();

  }

  /** {@inheritDoc} */
  @Override
  public void addStepHandler (final StepHandler handler) {

    super.addStepHandler(handler);

    // reinitialize the arrays
    initializeArrays();

  }

  /** {@inheritDoc} */
  @Override
  public void addEventHandler(final EventHandler function,
                              final double maxCheckInterval,
                              final double convergence,
                              final int maxIterationCount) {
    super.addEventHandler(function, maxCheckInterval, convergence, maxIterationCount);

    // reinitialize the arrays
    initializeArrays();

  }

  /** Initialize the integrator internal arrays. */
  private void initializeArrays() {

    final int size = maxOrder / 2;

    if ((sequence == null) || (sequence.length != size)) {
      // all arrays should be reallocated with the right size
      sequence        = new int[size];
      costPerStep     = new int[size];
      coeff           = new double[size][];
      costPerTimeUnit = new double[size];
      optimalStep     = new double[size];
    }

    if (requiresDenseOutput()) {
      // step size sequence: 2, 6, 10, 14, ...
      for (int k = 0; k < size; ++k) {
        sequence[k] = 4 * k + 2;
      }
    } else {
      // step size sequence: 2, 4, 6, 8, ...
      for (int k = 0; k < size; ++k) {
        sequence[k] = 2 * (k + 1);
      }
    }

    // initialize the order selection cost array
    // (number of function calls for each column of the extrapolation table)
    costPerStep[0] = sequence[0] + 1;
    for (int k = 1; k < size; ++k) {
      costPerStep[k] = costPerStep[k-1] + sequence[k];
    }

    // initialize the extrapolation tables
    for (int k = 0; k < size; ++k) {
      coeff[k] = (k > 0) ? new double[k] : null;
      for (int l = 0; l < k; ++l) {
        final double ratio = ((double) sequence[k]) / sequence[k-l-1];
        coeff[k][l] = 1.0 / (ratio * ratio - 1.0);
      }
    }

  }

  /** Set the interpolation order control parameter.
   * The interpolation order for dense output is 2k - mudif + 1. The
   * default value for mudif is 4 and the interpolation error is used
   * in stepsize control by default.
   *
   * @param useInterpolationErrorForControl if true, interpolation error is used
   * for stepsize control
   * @param mudifControlParameter interpolation order control parameter (the parameter
   * is reset to default if <= 0 or >= 7)
   */
  public void setInterpolationControl(final boolean useInterpolationErrorForControl,
                                      final int mudifControlParameter) {

    this.useInterpolationError = useInterpolationErrorForControl;

    if ((mudifControlParameter <= 0) || (mudifControlParameter >= 7)) {
      this.mudif = 4;
    } else {
      this.mudif = mudifControlParameter;
    }

  }

  /** Update scaling array.
   * @param y1 first state vector to use for scaling
   * @param y2 second state vector to use for scaling
   * @param scale scaling array to update (can be shorter than state)
   */
  private void rescale(final double[] y1, final double[] y2, final double[] scale) {
    if (vecAbsoluteTolerance == null) {
      for (int i = 0; i < scale.length; ++i) {
        final double yi = FastMath.max(FastMath.abs(y1[i]), FastMath.abs(y2[i]));
        scale[i] = scalAbsoluteTolerance + scalRelativeTolerance * yi;
      }
    } else {
      for (int i = 0; i < scale.length; ++i) {
        final double yi = FastMath.max(FastMath.abs(y1[i]), FastMath.abs(y2[i]));
        scale[i] = vecAbsoluteTolerance[i] + vecRelativeTolerance[i] * yi;
      }
    }
  }

  /** Perform integration over one step using substeps of a modified
   * midpoint method.
   * @param t0 initial time
   * @param y0 initial value of the state vector at t0
   * @param step global step
   * @param k iteration number (from 0 to sequence.length - 1)
   * @param scale scaling array (can be shorter than state)
   * @param f placeholder where to put the state vector derivatives at each substep
   *          (element 0 already contains initial derivative)
   * @param yMiddle placeholder where to put the state vector at the middle of the step
   * @param yEnd placeholder where to put the state vector at the end
   * @param yTmp placeholder for one state vector
   * @return true if computation was done properly,
   *         false if stability check failed before end of computation
   * @throws DerivativeException this exception is propagated to the caller if the
   * underlying user function triggers one
   */
  private boolean tryStep(final double t0, final double[] y0, final double step, final int k,
                          final double[] scale, final double[][] f,
                          final double[] yMiddle, final double[] yEnd,
                          final double[] yTmp)
    throws DerivativeException {

    final int    n        = sequence[k];
    final double subStep  = step / n;
    final double subStep2 = 2 * subStep;

    // first substep
    double t = t0 + subStep;
    for (int i = 0; i < y0.length; ++i) {
      yTmp[i] = y0[i];
      yEnd[i] = y0[i] + subStep * f[0][i];
    }
    computeDerivatives(t, yEnd, f[1]);

    // other substeps
    for (int j = 1; j < n; ++j) {

      if (2 * j == n) {
        // save the point at the middle of the step
        System.arraycopy(yEnd, 0, yMiddle, 0, y0.length);
      }

      t += subStep;
      for (int i = 0; i < y0.length; ++i) {
        final double middle = yEnd[i];
        yEnd[i]       = yTmp[i] + subStep2 * f[j][i];
        yTmp[i]       = middle;
      }

      computeDerivatives(t, yEnd, f[j+1]);

      // stability check
      if (performTest && (j <= maxChecks) && (k < maxIter)) {
        double initialNorm = 0.0;
        for (int l = 0; l < scale.length; ++l) {
          final double ratio = f[0][l] / scale[l];
          initialNorm += ratio * ratio;
        }
        double deltaNorm = 0.0;
        for (int l = 0; l < scale.length; ++l) {
          final double ratio = (f[j+1][l] - f[0][l]) / scale[l];
          deltaNorm += ratio * ratio;
        }
        if (deltaNorm > 4 * FastMath.max(1.0e-15, initialNorm)) {
          return false;
        }
      }

    }

    // correction of the last substep (at t0 + step)
    for (int i = 0; i < y0.length; ++i) {
      yEnd[i] = 0.5 * (yTmp[i] + yEnd[i] + subStep * f[n][i]);
    }

    return true;

  }

  /** Extrapolate a vector.
   * @param offset offset to use in the coefficients table
   * @param k index of the last updated point
   * @param diag working diagonal of the Aitken-Neville's
   * triangle, without the last element
   * @param last last element
   */
  private void extrapolate(final int offset, final int k,
                           final double[][] diag, final double[] last) {

    // update the diagonal
    for (int j = 1; j < k; ++j) {
      for (int i = 0; i < last.length; ++i) {
        // Aitken-Neville's recursive formula
        diag[k-j-1][i] = diag[k-j][i] +
                         coeff[k+offset][j-1] * (diag[k-j][i] - diag[k-j-1][i]);
      }
    }

    // update the last element
    for (int i = 0; i < last.length; ++i) {
      // Aitken-Neville's recursive formula
      last[i] = diag[0][i] + coeff[k+offset][k-1] * (diag[0][i] - last[i]);
    }
  }

  /** {@inheritDoc} */
  @Override
  public double integrate(final FirstOrderDifferentialEquations equations,
                          final double t0, final double[] y0, final double t, final double[] y)
      throws DerivativeException, IntegratorException {

    sanityChecks(equations, t0, y0, t, y);
    setEquations(equations);
    resetEvaluations();
    final boolean forward = t > t0;

    // create some internal working arrays
    final double[] yDot0   = new double[y0.length];
    final double[] y1      = new double[y0.length];
    final double[] yTmp    = new double[y0.length];
    final double[] yTmpDot = new double[y0.length];

    final double[][] diagonal = new double[sequence.length-1][];
    final double[][] y1Diag = new double[sequence.length-1][];
    for (int k = 0; k < sequence.length-1; ++k) {
      diagonal[k] = new double[y0.length];
      y1Diag[k] = new double[y0.length];
    }

    final double[][][] fk  = new double[sequence.length][][];
    for (int k = 0; k < sequence.length; ++k) {

      fk[k]    = new double[sequence[k] + 1][];

      // all substeps start at the same point, so share the first array
      fk[k][0] = yDot0;

      for (int l = 0; l < sequence[k]; ++l) {
        fk[k][l+1] = new double[y0.length];
      }

    }

    if (y != y0) {
      System.arraycopy(y0, 0, y, 0, y0.length);
    }

    double[] yDot1      = new double[y0.length];
    double[][] yMidDots = null;
    final boolean denseOutput = requiresDenseOutput();
    if (denseOutput) {
      yMidDots = new double[1 + 2 * sequence.length][];
      for (int j = 0; j < yMidDots.length; ++j) {
        yMidDots[j] = new double[y0.length];
      }
    } else {
      yMidDots    = new double[1][];
      yMidDots[0] = new double[y0.length];
    }

    // initial scaling
    final double[] scale = new double[mainSetDimension];
    rescale(y, y, scale);

    // initial order selection
    final double tol =
        (vecRelativeTolerance == null) ? scalRelativeTolerance : vecRelativeTolerance[0];
    final double log10R = FastMath.log10(FastMath.max(1.0e-10, tol));
    int targetIter = FastMath.max(1,
                              FastMath.min(sequence.length - 2,
                                       (int) FastMath.floor(0.5 - 0.6 * log10R)));
    // set up an interpolator sharing the integrator arrays
    AbstractStepInterpolator interpolator = null;
    if (denseOutput) {
      interpolator = new GraggBulirschStoerStepInterpolator(y, yDot0,
                                                            y1, yDot1,
                                                            yMidDots, forward);
    } else {
      interpolator = new DummyStepInterpolator(y, yDot1, forward);
    }
    interpolator.storeTime(t0);

    stepStart = t0;
    double  hNew             = 0;
    double  maxError         = Double.MAX_VALUE;
    boolean previousRejected = false;
    boolean firstTime        = true;
    boolean newStep          = true;
    boolean firstStepAlreadyComputed = false;
    for (StepHandler handler : stepHandlers) {
        handler.reset();
    }
    setStateInitialized(false);
    costPerTimeUnit[0] = 0;
    isLastStep = false;
    do {

      double error;
      boolean reject = false;

      if (newStep) {

        interpolator.shift();

        // first evaluation, at the beginning of the step
        if (! firstStepAlreadyComputed) {
          computeDerivatives(stepStart, y, yDot0);
        }

        if (firstTime) {
          hNew = initializeStep(equations, forward,
                                2 * targetIter + 1, scale,
                                stepStart, y, yDot0, yTmp, yTmpDot);
        }

        newStep = false;

      }

      stepSize = hNew;

      // step adjustment near bounds
      if ((forward && (stepStart + stepSize > t)) ||
          ((! forward) && (stepStart + stepSize < t))) {
        stepSize = t - stepStart;
      }
      final double nextT = stepStart + stepSize;
      isLastStep = forward ? (nextT >= t) : (nextT <= t);

      // iterate over several substep sizes
      int k = -1;
      for (boolean loop = true; loop; ) {

        ++k;

        // modified midpoint integration with the current substep
        if ( ! tryStep(stepStart, y, stepSize, k, scale, fk[k],
                       (k == 0) ? yMidDots[0] : diagonal[k-1],
                       (k == 0) ? y1 : y1Diag[k-1],
                       yTmp)) {

          // the stability check failed, we reduce the global step
          hNew   = FastMath.abs(filterStep(stepSize * stabilityReduction, forward, false));
          reject = true;
          loop   = false;

        } else {

          // the substep was computed successfully
          if (k > 0) {

            // extrapolate the state at the end of the step
            // using last iteration data
            extrapolate(0, k, y1Diag, y1);
            rescale(y, y1, scale);

            // estimate the error at the end of the step.
            error = 0;
            for (int j = 0; j < mainSetDimension; ++j) {
              final double e = FastMath.abs(y1[j] - y1Diag[0][j]) / scale[j];
              error += e * e;
            }
            error = FastMath.sqrt(error / mainSetDimension);

            if ((error > 1.0e15) || ((k > 1) && (error > maxError))) {
              // error is too big, we reduce the global step
              hNew   = FastMath.abs(filterStep(stepSize * stabilityReduction, forward, false));
              reject = true;
              loop   = false;
            } else {

              maxError = FastMath.max(4 * error, 1.0);

              // compute optimal stepsize for this order
              final double exp = 1.0 / (2 * k + 1);
              double fac = stepControl2 / FastMath.pow(error / stepControl1, exp);
              final double pow = FastMath.pow(stepControl3, exp);
              fac = FastMath.max(pow / stepControl4, FastMath.min(1 / pow, fac));
              optimalStep[k]     = FastMath.abs(filterStep(stepSize * fac, forward, true));
              costPerTimeUnit[k] = costPerStep[k] / optimalStep[k];

              // check convergence
              switch (k - targetIter) {

              case -1 :
                if ((targetIter > 1) && ! previousRejected) {

                  // check if we can stop iterations now
                  if (error <= 1.0) {
                    // convergence have been reached just before targetIter
                    loop = false;
                  } else {
                    // estimate if there is a chance convergence will
                    // be reached on next iteration, using the
                    // asymptotic evolution of error
                    final double ratio = ((double) sequence [targetIter] * sequence[targetIter + 1]) /
                                         (sequence[0] * sequence[0]);
                    if (error > ratio * ratio) {
                      // we don't expect to converge on next iteration
                      // we reject the step immediately and reduce order
                      reject = true;
                      loop   = false;
                      targetIter = k;
                      if ((targetIter > 1) &&
                          (costPerTimeUnit[targetIter-1] <
                           orderControl1 * costPerTimeUnit[targetIter])) {
                        --targetIter;
                      }
                      hNew = optimalStep[targetIter];
                    }
                  }
                }
                break;

              case 0:
                if (error <= 1.0) {
                  // convergence has been reached exactly at targetIter
                  loop = false;
                } else {
                  // estimate if there is a chance convergence will
                  // be reached on next iteration, using the
                  // asymptotic evolution of error
                  final double ratio = ((double) sequence[k+1]) / sequence[0];
                  if (error > ratio * ratio) {
                    // we don't expect to converge on next iteration
                    // we reject the step immediately
                    reject = true;
                    loop = false;
                    if ((targetIter > 1) &&
                        (costPerTimeUnit[targetIter-1] <
                         orderControl1 * costPerTimeUnit[targetIter])) {
                      --targetIter;
                    }
                    hNew = optimalStep[targetIter];
                  }
                }
                break;

              case 1 :
                if (error > 1.0) {
                  reject = true;
                  if ((targetIter > 1) &&
                      (costPerTimeUnit[targetIter-1] <
                       orderControl1 * costPerTimeUnit[targetIter])) {
                    --targetIter;
                  }
                  hNew = optimalStep[targetIter];
                }
                loop = false;
                break;

              default :
                if ((firstTime || isLastStep) && (error <= 1.0)) {
                  loop = false;
                }
                break;

              }

            }
          }
        }
      }

      if (! reject) {
          // derivatives at end of step
          computeDerivatives(stepStart + stepSize, y1, yDot1);
      }

      // dense output handling
      double hInt = getMaxStep();
      if (denseOutput && ! reject) {

        // extrapolate state at middle point of the step
        for (int j = 1; j <= k; ++j) {
          extrapolate(0, j, diagonal, yMidDots[0]);
        }

        final int mu = 2 * k - mudif + 3;

        for (int l = 0; l < mu; ++l) {

          // derivative at middle point of the step
          final int l2 = l / 2;
          double factor = FastMath.pow(0.5 * sequence[l2], l);
          int middleIndex = fk[l2].length / 2;
          for (int i = 0; i < y0.length; ++i) {
            yMidDots[l+1][i] = factor * fk[l2][middleIndex + l][i];
          }
          for (int j = 1; j <= k - l2; ++j) {
            factor = FastMath.pow(0.5 * sequence[j + l2], l);
            middleIndex = fk[l2+j].length / 2;
            for (int i = 0; i < y0.length; ++i) {
              diagonal[j-1][i] = factor * fk[l2+j][middleIndex+l][i];
            }
            extrapolate(l2, j, diagonal, yMidDots[l+1]);
          }
          for (int i = 0; i < y0.length; ++i) {
            yMidDots[l+1][i] *= stepSize;
          }

          // compute centered differences to evaluate next derivatives
          for (int j = (l + 1) / 2; j <= k; ++j) {
            for (int m = fk[j].length - 1; m >= 2 * (l + 1); --m) {
              for (int i = 0; i < y0.length; ++i) {
                fk[j][m][i] -= fk[j][m-2][i];
              }
            }
          }

        }

        if (mu >= 0) {

          // estimate the dense output coefficients
          final GraggBulirschStoerStepInterpolator gbsInterpolator
            = (GraggBulirschStoerStepInterpolator) interpolator;
          gbsInterpolator.computeCoefficients(mu, stepSize);

          if (useInterpolationError) {
            // use the interpolation error to limit stepsize
            final double interpError = gbsInterpolator.estimateError(scale);
            hInt = FastMath.abs(stepSize / FastMath.max(FastMath.pow(interpError, 1.0 / (mu+4)),
                                                0.01));
            if (interpError > 10.0) {
              hNew = hInt;
              reject = true;
            }
          }

        }

      }

      if (! reject) {

        // Discrete events handling
        interpolator.storeTime(stepStart + stepSize);
        stepStart = acceptStep(interpolator, y1, yDot1, t);

        // prepare next step
        interpolator.storeTime(stepStart);
        System.arraycopy(y1, 0, y, 0, y0.length);
        System.arraycopy(yDot1, 0, yDot0, 0, y0.length);
        firstStepAlreadyComputed = true;

        int optimalIter;
        if (k == 1) {
          optimalIter = 2;
          if (previousRejected) {
            optimalIter = 1;
          }
        } else if (k <= targetIter) {
          optimalIter = k;
          if (costPerTimeUnit[k-1] < orderControl1 * costPerTimeUnit[k]) {
            optimalIter = k-1;
          } else if (costPerTimeUnit[k] < orderControl2 * costPerTimeUnit[k-1]) {
            optimalIter = FastMath.min(k+1, sequence.length - 2);
          }
        } else {
          optimalIter = k - 1;
          if ((k > 2) &&
              (costPerTimeUnit[k-2] < orderControl1 * costPerTimeUnit[k-1])) {
            optimalIter = k - 2;
          }
          if (costPerTimeUnit[k] < orderControl2 * costPerTimeUnit[optimalIter]) {
            optimalIter = FastMath.min(k, sequence.length - 2);
          }
        }

        if (previousRejected) {
          // after a rejected step neither order nor stepsize
          // should increase
          targetIter = FastMath.min(optimalIter, k);
          hNew = FastMath.min(FastMath.abs(stepSize), optimalStep[targetIter]);
        } else {
          // stepsize control
          if (optimalIter <= k) {
            hNew = optimalStep[optimalIter];
          } else {
            if ((k < targetIter) &&
                (costPerTimeUnit[k] < orderControl2 * costPerTimeUnit[k-1])) {
              hNew = filterStep(optimalStep[k] * costPerStep[optimalIter+1] / costPerStep[k],
                               forward, false);
            } else {
              hNew = filterStep(optimalStep[k] * costPerStep[optimalIter] / costPerStep[k],
                                forward, false);
            }
          }

          targetIter = optimalIter;

        }

        newStep = true;

      }

      hNew = FastMath.min(hNew, hInt);
      if (! forward) {
        hNew = -hNew;
      }

      firstTime = false;

      if (reject) {
        isLastStep = false;
        previousRejected = true;
      } else {
        previousRejected = false;
      }

    } while (!isLastStep);

    final double stopTime = stepStart;
    resetInternalState();
    return stopTime;

  }

}