From e89c8c10088de50fc84e72484f05c7e311e309a5 Mon Sep 17 00:00:00 2001
From: vichik <54233741+vichik123@users.noreply.github.com>
Date: Mon, 22 Apr 2024 06:40:33 +0300
Subject: [PATCH] [wpiunits] Add isNear function implementation (#6396)

This implementation uses a tolerance in the same units as the measure it checks.
---
 .../java/edu/wpi/first/units/Measure.java     | 19 +++++++++
 .../java/edu/wpi/first/units/MeasureTest.java | 40 ++++++++++++++++++-
 2 files changed, 58 insertions(+), 1 deletion(-)

diff --git a/wpiunits/src/main/java/edu/wpi/first/units/Measure.java b/wpiunits/src/main/java/edu/wpi/first/units/Measure.java
index 3d77659704..41036b0e8e 100644
--- a/wpiunits/src/main/java/edu/wpi/first/units/Measure.java
+++ b/wpiunits/src/main/java/edu/wpi/first/units/Measure.java
@@ -265,6 +265,25 @@ public interface Measure<U extends Unit<U>> extends Comparable<Measure<U>> {
     return Math.abs(this.baseUnitMagnitude() - other.baseUnitMagnitude()) <= tolerance;
   }
 
+  /**
+   * Checks if this measure is near another measure of the same unit, with a specified tolerance of
+   * the same unit.
+   *
+   * <pre>
+   *     Meters.of(1).isNear(Meters.of(1.2), Millimeters.of(300)) // true
+   *     Degrees.of(90).isNear(Rotations.of(0.5), Degrees.of(45)) // false
+   * </pre>
+   *
+   * @param other the other measure to compare against.
+   * @param tolerance the tolerance allowed in which the two measures are defined as near each
+   *     other.
+   * @return true if this unit is near the other measure, otherwise false.
+   */
+  default boolean isNear(Measure<U> other, Measure<U> tolerance) {
+    return Math.abs(this.baseUnitMagnitude() - other.baseUnitMagnitude())
+        <= Math.abs(tolerance.baseUnitMagnitude());
+  }
+
   /**
    * Checks if this measure is equivalent to another measure of the same unit.
    *
diff --git a/wpiunits/src/test/java/edu/wpi/first/units/MeasureTest.java b/wpiunits/src/test/java/edu/wpi/first/units/MeasureTest.java
index 31be4ca581..dd943c449e 100644
--- a/wpiunits/src/test/java/edu/wpi/first/units/MeasureTest.java
+++ b/wpiunits/src/test/java/edu/wpi/first/units/MeasureTest.java
@@ -303,7 +303,7 @@ class MeasureTest {
   }
 
   @Test
-  void testIsNear() {
+  void testIsNearVarianceThreshold() {
     var unit = new ExampleUnit(92);
     var measureA = unit.of(1.21);
     var measureB = unit.ofBaseUnits(64);
@@ -344,4 +344,42 @@ class MeasureTest {
     assertTrue(Units.Feet.zero().isNear(Units.Millimeters.zero(), 0.001));
     assertFalse(Units.Feet.of(2).isNear(Units.Millimeters.of(0), 0.001));
   }
+
+  @Test
+  void testIsNearMeasureTolerance() {
+    var measureCompared = Units.Meters.of(1);
+    var measureComparing = Units.Meters.of(1.2);
+
+    // Positive value with positive tolerance
+    assertTrue(measureCompared.isNear(measureComparing, Units.Millimeters.of(300)));
+    assertFalse(measureCompared.isNear(measureComparing, Units.Centimeters.of(10)));
+
+    measureCompared = measureCompared.negate();
+    measureComparing = measureComparing.negate();
+
+    // Negative value with positive tolerance
+    assertTrue(measureCompared.isNear(measureComparing, Units.Millimeters.of(300)));
+    assertFalse(measureCompared.isNear(measureComparing, Units.Centimeters.of(10)));
+
+    measureCompared = measureCompared.negate();
+    measureComparing = measureComparing.negate();
+
+    // Positive value with negative tolerance
+    assertTrue(measureCompared.isNear(measureComparing, Units.Millimeters.of(-300)));
+    assertFalse(measureCompared.isNear(measureComparing, Units.Centimeters.of(-10)));
+
+    measureCompared = measureCompared.negate();
+    measureComparing = measureComparing.negate();
+
+    // Negative value with negative tolerance.
+    assertTrue(measureCompared.isNear(measureComparing, Units.Millimeters.of(-300)));
+    assertFalse(measureCompared.isNear(measureComparing, Units.Centimeters.of(-10)));
+
+    measureCompared = measureCompared.negate();
+    measureComparing = measureComparing.negate();
+
+    // Tolerance exact difference between measures.
+    assertTrue(measureCompared.isNear(measureComparing, Units.Millimeters.of(200)));
+    assertTrue(measureCompared.isNear(measureComparing, Units.Centimeters.of(-20)));
+  }
 }