package com.team687.frc2017.utilities;

import java.util.Arrays;

/**
 * Linear interpolation (and extrapolation if needed). Intended to use for
 * determining distance from the area of contour(s) from vision.
 * 
 * @author tedlin
 *
 */

public class LinearInterpolator {

    private double[] m_key;
    private double[] m_value;

    public LinearInterpolator(double[] key, double[] value) {
	Arrays.sort(key);
	Arrays.sort(value);
	m_key = key;
	m_value = value;
    }

    public double estimate(double input) {
	double output = 0;

	// put a hard limit on the minimum value
	if (input < m_key[0]) {
	    output = m_value[0];
	}
	// extrapolation (try to avoid this)
	if (input > m_key[m_key.length - 1]) {
	    double slope = (m_value[m_value.length - 1] - m_value[0]) / (m_key[m_key.length - 1] - m_key[0]);
	    double intercept = m_value[0] - slope * m_key[0];
	    output = slope * input + intercept;
	}
	// interpolation
	for (int i = 0; i < m_value.length - 2; i++) {
	    if (input > m_key[i] && input < m_key[i + 1]) {
		output = (m_value[i] * (m_key[i + 1] - input) + m_value[i + 1] * (input - m_key[i]))
			/ (m_key[i + 1] - m_key[i]);
	    }
	}
	return output;
    }

}

@@ -530,16 +530,73 @@
 put = 0;
+%0A%09int maxLength = Math.max(m_key.length, m_value.length);
 %0A%0A%09// pu
@@ -739,30 +739,27 @@
 ut %3E m_key%5Bm
-_key.l
+axL
 ength - 1%5D) 
@@ -790,24 +790,19 @@
 _value%5Bm
-_value.l
+axL
 ength - 
@@ -828,22 +828,19 @@
 (m_key%5Bm
-_key.l
+axL
 ength - 
@@ -994,24 +994,19 @@
 0; i %3C m
-_value.l
+axL
 ength - 

handle when the number of keys != number of values