Add unit tests for history

Author: rjbell4

build.gradle

@@ -41,7 +41,6 @@ deploy {
 def includeDesktopSupport = false
 
 // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
-// Also defines JUnit 4.
 dependencies {
     implementation wpi.deps.wpilib()
     nativeZip wpi.deps.wpilibJni(wpi.platforms.roborio)
@@ -52,7 +51,8 @@ dependencies {
     nativeZip wpi.deps.vendor.jni(wpi.platforms.roborio)
     nativeDesktopZip wpi.deps.vendor.jni(wpi.platforms.desktop)
 
-    testImplementation 'junit:junit:4.12'
+    testImplementation('org.junit.jupiter:junit-jupiter-api:5.6.0')
+    testRuntime('org.junit.jupiter:junit-jupiter-engine:5.6.0')
 
     // Enable simulation gui support. Must check the box in vscode to enable support
     // upon debugging
@@ -65,4 +65,4 @@ dependencies {
 jar {
     from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } }
     manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
-}
+}

src/main/java/org/mayheminc/util/History.java

@@ -30,25 +30,20 @@ public double getAzForTime(double t) {
         int i = index - 1;
         int count = 0;
 
-        // if (t < 0) {
-        // DriverStation.reportError("Negative time in history", false);
-        // return 0.0; // no negative times.
-        // }
-
         while (i != index) {
             if (i < 0) {
                 i = HISTORY_SIZE - 1;
             }
 
             if (time[i] <= t) {
                 int prev = (i + 1) % HISTORY_SIZE;
-                if (prev != index) {
+                if (prev != index && time[i] >= 0.0) {
                     // Interpolate between the two closest entries
                     assert(time[i] < time[prev]);
                     double factor = (t - time[i]) / (time[prev] - time[i]);
                     az = azimuth[i] + factor * (azimuth[prev] - azimuth[i]);
                 } else {
-                    // Only one entry; no interpolation possible
+                    // Only one (real) entry; no interpolation possible
                     az = azimuth[i];
                 }
                 break;

src/test/java/org/mayheminc/util/HistoryTest.java

@@ -0,0 +1,43 @@
+package org.mayheminc.util;
+
+import org.junit.jupiter.api.Test;
+import static org.junit.jupiter.api.Assertions.*;
+
+public class HistoryTest {
+    @Test
+    public void runHistoryTest() {
+        History hist = new History();
+
+        // Returns 0 before any data
+        assertEquals(0.0, hist.getAzForTime(1.0));
+        
+        // Add first "real" data
+        hist.add(1.0, 101.0);
+        assertEquals(101.0, hist.getAzForTime(1.0));
+        assertEquals(101.0, hist.getAzForTime(1.5));
+        assertEquals(0.0, hist.getAzForTime(0.5));
+
+        // Add second entry and check interpolation
+        hist.add(2.0, 102.0);
+        assertEquals(102.0, hist.getAzForTime(2.0));
+        assertEquals(101.0, hist.getAzForTime(1.0));
+        assertEquals(101.5, hist.getAzForTime(1.5));
+        assertEquals(101.8, hist.getAzForTime(1.8));
+
+        // Fill history -- depends on the value of HISTORY_SIZE
+        for (int i=3; i <= 50; i++) {
+            hist.add(i, 100 + i);
+        }
+        assertEquals(150.0, hist.getAzForTime(50));
+        assertEquals(150.0, hist.getAzForTime(51));
+        assertEquals(150.0, hist.getAzForTime(100));
+        assertEquals(101.0, hist.getAzForTime(1.0));
+
+        // Overflow history
+        hist.add(51, 151);
+        assertEquals(151.0, hist.getAzForTime(51));
+        assertEquals(151.0, hist.getAzForTime(100));
+        assertEquals(102.0, hist.getAzForTime(0.1));
+        assertEquals(150.7, hist.getAzForTime(50.7));
+    }
+}