From 00342a98fd0db6644dc4753e9f00ac8a1836b944 Mon Sep 17 00:00:00 2001
From: Ray Healy <raymond.m.healy@gmail.com>
Date: Sat, 28 Feb 2026 08:44:20 -0500
Subject: [PATCH] trajectory/ballistics/constraints/ HeightAtPositionBC: Added
 unit test module

Also, manually invoke the ReversableRadialBM
handler if type check passes
---
 .../constraints/HeightAtPositionBC.java       |   7 +-
 .../constraints/HeightAtPositionBCTest.java   | 130 ++++++++++++++++++
 2 files changed, 135 insertions(+), 2 deletions(-)
 create mode 100644 lib/src/test/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/constraints/HeightAtPositionBCTest.java

diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/constraints/HeightAtPositionBC.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/constraints/HeightAtPositionBC.java
index 05e49d5..3ef6d34 100644
--- a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/constraints/HeightAtPositionBC.java
+++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/constraints/HeightAtPositionBC.java
@@ -44,7 +44,7 @@ public class HeightAtPositionBC extends BallisticConstraint {
     }
 
 
-    public <M extends ReversableRadialBM<P>, P> Double evaluate(
+    public <M extends ReversableRadialBM<?>> Double evaluate_reversable(
         M model, BCEvalParams evalParams
     ) throws UnhandledEnumException{
         Time timeAtTarget;
@@ -87,7 +87,10 @@ public class HeightAtPositionBC extends BallisticConstraint {
 
     public <M extends BallisticModel<P>, P> Double evaluate(
         M model, BCEvalParams evalParams
-    ) {
+    ) throws UnhandledEnumException {
+        if(model instanceof ReversableRadialBM) {
+            return this.evaluate_reversable((ReversableRadialBM<?>) model, evalParams);
+        }
         return null;
     }
 }
diff --git a/lib/src/test/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/constraints/HeightAtPositionBCTest.java b/lib/src/test/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/constraints/HeightAtPositionBCTest.java
new file mode 100644
index 0000000..1524f91
--- /dev/null
+++ b/lib/src/test/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/constraints/HeightAtPositionBCTest.java
@@ -0,0 +1,130 @@
+package org.fairportrobotics.frc.robolib.trajectory.ballistics.constraints;
+
+import java.util.List;
+
+import org.fairportrobotics.frc.robolib.trajectory.Velocity3d;
+import org.fairportrobotics.frc.robolib.trajectory.ballistics.constraints.HeightAtPositionBC.PositionType;
+import org.fairportrobotics.frc.robolib.trajectory.ballistics.models.ConstantBM;
+
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.measure.Distance;
+
+import org.junit.jupiter.api.Test;
+
+public class HeightAtPositionBCTest extends BallisticConstraintTest<HeightAtPositionBC> {
+
+    //
+    //  Tests
+    //
+
+    private TestConstraintEvalParams<HeightAtPositionBC> mkParams(
+        String name, Distance resultHeight, PositionType positionType,
+        Double expected
+    ) {
+        return this.mkParams(
+            name, resultHeight, Units.Meters.of(0.5), positionType,
+            expected
+        );
+    }
+
+    private TestConstraintEvalParams<HeightAtPositionBC> mkParams(
+        String name, Distance resultHeight, Distance testPos,
+        PositionType positionType, Double expected
+    ) {
+        return this.mkParams(
+            name,
+            resultHeight,
+            Units.Meters.of(0.0),
+            Units.Meters.of(1.0),
+            testPos,
+            positionType,
+            expected
+        );
+    }
+
+    private TestConstraintEvalParams<HeightAtPositionBC> mkParams(
+        String name, Distance resultHeight, Distance minHeight,
+        Distance safeHeight, Distance testPos, PositionType positionType,
+        Double expected
+    ) {
+        return this.mkParams(
+            name,
+            new ConstantBM(
+                Translation3d.kZero,
+                new Translation3d(
+                    Units.Meters.of(1.0),
+                    Units.Meters.of(1.0),
+                    Units.Meters.of(1.0)
+                )
+            ).setPositionResult(
+                Units.Meters.of(0.0),
+                Units.Meters.of(0.0),
+                resultHeight
+            ),
+            minHeight,
+            safeHeight,
+            testPos,
+            positionType,
+            expected
+        );
+    }
+
+    private TestConstraintEvalParams<HeightAtPositionBC> mkParams(
+        String name, ConstantBM model, Distance minHeight, Distance safeHeight,
+        Distance testPos, PositionType positionType, Double expected
+    ) {
+        return new TestConstraintEvalParams<HeightAtPositionBC>(
+            name,
+            model,
+            new BCEvalParams(
+                Velocity3d.zero, Units.Degrees.of(0), Velocity3d.zero, Velocity3d.zero
+            ),
+            new HeightAtPositionBC(
+                minHeight,
+                safeHeight,
+                testPos,
+                positionType
+            ),
+            expected
+        );
+    }
+
+    @Test void testHeightEval() {
+        for (PositionType positionType : PositionType.values()) {
+            this.testConstraintEval(List.of(
+                this.mkParams(
+                    "Pass (Ideal, " + positionType.name() + ')',
+                    Units.Meters.of(2.0),
+                    PositionType.TargetRadius,
+                    0.0
+                ),
+                this.mkParams(
+                    "Fail (" + positionType.name() + ')',
+                    Units.Meters.of(-1.0),
+                    PositionType.TargetRadius,
+                    null
+                ),
+                this.mkParams(
+                    "Pass (Near Border, " + positionType.name() + ')',
+                    Units.Meters.of(    1.0),
+                    PositionType.TargetRadius,
+                    1.0
+                ),
+                this.mkParams(
+                    "Pass (Mid, " + positionType.name() + ')',
+                    Units.Meters.of(0.5),
+                    PositionType.TargetRadius,
+                    0.5
+                ),
+                this.mkParams(
+                    "Pass (Far Edge, " + positionType.name() + ')',
+                    Units.Meters.of(0.0),
+                    PositionType.TargetRadius,
+                    0.0
+                )
+            ));
+        }
+    }
+
+}