public void positivityClampedTest() {
    final double[] xValues = new double[] {1., 2., 3., 4., 5.};
    final double[] yValues = new double[] {0., 0.1, 1., 1., 20., 5., 0.};

    PiecewisePolynomialInterpolator interp = new CubicSplineInterpolator();
    PiecewisePolynomialResult result = interp.interpolate(xValues, yValues);

    PiecewisePolynomialFunction1D function = new PiecewisePolynomialFunction1D();

    PiecewisePolynomialInterpolator interpPos =
        new NonnegativityPreservingCubicSplineInterpolator(interp);
    PiecewisePolynomialResult resultPos = interpPos.interpolate(xValues, yValues);

    assertEquals(resultPos.getDimensions(), result.getDimensions());
    assertEquals(resultPos.getNumberOfIntervals(), result.getNumberOfIntervals());
    assertEquals(resultPos.getOrder(), result.getOrder());

    final int nPts = 101;
    for (int i = 0; i < 101; ++i) {
      final double key = 1. + 4. / (nPts - 1) * i;
      assertTrue(function.evaluate(resultPos, key).get(0) >= 0.);
    }

    final int nData = xValues.length;
    for (int i = 1; i < nData - 2; ++i) {
      final double tau = Math.signum(resultPos.getCoefMatrix().get(i, 3));
      assertTrue(
          resultPos.getCoefMatrix().get(i, 2) * tau
              >= -3. * yValues[i + 1] * tau / (xValues[i + 1] - xValues[i]));
      assertTrue(
          resultPos.getCoefMatrix().get(i, 2) * tau
              <= 3. * yValues[i + 1] * tau / (xValues[i] - xValues[i - 1]));
    }
  }
  public void flipTest() {
    final double[] xValues = new double[] {1., 2., 3., 4., 5., 6.};
    final double[] yValues = new double[] {3., 0.1, 0.01, 0.01, 0.1, 3.};

    final double[] xValuesFlip = new double[] {6., 2., 3., 5., 4., 1.};
    final double[] yValuesFlip = new double[] {3., 0.1, 0.01, 0.1, 0.01, 3.};

    PiecewisePolynomialInterpolator interp = new NaturalSplineInterpolator();

    PiecewisePolynomialFunction1D function = new PiecewisePolynomialFunction1D();

    PiecewisePolynomialInterpolator interpPos =
        new NonnegativityPreservingCubicSplineInterpolator(interp);
    PiecewisePolynomialResult resultPos = interpPos.interpolate(xValues, yValues);
    PiecewisePolynomialResult resultPosFlip = interpPos.interpolate(xValuesFlip, yValuesFlip);

    assertEquals(resultPos.getDimensions(), resultPosFlip.getDimensions());
    assertEquals(resultPos.getNumberOfIntervals(), resultPosFlip.getNumberOfIntervals());
    assertEquals(resultPos.getOrder(), resultPosFlip.getOrder());

    final int nPts = 101;
    for (int i = 0; i < 101; ++i) {
      final double key = 1. + 5. / (nPts - 1) * i;
      assertTrue(function.evaluate(resultPos, key).get(0) >= 0.);
    }

    final int nData = xValues.length;
    for (int i = 0; i < nData - 1; ++i) {
      for (int k = 0; k < 4; ++k)
        assertEquals(resultPos.getCoefMatrix().get(i, k), resultPosFlip.getCoefMatrix().get(i, k));
    }
  }
  /** PiecewiseCubicHermiteSplineInterpolator is not modified for positive data */
  public void noModificationTest() {
    final double[] xValues = new double[] {1., 2., 3., 4., 5.};
    final double[][] yValues = new double[][] {{0.1, 1., 1., 20., 5.}, {1., 2., 3., 0., 0.}};

    PiecewisePolynomialInterpolator interp = new PiecewiseCubicHermiteSplineInterpolator();
    PiecewisePolynomialResult result = interp.interpolate(xValues, yValues);

    PiecewisePolynomialInterpolator interpPos =
        new NonnegativityPreservingCubicSplineInterpolator(interp);
    PiecewisePolynomialResult resultPos = interpPos.interpolate(xValues, yValues);

    assertEquals(resultPos.getDimensions(), result.getDimensions());
    assertEquals(resultPos.getNumberOfIntervals(), result.getNumberOfIntervals());
    assertEquals(resultPos.getOrder(), result.getOrder());

    for (int i = 1; i < xValues.length - 1; ++i) {
      for (int j = 0; j < 4; ++j) {
        final double ref =
            result.getCoefMatrix().get(i, j) == 0.
                ? 1.
                : Math.abs(result.getCoefMatrix().get(i, j));
        assertEquals(
            resultPos.getCoefMatrix().get(i, j), result.getCoefMatrix().get(i, j), ref * EPS);
      }
    }
  }
  public void positivityEndIntervalsTest() {
    final double[] xValues = new double[] {1., 2., 3., 4., 5., 6.};
    final double[][] yValues =
        new double[][] {{0.01, 0.01, 0.01, 10., 20., 1.}, {0.01, 0.01, 10., 10., 0.01, 0.01}};

    PiecewisePolynomialInterpolator interp = new NaturalSplineInterpolator();
    PiecewisePolynomialResult result = interp.interpolate(xValues, yValues);

    PiecewisePolynomialFunction1D function = new PiecewisePolynomialFunction1D();

    PiecewisePolynomialInterpolator interpPos =
        new NonnegativityPreservingCubicSplineInterpolator(interp);
    PiecewisePolynomialResult resultPos = interpPos.interpolate(xValues, yValues);

    assertEquals(resultPos.getDimensions(), result.getDimensions());
    assertEquals(resultPos.getNumberOfIntervals(), result.getNumberOfIntervals());
    assertEquals(resultPos.getOrder(), result.getOrder());

    final int nPts = 101;
    for (int i = 0; i < 101; ++i) {
      final double key = 1. + 5. / (nPts - 1) * i;
      assertTrue(function.evaluate(resultPos, key).get(0) >= 0.);
    }

    int dim = yValues.length;
    int nData = xValues.length;
    for (int j = 0; j < dim; ++j) {
      for (int i = 1; i < nData - 2; ++i) {
        DoubleMatrix coefMatrix = resultPos.getCoefMatrix();
        double tau = Math.signum(coefMatrix.get(dim * i + j, 3));
        assertTrue(
            coefMatrix.get(dim * i + j, 2) * tau
                >= -3. * yValues[j][i] * tau / (xValues[i + 1] - xValues[i]));
        assertTrue(
            coefMatrix.get(dim * i + j, 2) * tau
                <= 3. * yValues[j][i] * tau / (xValues[i] - xValues[i - 1]));
      }
    }
  }
  @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);
  }