Interpolate.java

import java.lang.Double;
import java.lang.Integer;
import java.util.List;
import java.util.LinkedList;
import java.util.Arrays;
import java.math.BigDecimal;
import java.lang.System;

public class Interpolate {
    public static VetoSet[] interpVetoArray(VetoSet[] input, int power, int numPoints) {
        VetoSet[] interpInput = new VetoSet[input.length];
        for(int i = 0; i < input.length; i++)
            interpInput[i] = interpVeto(input[i], power, numPoints);
        return interpInput;
    }

    public static VetoSet interpVeto(VetoSet input, int power, int numPoints) {
        BigDecimal depth = input.getDepth();
        Pair[] inputVP = input.getVetoPairs();
        Pair[] outputVP = Interpolate.fixValues(inputVP, power, numPoints);
        VetoSet result = new VetoSet(depth, outputVP);
        return result;
    }

    public static Pair[] fixValues(Pair[] data, int power, int numPoints) {
        List<Pair> resultList = new LinkedList<Pair>();
        double maxX = ((BigDecimal) Utility.maximum(Utility.unzipFst(data))).doubleValue();
        double minX = ((BigDecimal) Utility.minimum(Utility.unzipFst(data))).doubleValue();
        double increase = (maxX - minX) / numPoints;
        BigDecimal first, second;
        for(double i = minX; i < maxX; i += increase) {
            first = new BigDecimal(i);
            second = new BigDecimal(interpolate(data, BigDecimal.valueOf(i), power));
            resultList.add(new Pair(first, second));
        }
        Pair[] result = new Pair[resultList.size()];
        resultList.toArray(result);
        return result;
    }

    // Wrapper function for interpCore
    public static Double interpolate(Pair[] data, BigDecimal x, int power) {
        double[] dataX = new double[data.length];
        double[] dataY = new double[data.length];
        for(int i = 0; i < data.length; i++) {
            dataX[i] = data[i].getFst().doubleValue();
            dataY[i] = data[i].getSnd().doubleValue();
        }
        double input = x.doubleValue();
        Double output = new Double(interpCore(dataX, dataY, input, power));
        return output;
    }

    private static double interpCore(double[] dataX, double[] dataY, double x, int power) {
        int xIndex = 0;
        while(xIndex < dataX.length - (1 + power + (dataX.length - 1) % power) && dataX[xIndex + power] < x) {
            xIndex += power;
        }

        double matrix[][] = new double[power + 1][power + 2];
        for(int i = 0; i < power + 1; ++i) {
            for(int j = 0; j < power; ++j)  {
                matrix[i][j] = Math.pow(dataX[xIndex + i], (power - j));
            }
            matrix[i][power] = 1;
            matrix[i][power + 1] = dataY[xIndex + i];
        }
        double[] coefficients = lin_solve(matrix);
        double answer = 0;
        for(int i = 0; i < coefficients.length; ++i) {
            answer += coefficients[i] * Math.pow(x, (power - i));
        }
        return answer;
    }

    private static double[] lin_solve(double[][] matrix)
    {
        double[] results = new double[matrix.length];
        int[] order = new int[matrix.length];
        for(int i = 0; i < order.length; ++i) { order[i] = i; }
        int maxIndex;
        for(int i = 0; i < matrix.length; ++i) {
            maxIndex = i;
            for(int j = i + 1; j < matrix.length; ++j) {
                if(Math.abs(matrix[maxIndex][i]) < Math.abs(matrix[j][i])) { maxIndex = j; }
            }
            if(maxIndex != i) {
                {
                    int temp = order[i];
                    order[i] = order[maxIndex];
                    order[maxIndex] = temp;
                }

                for(int j = 0; j < matrix[0].length; ++j)
                {
                    double temp = matrix[i][j];
                    matrix[i][j] = matrix[maxIndex][j];
                    matrix[maxIndex][j] = temp;
                }
            }
            if(Math.abs(matrix[i][i]) < 1e-15) {
                throw new RuntimeException("Singularity detected");
            }
            for(int j = i + 1; j < matrix.length; ++j) {
                double factor = matrix[j][i] / matrix[i][i];
                for(int k = i; k < matrix[0].length; ++k) {
                    matrix[j][k] -= matrix[i][k] * factor;
                }
            }
        }
        for (int i = matrix.length - 1; i >= 0; --i)
        {
            results[i] = matrix[i][matrix.length];
            for (int j = i + 1; j < matrix.length; ++j) { results[i] -= results[j] * matrix[i][j]; }
            results[i] /= matrix[i][i];
        }
        double[] correctResults = new double[results.length];
        for (int i = 0; i < order.length; ++i) { correctResults[order[i]] = results[i]; }
        return results;
    }
}