@Test(expectedExceptions = IllegalArgumentException.class)
  public void nullxDiffTest() {
    double[] xValues = new double[] {1, 2, 3, 4};
    DoubleMatrix2D coefsMatrix =
        new DoubleMatrix2D(
            new double[][] {
              {1., -3., 3., -1},
              {0., 5., -20., 20},
              {1., 0., 0., 0.},
              {0., 5., -10., 5},
              {1., 3., 3., 1.},
              {0., 5., 0., 0.}
            });
    double[] xKeys = new double[] {-2, 1, 2, 2.5};
    final int dim = 2;
    final int nCoefs = 4;

    xKeys = null;

    PiecewisePolynomialResult pp =
        new PiecewisePolynomialResult(new DoubleMatrix1D(xValues), coefsMatrix, nCoefs, dim);
    PiecewisePolynomialFunction1D function = new PiecewisePolynomialFunction1D();

    function.differentiate(pp, xKeys);
  }
 @Override
 public double firstDerivative(final Interpolator1DDataBundle data, final Double value) {
   Validate.notNull(value, "value");
   Validate.notNull(data, "data bundle");
   Validate.isTrue(data instanceof Interpolator1DPiecewisePoynomialWithExtraKnotsDataBundle);
   final Interpolator1DPiecewisePoynomialWithExtraKnotsDataBundle polyData =
       (Interpolator1DPiecewisePoynomialWithExtraKnotsDataBundle) data;
   final DoubleMatrix1D res = FUNC.differentiate(polyData.getPiecewisePolynomialResult(), value);
   return res.getEntry(0);
 }
  @Test
  public void linearAllTest() {

    final double[] knots = new double[] {1., 4.};
    final DoubleMatrix2D coefsMatrix = new DoubleMatrix2D(new double[][] {{0., 1., 1.}});
    final double[] xKeys = new double[] {-2, 1., 2.5, 4.};
    final double[] initials = new double[] {-0.5, 1., 2.5, 5.};
    final int nKeys = xKeys.length;
    final int nInit = initials.length;

    final double[] valuesExp = new double[] {-2, 1, 2.5, 4.};
    final double[][] integrateExp = new double[nInit][nKeys];
    for (int i = 0; i < nInit; ++i) {
      for (int j = 0; j < nKeys; ++j) {
        integrateExp[i][j] = 0.5 * (xKeys[j] * xKeys[j] - initials[i] * initials[i]);
      }
    }
    final double[] differentiateExp = new double[] {1., 1., 1., 1.};

    PiecewisePolynomialResult result =
        new PiecewisePolynomialResult(new DoubleMatrix1D(knots), coefsMatrix, 3, 1);
    PiecewisePolynomialFunction1D function = new PiecewisePolynomialFunction1D();

    final double[] values = function.evaluate(result, xKeys).getData()[0];
    final double[] differentiate = function.differentiate(result, xKeys).getData()[0];
    final double[][] integrate = new double[nInit][nKeys];
    for (int i = 0; i < nInit; ++i) {
      for (int j = 0; j < nKeys; ++j) {
        integrate[i][j] = function.integrate(result, initials[i], xKeys).getData()[j];
      }
    }

    for (int i = 0; i < nKeys; ++i) {
      final double ref = valuesExp[i] == 0. ? 1. : Math.abs(valuesExp[i]);
      assertEquals(values[i], valuesExp[i], ref * EPS);
    }

    for (int i = 0; i < nKeys; ++i) {
      final double ref = differentiateExp[i] == 0. ? 1. : Math.abs(differentiateExp[i]);
      assertEquals(differentiate[i], differentiateExp[i], ref * EPS);
    }

    for (int j = 0; j < nInit; ++j) {
      for (int i = 0; i < nKeys; ++i) {
        final double ref = integrateExp[j][i] == 0. ? 1. : Math.abs(integrateExp[j][i]);
        assertEquals(integrate[j][i], integrateExp[j][i], ref * EPS);
      }
    }
  }
  @Test
  public void crossDerivativeTest() {
    double[] x0Values = new double[] {1., 2., 3., 4.};
    double[] x1Values = new double[] {-1., 0., 1., 2., 3.};
    final int n0Data = x0Values.length;
    final int n1Data = x1Values.length;
    double[][] yValues =
        new double[][] {
          {
            1.0, -1.0, 0.0, 1.0, 0.0,
          },
          {1.0, -1.0, 0.0, 1.0, -2.0},
          {1.0, -2.0, 0.0, -2.0, -2.0},
          {-1.0, -1.0, -2.0, -2.0, -1.0}
        };

    NaturalSplineInterpolator method = new NaturalSplineInterpolator();
    PiecewisePolynomialInterpolator2D interp = new BicubicSplineInterpolator(method);
    PiecewisePolynomialResult2D result = interp.interpolate(x0Values, x1Values, yValues);

    final int n0IntExp = n0Data - 1;
    final int n1IntExp = n1Data - 1;
    final int orderExp = 4;

    final int n0Keys = 51;
    final int n1Keys = 61;
    double[] x0Keys = new double[n0Keys];
    double[] x1Keys = new double[n1Keys];
    for (int i = 0; i < n0Keys; ++i) {
      x0Keys[i] = 0. + 5. * i / (n0Keys - 1);
    }
    for (int i = 0; i < n1Keys; ++i) {
      x1Keys[i] = -2. + 6. * i / (n1Keys - 1);
    }

    assertEquals(result.getNumberOfIntervals()[0], n0IntExp);
    assertEquals(result.getNumberOfIntervals()[1], n1IntExp);
    assertEquals(result.getOrder()[0], orderExp);
    assertEquals(result.getOrder()[1], orderExp);

    for (int i = 0; i < n0Data; ++i) {
      final double ref = Math.abs(x0Values[i]) == 0. ? 1. : Math.abs(x0Values[i]);
      assertEquals(result.getKnots0().getData()[i], x0Values[i], ref * EPS);
      assertEquals(result.getKnots2D().get(0).getData()[i], x0Values[i], ref * EPS);
    }
    for (int i = 0; i < n1Data; ++i) {
      final double ref = Math.abs(x1Values[i]) == 0. ? 1. : Math.abs(x1Values[i]);
      assertEquals(result.getKnots1().getData()[i], x1Values[i], ref * EPS);
      assertEquals(result.getKnots2D().get(1).getData()[i], x1Values[i], ref * EPS);
    }

    for (int i = 0; i < n0Data - 1; ++i) {
      for (int j = 0; j < n1Data - 1; ++j) {
        final double ref = Math.abs(yValues[i][j]) == 0. ? 1. : Math.abs(yValues[i][j]);
        assertEquals(
            result.getCoefs()[i][j].getData()[orderExp - 1][orderExp - 1],
            yValues[i][j],
            ref * EPS);
      }
    }

    double[][] resValues =
        interp.interpolate(x0Values, x1Values, yValues, x0Values, x1Values).getData();
    final PiecewisePolynomialFunction2D func2D = new PiecewisePolynomialFunction2D();
    double[][] resDiffX0 = func2D.differentiateX0(result, x0Values, x1Values).getData();
    double[][] resDiffX1 = func2D.differentiateX1(result, x0Values, x1Values).getData();

    final PiecewisePolynomialFunction1D func1D = new PiecewisePolynomialFunction1D();
    double[][] expDiffX0 =
        func1D
            .differentiate(
                method.interpolate(
                    x0Values, OG_ALGEBRA.getTranspose(new DoubleMatrix2D(yValues)).getData()),
                x0Values)
            .getData();
    double[][] expDiffX1 =
        func1D.differentiate(method.interpolate(x1Values, yValues), x1Values).getData();

    for (int i = 0; i < n0Data; ++i) {
      for (int j = 0; j < n1Data; ++j) {
        final double expVal = expDiffX1[i][j];
        final double ref = Math.abs(expVal) == 0. ? 1. : Math.abs(expVal);
        assertEquals(resDiffX1[i][j], expVal, ref * EPS);
      }
    }
    //    System.out.println(new DoubleMatrix2D(expDiffX0));
    //    System.out.println(new DoubleMatrix2D(resDiffX0));
    for (int i = 0; i < n0Data; ++i) {
      for (int j = 0; j < n1Data; ++j) {
        final double expVal = expDiffX0[j][i];
        final double ref = Math.abs(expVal) == 0. ? 1. : Math.abs(expVal);
        assertEquals(resDiffX0[i][j], expVal, ref * EPS);
      }
    }

    for (int i = 0; i < n0Data; ++i) {
      for (int j = 0; j < n1Data; ++j) {
        final double expVal = yValues[i][j];
        final double ref = Math.abs(expVal) == 0. ? 1. : Math.abs(expVal);
        assertEquals(resValues[i][j], expVal, ref * EPS);
      }
    }
  }
  /** Sample function is f(x) = (x-1)^4 */
  @Test
  public void GeneralIntegrateDifferentiateTest() {
    final double[] knots = new double[] {1., 2., 3., 4};
    final double[][] coefMat =
        new double[][] {{1., 0., 0., 0., 0.}, {1., 4., 6., 4., 1.}, {1., 8., 24., 32., 16.}};
    final double[] xKeys = new double[] {-2, 1, 2.5, 4.};
    final double[] initials = new double[] {1., 2.5, 23. / 7., 7.};
    final int nKeys = xKeys.length;
    final int nInit = initials.length;

    final double[][] integrateExp = new double[nInit][nKeys];
    for (int i = 0; i < nInit; ++i) {
      for (int j = 0; j < nKeys; ++j) {
        integrateExp[i][j] = Math.pow(xKeys[j] - 1., 5.) / 5. - Math.pow(initials[i] - 1., 5.) / 5.;
      }
    }
    final double[] differentiateExp = new double[] {-108., 0., 27. / 2., 108.};
    final double[] differentiateTwiceExp = new double[nKeys];
    for (int i = 0; i < nKeys; ++i) {
      differentiateTwiceExp[i] = 12. * (xKeys[i] - 1.) * (xKeys[i] - 1.);
    }

    PiecewisePolynomialFunction1D function = new PiecewisePolynomialFunction1D();
    PiecewisePolynomialResult result =
        new PiecewisePolynomialResult(new DoubleMatrix1D(knots), new DoubleMatrix2D(coefMat), 5, 1);

    final double[] differentiate = function.differentiate(result, xKeys).getData()[0];
    final double[] differentiateTwice = function.differentiateTwice(result, xKeys).getData()[0];
    final double[][] integrate = new double[nInit][nKeys];
    for (int i = 0; i < nInit; ++i) {
      for (int j = 0; j < nKeys; ++j) {
        integrate[i][j] = function.integrate(result, initials[i], xKeys).getData()[j];
      }
    }

    for (int i = 0; i < nKeys; ++i) {
      final double ref = differentiateExp[i] == 0. ? 1. : Math.abs(differentiateExp[i]);
      assertEquals(differentiate[i], differentiateExp[i], ref * EPS);
    }
    for (int i = 0; i < nKeys; ++i) {
      final double ref = differentiateTwiceExp[i] == 0. ? 1. : Math.abs(differentiateTwiceExp[i]);
      assertEquals(differentiateTwice[i], differentiateTwiceExp[i], ref * EPS);
    }

    for (int j = 0; j < nInit; ++j) {
      for (int i = 0; i < nKeys; ++i) {
        final double ref = integrateExp[j][i] == 0. ? 1. : Math.abs(integrateExp[j][i]);
        assertEquals(integrate[j][i], integrateExp[j][i], ref * EPS);
      }
    }

    {
      final double ref = differentiateExp[0] == 0. ? 1. : Math.abs(differentiateExp[0]);
      assertEquals(
          function.differentiate(result, xKeys[0]).getData()[0], differentiateExp[0], ref * EPS);
    }
    {
      final double ref = differentiateExp[3] == 0. ? 1. : Math.abs(differentiateExp[3]);
      assertEquals(
          function.differentiate(result, xKeys[3]).getData()[0], differentiateExp[3], ref * EPS);
    }
    {
      final double ref = differentiateTwiceExp[0] == 0. ? 1. : Math.abs(differentiateTwiceExp[0]);
      assertEquals(
          function.differentiateTwice(result, xKeys[0]).getData()[0],
          differentiateTwiceExp[0],
          ref * EPS);
    }
    {
      final double ref = differentiateTwiceExp[3] == 0. ? 1. : Math.abs(differentiateTwiceExp[3]);
      assertEquals(
          function.differentiateTwice(result, xKeys[3]).getData()[0],
          differentiateTwiceExp[3],
          ref * EPS);
    }
    {
      final double ref = integrateExp[0][0] == 0. ? 1. : Math.abs(integrateExp[0][0]);
      assertEquals(
          function.integrate(result, initials[0], xKeys[0]), integrateExp[0][0], ref * EPS);
    }
    {
      final double ref = integrateExp[0][3] == 0. ? 1. : Math.abs(integrateExp[0][3]);
      assertEquals(
          function.integrate(result, initials[0], xKeys[3]), integrateExp[0][3], ref * EPS);
    }
    {
      final double ref = integrateExp[3][0] == 0. ? 1. : Math.abs(integrateExp[3][0]);
      assertEquals(
          function.integrate(result, initials[3], xKeys[0]), integrateExp[3][0], ref * EPS);
    }
    {
      final double ref = integrateExp[1][0] == 0. ? 1. : Math.abs(integrateExp[1][0]);
      assertEquals(
          function.integrate(result, initials[1], xKeys[0]), integrateExp[1][0], ref * EPS);
    }
  }
  @Override
  public PiecewisePolynomialResult interpolate(final double[] xValues, final double[] yValues) {
    ArgumentChecker.notNull(xValues, "xValues");
    ArgumentChecker.notNull(yValues, "yValues");

    ArgumentChecker.isTrue(
        xValues.length == yValues.length | xValues.length + 2 == yValues.length,
        "(xValues length = yValues length) or (xValues length + 2 = yValues length)");
    ArgumentChecker.isTrue(xValues.length > 2, "Data points should be more than 2");

    final int nDataPts = xValues.length;
    final int yValuesLen = yValues.length;

    for (int i = 0; i < nDataPts; ++i) {
      ArgumentChecker.isFalse(Double.isNaN(xValues[i]), "xValues containing NaN");
      ArgumentChecker.isFalse(Double.isInfinite(xValues[i]), "xValues containing Infinity");
    }
    for (int i = 0; i < yValuesLen; ++i) {
      ArgumentChecker.isFalse(Double.isNaN(yValues[i]), "yValues containing NaN");
      ArgumentChecker.isFalse(Double.isInfinite(yValues[i]), "yValues containing Infinity");
    }

    for (int i = 0; i < nDataPts - 1; ++i) {
      for (int j = i + 1; j < nDataPts; ++j) {
        ArgumentChecker.isFalse(xValues[i] == xValues[j], "xValues should be distinct");
      }
    }

    double[] xValuesSrt = Arrays.copyOf(xValues, nDataPts);
    double[] yValuesSrt = new double[nDataPts];
    if (nDataPts == yValuesLen) {
      yValuesSrt = Arrays.copyOf(yValues, nDataPts);
    } else {
      yValuesSrt = Arrays.copyOfRange(yValues, 1, nDataPts + 1);
    }
    ParallelArrayBinarySort.parallelBinarySort(xValuesSrt, yValuesSrt);

    final double[] intervals = _solver.intervalsCalculator(xValuesSrt);
    final double[] slopes = _solver.slopesCalculator(yValuesSrt, intervals);
    final PiecewisePolynomialResult result = _method.interpolate(xValues, yValues);

    ArgumentChecker.isTrue(result.getOrder() >= 3, "Primary interpolant should be degree >= 2");

    final double[] initialFirst = _function.differentiate(result, xValuesSrt).getData()[0];
    final double[] initialSecond = _function.differentiateTwice(result, xValuesSrt).getData()[0];
    double[] first = firstDerivativeCalculator(yValuesSrt, intervals, slopes, initialFirst);

    boolean modFirst = false;
    int k;
    double[] aValues = aValuesCalculator(slopes, first);
    double[] bValues = bValuesCalculator(slopes, first);
    double[][] intervalsA = getIntervalsA(intervals, slopes, first, bValues);
    double[][] intervalsB = getIntervalsB(intervals, slopes, first, aValues);
    while (modFirst == false) {
      k = 0;
      for (int i = 0; i < nDataPts - 2; ++i) {
        if (first[i + 1] > 0.) {
          if (intervalsA[i + 1][1] + Math.abs(intervalsA[i + 1][1]) * ERROR
                  < intervalsB[i][0] - Math.abs(intervalsB[i][0]) * ERROR
              | intervalsA[i + 1][0] - Math.abs(intervalsA[i + 1][0]) * ERROR
                  > intervalsB[i][1] + Math.abs(intervalsB[i][1]) * ERROR) {
            ++k;
            first[i + 1] = firstDerivativesRecalculator(intervals, slopes, aValues, bValues, i + 1);
          }
        }
      }
      if (k == 0) {
        modFirst = true;
      }
      aValues = aValuesCalculator(slopes, first);
      bValues = bValuesCalculator(slopes, first);
      intervalsA = getIntervalsA(intervals, slopes, first, bValues);
      intervalsB = getIntervalsB(intervals, slopes, first, aValues);
    }
    final double[] second = secondDerivativeCalculator(initialSecond, intervalsA, intervalsB);
    final double[][] coefs = _solver.solve(yValuesSrt, intervals, slopes, first, second);

    for (int i = 0; i < nDataPts - 1; ++i) {
      for (int j = 0; j < 6; ++j) {
        ArgumentChecker.isFalse(Double.isNaN(coefs[i][j]), "Too large input");
        ArgumentChecker.isFalse(Double.isInfinite(coefs[i][j]), "Too large input");
      }
    }

    return new PiecewisePolynomialResult(
        new DoubleMatrix1D(xValuesSrt), new DoubleMatrix2D(coefs), 6, 1);
  }
  @Override
  public PiecewisePolynomialResultsWithSensitivity interpolateWithSensitivity(
      final double[] xValues, final double[] yValues) {

    ArgumentChecker.notNull(xValues, "xValues");
    ArgumentChecker.notNull(yValues, "yValues");

    ArgumentChecker.isTrue(
        xValues.length == yValues.length | xValues.length + 2 == yValues.length,
        "(xValues length = yValues length) or (xValues length + 2 = yValues length)");
    ArgumentChecker.isTrue(xValues.length > 2, "Data points should be more than 2");

    final int nDataPts = xValues.length;
    final int yValuesLen = yValues.length;

    for (int i = 0; i < nDataPts; ++i) {
      ArgumentChecker.isFalse(Double.isNaN(xValues[i]), "xValues containing NaN");
      ArgumentChecker.isFalse(Double.isInfinite(xValues[i]), "xValues containing Infinity");
    }
    for (int i = 0; i < yValuesLen; ++i) {
      ArgumentChecker.isFalse(Double.isNaN(yValues[i]), "yValues containing NaN");
      ArgumentChecker.isFalse(Double.isInfinite(yValues[i]), "yValues containing Infinity");
    }

    for (int i = 0; i < nDataPts - 1; ++i) {
      for (int j = i + 1; j < nDataPts; ++j) {
        ArgumentChecker.isFalse(xValues[i] == xValues[j], "xValues should be distinct");
      }
    }

    double[] yValuesSrt = new double[nDataPts];
    if (nDataPts == yValuesLen) {
      yValuesSrt = Arrays.copyOf(yValues, nDataPts);
    } else {
      yValuesSrt = Arrays.copyOfRange(yValues, 1, nDataPts + 1);
    }

    final double[] intervals = _solver.intervalsCalculator(xValues);
    final double[] slopes = _solver.slopesCalculator(yValuesSrt, intervals);
    double[][] slopesSensitivity = _solver.slopeSensitivityCalculator(intervals);
    final DoubleMatrix1D[] firstWithSensitivity = new DoubleMatrix1D[nDataPts + 1];
    final DoubleMatrix1D[] secondWithSensitivity = new DoubleMatrix1D[nDataPts + 1];
    final PiecewisePolynomialResult result = _method.interpolate(xValues, yValues);

    ArgumentChecker.isTrue(result.getOrder() >= 3, "Primary interpolant should be degree >= 2");

    final double[] initialFirst = _function.differentiate(result, xValues).getData()[0];
    final double[] initialSecond = _function.differentiateTwice(result, xValues).getData()[0];
    double[] first = firstDerivativeCalculator(yValuesSrt, intervals, slopes, initialFirst);
    boolean modFirst = false;
    int k;
    double[] aValues = aValuesCalculator(slopes, first);
    double[] bValues = bValuesCalculator(slopes, first);
    double[][] intervalsA = getIntervalsA(intervals, slopes, first, bValues);
    double[][] intervalsB = getIntervalsB(intervals, slopes, first, aValues);
    while (modFirst == false) {
      k = 0;
      for (int i = 0; i < nDataPts - 2; ++i) {
        if (first[i + 1] > 0.) {
          if (intervalsA[i + 1][1] + Math.abs(intervalsA[i + 1][1]) * ERROR
                  < intervalsB[i][0] - Math.abs(intervalsB[i][0]) * ERROR
              | intervalsA[i + 1][0] - Math.abs(intervalsA[i + 1][0]) * ERROR
                  > intervalsB[i][1] + Math.abs(intervalsB[i][1]) * ERROR) {
            ++k;
            first[i + 1] = firstDerivativesRecalculator(intervals, slopes, aValues, bValues, i + 1);
          }
        }
      }
      if (k == 0) {
        modFirst = true;
      }
      aValues = aValuesCalculator(slopes, first);
      bValues = bValuesCalculator(slopes, first);
      intervalsA = getIntervalsA(intervals, slopes, first, bValues);
      intervalsB = getIntervalsB(intervals, slopes, first, aValues);
    }
    final double[] second = secondDerivativeCalculator(initialSecond, intervalsA, intervalsB);
    firstWithSensitivity[0] = new DoubleMatrix1D(first);
    secondWithSensitivity[0] = new DoubleMatrix1D(second);

    /*
     * Centered finite difference method is used for computing node sensitivity
     */
    int nExtra = (nDataPts == yValuesLen) ? 0 : 1;
    final double[] yValuesUp = Arrays.copyOf(yValues, nDataPts + 2 * nExtra);
    final double[] yValuesDw = Arrays.copyOf(yValues, nDataPts + 2 * nExtra);
    final double[][] tmpFirst = new double[nDataPts][nDataPts];
    final double[][] tmpSecond = new double[nDataPts][nDataPts];
    for (int l = nExtra; l < nDataPts + nExtra; ++l) {
      final double den = Math.abs(yValues[l]) < SMALL ? EPS : yValues[l] * EPS;
      yValuesUp[l] = Math.abs(yValues[l]) < SMALL ? EPS : yValues[l] * (1. + EPS);
      yValuesDw[l] = Math.abs(yValues[l]) < SMALL ? -EPS : yValues[l] * (1. - EPS);
      final double[] yValuesSrtUp = Arrays.copyOfRange(yValuesUp, nExtra, nDataPts + nExtra);
      final double[] yValuesSrtDw = Arrays.copyOfRange(yValuesDw, nExtra, nDataPts + nExtra);

      final DoubleMatrix1D[] yValuesUpDw =
          new DoubleMatrix1D[] {new DoubleMatrix1D(yValuesUp), new DoubleMatrix1D(yValuesDw)};
      final DoubleMatrix1D[] yValuesSrtUpDw =
          new DoubleMatrix1D[] {new DoubleMatrix1D(yValuesSrtUp), new DoubleMatrix1D(yValuesSrtDw)};
      final DoubleMatrix1D[] firstSecondUpDw = new DoubleMatrix1D[4];
      for (int ii = 0; ii < 2; ++ii) {
        final double[] slopesUpDw =
            _solver.slopesCalculator(yValuesSrtUpDw[ii].getData(), intervals);
        final PiecewisePolynomialResult resultUpDw =
            _method.interpolate(xValues, yValuesUpDw[ii].getData());
        final double[] initialFirstUpDw = _function.differentiate(resultUpDw, xValues).getData()[0];
        final double[] initialSecondUpDw =
            _function.differentiateTwice(resultUpDw, xValues).getData()[0];
        double[] firstUpDw =
            firstDerivativeCalculator(
                yValuesSrtUpDw[ii].getData(), intervals, slopesUpDw, initialFirstUpDw);
        boolean modFirstUpDw = false;
        double[] aValuesUpDw = aValuesCalculator(slopesUpDw, firstUpDw);
        double[] bValuesUpDw = bValuesCalculator(slopesUpDw, firstUpDw);
        double[][] intervalsAUpDw = getIntervalsA(intervals, slopesUpDw, firstUpDw, bValuesUpDw);
        double[][] intervalsBUpDw = getIntervalsB(intervals, slopesUpDw, firstUpDw, aValuesUpDw);
        while (modFirstUpDw == false) {
          k = 0;
          for (int i = 0; i < nDataPts - 2; ++i) {
            if (firstUpDw[i + 1] > 0.) {
              if (intervalsAUpDw[i + 1][1] + Math.abs(intervalsAUpDw[i + 1][1]) * ERROR
                      < intervalsBUpDw[i][0] - Math.abs(intervalsBUpDw[i][0]) * ERROR
                  | intervalsAUpDw[i + 1][0] - Math.abs(intervalsAUpDw[i + 1][0]) * ERROR
                      > intervalsBUpDw[i][1] + Math.abs(intervalsBUpDw[i][1]) * ERROR) {
                ++k;
                firstUpDw[i + 1] =
                    firstDerivativesRecalculator(
                        intervals, slopesUpDw, aValuesUpDw, bValuesUpDw, i + 1);
              }
            }
          }
          if (k == 0) {
            modFirstUpDw = true;
          }
          aValuesUpDw = aValuesCalculator(slopesUpDw, firstUpDw);
          bValuesUpDw = bValuesCalculator(slopesUpDw, firstUpDw);
          intervalsAUpDw = getIntervalsA(intervals, slopesUpDw, firstUpDw, bValuesUpDw);
          intervalsBUpDw = getIntervalsB(intervals, slopesUpDw, firstUpDw, aValuesUpDw);
        }
        final double[] secondUpDw =
            secondDerivativeCalculator(initialSecondUpDw, intervalsAUpDw, intervalsBUpDw);
        firstSecondUpDw[ii] = new DoubleMatrix1D(firstUpDw);
        firstSecondUpDw[2 + ii] = new DoubleMatrix1D(secondUpDw);
      }
      for (int j = 0; j < nDataPts; ++j) {
        tmpFirst[j][l - nExtra] =
            0.5 * (firstSecondUpDw[0].getData()[j] - firstSecondUpDw[1].getData()[j]) / den;
        tmpSecond[j][l - nExtra] =
            0.5 * (firstSecondUpDw[2].getData()[j] - firstSecondUpDw[3].getData()[j]) / den;
      }
      yValuesUp[l] = yValues[l];
      yValuesDw[l] = yValues[l];
    }
    for (int i = 0; i < nDataPts; ++i) {
      firstWithSensitivity[i + 1] = new DoubleMatrix1D(tmpFirst[i]);
      secondWithSensitivity[i + 1] = new DoubleMatrix1D(tmpSecond[i]);
    }

    final DoubleMatrix2D[] resMatrix =
        _solver.solveWithSensitivity(
            yValuesSrt,
            intervals,
            slopes,
            slopesSensitivity,
            firstWithSensitivity,
            secondWithSensitivity);

    for (int l = 0; l < nDataPts; ++l) {
      DoubleMatrix2D m = resMatrix[l];
      final int rows = m.getNumberOfRows();
      final int cols = m.getNumberOfColumns();
      for (int i = 0; i < rows; ++i) {
        for (int j = 0; j < cols; ++j) {
          ArgumentChecker.isTrue(
              Doubles.isFinite(m.getEntry(i, j)), "Matrix contains a NaN or infinite");
        }
      }
    }

    final DoubleMatrix2D coefMatrix = resMatrix[0];
    final DoubleMatrix2D[] coefSenseMatrix = new DoubleMatrix2D[nDataPts - 1];
    System.arraycopy(resMatrix, 1, coefSenseMatrix, 0, nDataPts - 1);
    final int nCoefs = coefMatrix.getNumberOfColumns();

    return new PiecewisePolynomialResultsWithSensitivity(
        new DoubleMatrix1D(xValues), coefMatrix, nCoefs, 1, coefSenseMatrix);
  }