protected <T extends RealFieldElement<T>> void doInterpolationAtBounds(
      final Field<T> field, double epsilon) {

    RungeKuttaFieldStepInterpolator<T> interpolator =
        setUpInterpolator(field, new SinCos<T>(field), 0.0, new double[] {0.0, 1.0}, 0.125);

    Assert.assertEquals(0.0, interpolator.getPreviousState().getTime().getReal(), 1.0e-15);
    for (int i = 0; i < 2; ++i) {
      Assert.assertEquals(
          interpolator.getPreviousState().getState()[i].getReal(),
          interpolator
              .getInterpolatedState(interpolator.getPreviousState().getTime())
              .getState()[i]
              .getReal(),
          epsilon);
    }
    Assert.assertEquals(0.125, interpolator.getCurrentState().getTime().getReal(), 1.0e-15);
    for (int i = 0; i < 2; ++i) {
      Assert.assertEquals(
          interpolator.getCurrentState().getState()[i].getReal(),
          interpolator
              .getInterpolatedState(interpolator.getCurrentState().getTime())
              .getState()[i]
              .getReal(),
          epsilon);
    }
  }
  protected <T extends RealFieldElement<T>> void doInterpolationInside(
      final Field<T> field, double epsilonSin, double epsilonCos) {

    RungeKuttaFieldStepInterpolator<T> interpolator =
        setUpInterpolator(field, new SinCos<T>(field), 0.0, new double[] {0.0, 1.0}, 0.0125);

    int n = 100;
    double maxErrorSin = 0;
    double maxErrorCos = 0;
    for (int i = 0; i <= n; ++i) {
      T t =
          interpolator
              .getPreviousState()
              .getTime()
              .multiply(n - i)
              .add(interpolator.getCurrentState().getTime().multiply(i))
              .divide(n);
      FieldODEStateAndDerivative<T> state = interpolator.getInterpolatedState(t);
      maxErrorSin =
          FastMath.max(maxErrorSin, state.getState()[0].subtract(t.sin()).abs().getReal());
      maxErrorCos =
          FastMath.max(maxErrorCos, state.getState()[1].subtract(t.cos()).abs().getReal());
    }
    Assert.assertEquals(0.0, maxErrorSin, epsilonSin);
    Assert.assertEquals(0.0, maxErrorCos, epsilonCos);
  }
  protected <T extends RealFieldElement<T>> void doNonFieldInterpolatorConsistency(
      final Field<T> field,
      double epsilonSin,
      double epsilonCos,
      double epsilonSinDot,
      double epsilonCosDot) {

    FirstOrderFieldDifferentialEquations<T> eqn = new SinCos<T>(field);
    RungeKuttaFieldStepInterpolator<T> fieldInterpolator =
        setUpInterpolator(field, eqn, 0.0, new double[] {0.0, 1.0}, 0.125);
    RungeKuttaStepInterpolator regularInterpolator = convertInterpolator(fieldInterpolator, eqn);

    int n = 100;
    double maxErrorSin = 0;
    double maxErrorCos = 0;
    double maxErrorSinDot = 0;
    double maxErrorCosDot = 0;
    for (int i = 0; i <= n; ++i) {

      T t =
          fieldInterpolator
              .getPreviousState()
              .getTime()
              .multiply(n - i)
              .add(fieldInterpolator.getCurrentState().getTime().multiply(i))
              .divide(n);

      FieldODEStateAndDerivative<T> state = fieldInterpolator.getInterpolatedState(t);
      T[] fieldY = state.getState();
      T[] fieldYDot = state.getDerivative();

      regularInterpolator.setInterpolatedTime(t.getReal());
      double[] regularY = regularInterpolator.getInterpolatedState();
      double[] regularYDot = regularInterpolator.getInterpolatedDerivatives();

      maxErrorSin = FastMath.max(maxErrorSin, fieldY[0].subtract(regularY[0]).abs().getReal());
      maxErrorCos = FastMath.max(maxErrorCos, fieldY[1].subtract(regularY[1]).abs().getReal());
      maxErrorSinDot =
          FastMath.max(maxErrorSinDot, fieldYDot[0].subtract(regularYDot[0]).abs().getReal());
      maxErrorCosDot =
          FastMath.max(maxErrorCosDot, fieldYDot[1].subtract(regularYDot[1]).abs().getReal());
    }
    Assert.assertEquals(0.0, maxErrorSin, epsilonSin);
    Assert.assertEquals(0.0, maxErrorCos, epsilonCos);
    Assert.assertEquals(0.0, maxErrorSinDot, epsilonSinDot);
    Assert.assertEquals(0.0, maxErrorCosDot, epsilonCosDot);
  }
  private <T extends RealFieldElement<T>> RungeKuttaStepInterpolator convertInterpolator(
      final RungeKuttaFieldStepInterpolator<T> fieldInterpolator,
      final FirstOrderFieldDifferentialEquations<T> eqn) {

    RungeKuttaStepInterpolator regularInterpolator = null;
    try {

      String interpolatorName = fieldInterpolator.getClass().getName();
      String integratorName = interpolatorName.replaceAll("Field", "");
      @SuppressWarnings("unchecked")
      Class<RungeKuttaStepInterpolator> clz =
          (Class<RungeKuttaStepInterpolator>) Class.forName(integratorName);
      regularInterpolator = clz.newInstance();

      double[][] yDotArray = null;
      java.lang.reflect.Field fYD = RungeKuttaFieldStepInterpolator.class.getDeclaredField("yDotK");
      fYD.setAccessible(true);
      @SuppressWarnings("unchecked")
      T[][] fieldYDotk = (T[][]) fYD.get(fieldInterpolator);
      yDotArray = new double[fieldYDotk.length][];
      for (int i = 0; i < yDotArray.length; ++i) {
        yDotArray[i] = new double[fieldYDotk[i].length];
        for (int j = 0; j < yDotArray[i].length; ++j) {
          yDotArray[i][j] = fieldYDotk[i][j].getReal();
        }
      }
      double[] y = new double[yDotArray[0].length];

      EquationsMapper primaryMapper = null;
      EquationsMapper[] secondaryMappers = null;
      java.lang.reflect.Field fMapper =
          AbstractFieldStepInterpolator.class.getDeclaredField("mapper");
      fMapper.setAccessible(true);
      @SuppressWarnings("unchecked")
      FieldEquationsMapper<T> mapper = (FieldEquationsMapper<T>) fMapper.get(fieldInterpolator);
      java.lang.reflect.Field fStart = FieldEquationsMapper.class.getDeclaredField("start");
      fStart.setAccessible(true);
      int[] start = (int[]) fStart.get(mapper);
      primaryMapper = new EquationsMapper(start[0], start[1]);
      secondaryMappers = new EquationsMapper[mapper.getNumberOfEquations() - 1];
      for (int i = 0; i < secondaryMappers.length; ++i) {
        secondaryMappers[i] = new EquationsMapper(start[i + 1], start[i + 2]);
      }

      AbstractIntegrator dummyIntegrator =
          new AbstractIntegrator("dummy") {
            @Override
            public void integrate(ExpandableStatefulODE equations, double t) {
              Assert.fail("this method should not be called");
            }

            @Override
            public void computeDerivatives(final double t, final double[] y, final double[] yDot) {
              T fieldT = fieldInterpolator.getCurrentState().getTime().getField().getZero().add(t);
              T[] fieldY =
                  MathArrays.buildArray(
                      fieldInterpolator.getCurrentState().getTime().getField(), y.length);
              for (int i = 0; i < y.length; ++i) {
                fieldY[i] =
                    fieldInterpolator.getCurrentState().getTime().getField().getZero().add(y[i]);
              }
              T[] fieldYDot = eqn.computeDerivatives(fieldT, fieldY);
              for (int i = 0; i < yDot.length; ++i) {
                yDot[i] = fieldYDot[i].getReal();
              }
            }
          };
      regularInterpolator.reinitialize(
          dummyIntegrator,
          y,
          yDotArray,
          fieldInterpolator.isForward(),
          primaryMapper,
          secondaryMappers);

      T[] fieldPreviousY = fieldInterpolator.getPreviousState().getState();
      for (int i = 0; i < y.length; ++i) {
        y[i] = fieldPreviousY[i].getReal();
      }
      regularInterpolator.storeTime(fieldInterpolator.getPreviousState().getTime().getReal());

      regularInterpolator.shift();

      T[] fieldCurrentY = fieldInterpolator.getCurrentState().getState();
      for (int i = 0; i < y.length; ++i) {
        y[i] = fieldCurrentY[i].getReal();
      }
      regularInterpolator.storeTime(fieldInterpolator.getCurrentState().getTime().getReal());

    } catch (ClassNotFoundException cnfe) {
      Assert.fail(cnfe.getLocalizedMessage());
    } catch (InstantiationException ie) {
      Assert.fail(ie.getLocalizedMessage());
    } catch (IllegalAccessException iae) {
      Assert.fail(iae.getLocalizedMessage());
    } catch (NoSuchFieldException nsfe) {
      Assert.fail(nsfe.getLocalizedMessage());
    } catch (IllegalArgumentException iae) {
      Assert.fail(iae.getLocalizedMessage());
    }

    return regularInterpolator;
  }