fix LinearInterpolation dataset sorting

Author: CoolSpy3

src/main/java/frc/robot/lib/LinearInterpolation.java

@@ -5,6 +5,7 @@
 import java.io.IOException;
 import java.nio.file.Paths;
 import java.util.Arrays;
+import java.util.HashMap;
 import java.util.List;
 
 import org.apache.commons.csv.CSVFormat;
@@ -19,7 +20,7 @@ public class LinearInterpolation {
     public double minX, maxX;
     public int numPoints = 0;
 
-    // Performs linear interpolation for a strictly monotonically increasing function.
+    // Performs linear interpolation (Pass a filename relative to the src/main/deploy directory File extensions are not assumed)
     public LinearInterpolation(String filename) {
         try {
             CSVParser csvParser = CSVFormat.DEFAULT.parse(new FileReader(Filesystem.getDeployDirectory().toPath().resolve(Paths.get(filename)).toFile()));
@@ -39,10 +40,9 @@ public LinearInterpolation(String filename) {
                 }
             }
             csvParser.close();
-            Arrays.sort(xs);
+            sortData();
             minX = xs[0];
             maxX = xs[xs.length - 1];
-            Arrays.sort(ys);
             for (int i = 1; i < numPoints; i++) {
                 // Linear interpolation (y = mx + b)
                 slopes[i - 1] = (ys[i] - ys[i - 1]) / (xs[i] - xs[i - 1]);
@@ -71,4 +71,15 @@ public double calculate(double x) {
             return 0.0;
         }
     }
+
+    private void sortData() {
+        HashMap<Double, Double> points = new HashMap<>();
+        for(int i = 0; i < numPoints; i++) {
+            points.put(xs[i], ys[i]);
+        }
+        Arrays.sort(xs);
+        for(int i = 0; i < xs.length; i++) {
+            ys[i] = points.get(xs[i]);
+        }
+    }
 }

src/test/java/frc/robot/lib/LinearInterpolationTest.java

@@ -8,18 +8,37 @@ public class LinearInterpolationTest {
 
     @Test
     public void testSimpleLinearInterpolation() {
-        LinearInterpolation lInterp = new LinearInterpolation("src/test/resources/LinearInterpolData1.csv");
+        LinearInterpolation lInterp = new LinearInterpolation("../../test/resources/LinearInterpolData1.csv");
         assertEquals(3, lInterp.calculate(3), 0.01);
 
-        lInterp = new LinearInterpolation("src/test/resources/LinearInterpolData2.csv");
+        lInterp = new LinearInterpolation("../../test/resources/LinearInterpolData2.csv");
         assertEquals(3, lInterp.calculate(3), 0.01);
     }
 
     @Test
     public void testLinearInterpolation() {
-        LinearInterpolation lInterp = new LinearInterpolation("src/test/resources/LinearInterpolData3.csv");
+        LinearInterpolation lInterp = new LinearInterpolation("../../test/resources/LinearInterpolData3.csv");
         assertEquals(2, lInterp.calculate(2), 0.01);
         assertEquals(3.5, lInterp.calculate(4), 0.01);
     }
 
+    @Test
+    public void testNegativeSlopeLinearInterpolation() {
+        LinearInterpolation lInterp = new LinearInterpolation("../../test/resources/LinearInterpolData4.csv");
+        assertEquals(0.75, lInterp.calculate(0.25), 0.01);
+        assertEquals(0.25, lInterp.calculate(0.75), 0.01);
+    }
+
+    @Test
+    public void testPiecewiseLinearInterpolation() {
+        LinearInterpolation lInterp = new LinearInterpolation("../../test/resources/LinearInterpolData5.csv");
+        assertEquals(1, lInterp.calculate(0), 0.01);
+        assertEquals(0.75, lInterp.calculate(0.25), 0.01);
+        assertEquals(0.25, lInterp.calculate(0.75), 0.01);
+        assertEquals(0, lInterp.calculate(1), 0.01);
+        assertEquals(0.25, lInterp.calculate(1.25), 0.01);
+        assertEquals(0.75, lInterp.calculate(1.75), 0.01);
+        assertEquals(1, lInterp.calculate(2), 0.01);
+    }
+
 }

src/test/resources/LinearInterpolData4.csv

@@ -0,0 +1,3 @@
+x,y
+0,1
+1,0

src/test/resources/LinearInterpolData5.csv

@@ -0,0 +1,4 @@
+x,y
+0,1
+1,0
+2,1