From d8c59ccc71b89f315e281979efcf442e13b53e3a Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Thu, 3 Aug 2023 23:43:20 -0700
Subject: [PATCH] [wpimath] Add tests for MathUtil clamp() and interpolate()
 (#5501)

---
 .../java/edu/wpi/first/math/MathUtilTest.java | 26 +++++++++++++++++++
 1 file changed, 26 insertions(+)

diff --git a/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java b/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java
index f5764773e6..44f7c50a0e 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java
@@ -16,6 +16,21 @@ class MathUtilTest extends UtilityClassTest<MathUtil> {
     super(MathUtil.class);
   }
 
+  @Test
+  void testClamp() {
+    // int
+    assertEquals(5, MathUtil.clamp(10, 1, 5));
+
+    // double
+    assertEquals(5.5, MathUtil.clamp(10.5, 1.5, 5.5));
+
+    // negative int
+    assertEquals(-5, MathUtil.clamp(-10, -5, -1));
+
+    // negative double
+    assertEquals(-5.5, MathUtil.clamp(-10.5, -5.5, -1.5));
+  }
+
   @Test
   void testApplyDeadbandUnityScale() {
     // < 0
@@ -96,6 +111,17 @@ class MathUtilTest extends UtilityClassTest<MathUtil> {
     assertEquals(MathUtil.angleModulus(-Math.PI / 2), -Math.PI / 2);
   }
 
+  @Test
+  void testInterpolate() {
+    assertEquals(50, MathUtil.interpolate(0, 100, 0.5));
+    assertEquals(-50, MathUtil.interpolate(0, -100, 0.5));
+    assertEquals(0, MathUtil.interpolate(-50, 50, 0.5));
+    assertEquals(-25, MathUtil.interpolate(-50, 50, 0.25));
+    assertEquals(25, MathUtil.interpolate(-50, 50, 0.75));
+
+    assertEquals(0, MathUtil.interpolate(0, -100, -0.5));
+  }
+
   @Test
   void testIsNear() {
     // The answer is always 42