From 13ac990715a1b118dfa3f245fa177e97243eb2ca Mon Sep 17 00:00:00 2001
From: dlpond917 <dlpond@pobox.com>
Date: Sun, 18 Feb 2024 20:20:56 -0800
Subject: [PATCH] Add test for InterpolatingDoubleTreeMap class from WPI. To
 run unit tests, right-click on the test directory and choose the "Run Tests"
 option.

---
 build.gradle                         |  5 +++
 src/test/java/InterpolationTest.java | 67 ++++++++++++++++++++++++++++
 2 files changed, 72 insertions(+)
 create mode 100644 src/test/java/InterpolationTest.java

diff --git a/build.gradle b/build.gradle
index 1ac90cf..55dbaa9 100644
--- a/build.gradle
+++ b/build.gradle
@@ -161,4 +161,9 @@ idea {
         // Exclude the .vscode directory from indexing and search
         excludeDirs+=file(".vscode" )
     }
+}
+
+test {
+    useJUnitPlatform()
+    systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true'
 }
\ No newline at end of file
diff --git a/src/test/java/InterpolationTest.java b/src/test/java/InterpolationTest.java
new file mode 100644
index 0000000..b93eb7b
--- /dev/null
+++ b/src/test/java/InterpolationTest.java
@@ -0,0 +1,67 @@
+import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.*;
+
+/**
+ * Demonstrate how WPILib class InterpolatingDoubleTreeMap works
+ */
+class InterpolationTest {
+
+    static final InterpolatingDoubleTreeMap SHOOTER_ANGLE_LOW_FRONT = new InterpolatingDoubleTreeMap();
+    static {
+        // First number is the distance in feet. Second number is the shooter angle.
+        SHOOTER_ANGLE_LOW_FRONT.put(0.0, 54.0);
+        SHOOTER_ANGLE_LOW_FRONT.put(6.0, 27.0);
+        SHOOTER_ANGLE_LOW_FRONT.put(12.0, 24.0);
+        SHOOTER_ANGLE_LOW_FRONT.put(18.0, 22.0);
+        SHOOTER_ANGLE_LOW_FRONT.put(24.0, 20.0);
+    }
+
+    static final InterpolatingDoubleTreeMap SHOOTER_ANGLE_LOW_BACK = new InterpolatingDoubleTreeMap();
+    static {
+        // First number is the distance in feet. Second number is the shooter angle.
+        SHOOTER_ANGLE_LOW_BACK.put(0.0, 54.0);
+        SHOOTER_ANGLE_LOW_BACK.put(6.0, 27.0);
+        SHOOTER_ANGLE_LOW_BACK.put(12.0, 24.0);
+        SHOOTER_ANGLE_LOW_BACK.put(18.0, 22.0);
+        SHOOTER_ANGLE_LOW_BACK.put(24.0, 20.0);
+    }
+
+    @BeforeEach // this method will run before each test
+    void setup() {
+    }
+
+    @AfterEach // this method will run after each test
+    void shutdown() throws Exception {
+    }
+
+    @Test // marks this method as a test
+    void valuesThatWereMeasured() {
+        assertEquals(20.0, SHOOTER_ANGLE_LOW_FRONT.get(24.0));
+        assertEquals(54.0, SHOOTER_ANGLE_LOW_FRONT.get(0.0));
+    }
+
+    @Test // marks this method as a test
+    void valuesThatWereNotMeasured() {
+        // Test the angle halfway between two measured points
+        assertEquals(27.0 + (54.0 - 27.0)/2.0, SHOOTER_ANGLE_LOW_FRONT.get(3.0));
+        assertEquals(24.0 + (27.0 - 24.0)/2.0, SHOOTER_ANGLE_LOW_FRONT.get(9.0));
+    }
+    @Test // marks this method as a test
+    void valueGreaterThanLargestMeasurement() {
+        // The angle never goes lower than the last measurement made
+        assertEquals(20.0, SHOOTER_ANGLE_LOW_FRONT.get(90.0));
+    }
+
+    @Test
+    void valueVeryCloseToMeasuredValue() {
+        double shooterAngle = SHOOTER_ANGLE_LOW_FRONT.get(18.7);
+        System.out.print(shooterAngle);
+        assertNotEquals(22.0, shooterAngle);
+        assertTrue(shooterAngle < 22.0);
+        assertTrue(shooterAngle > 21.7);
+    }
+}
\ No newline at end of file