From b7ee757181056f6599f428565d2155f7175bf406 Mon Sep 17 00:00:00 2001
From: Ray Healy <raymond.m.healy@gmail.com>
Date: Tue, 10 Feb 2026 20:39:27 -0500
Subject: [PATCH] src/robolib/trajectory/ballistics: Added
 HillDescentBallisticCalculator class

Uses a hill descent algorithm
---
 .../HillDescentBallisticCalculator.java       | 56 +++++++++++++++++++
 1 file changed, 56 insertions(+)
 create mode 100644 lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/HillDescentBallisticCalculator.java

diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/HillDescentBallisticCalculator.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/HillDescentBallisticCalculator.java
new file mode 100644
index 0000000..cae4391
--- /dev/null
+++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/HillDescentBallisticCalculator.java
@@ -0,0 +1,56 @@
+package org.fairportrobotics.frc.robolib.trajectory.ballistics.calculators;
+
+import org.fairportrobotics.frc.robolib.trajectory.Velocity3d;
+import org.fairportrobotics.frc.robolib.trajectory.ballistics.constraints.BCEvalParams;
+import org.fairportrobotics.frc.robolib.trajectory.ballistics.models.BallisticModel;
+
+import edu.wpi.first.units.measure.Angle;
+
+public class HillDescentBallisticCalculator extends BallisticCalculator {
+    public HillDescentBallisticCalculator() {
+        super();
+    }
+
+
+    private <M extends BallisticModel<P>, P> BCResult evalSingleCandidate(
+        M           model,
+        P           modelParam,
+        Velocity3d  robotVelocity,
+        Angle       robotAngle,
+        Velocity3d  shooterVelocity
+    ) {
+        Velocity3d candidateVelocity    = model.getCandidateVelocity(modelParam);
+        BCEvalParams evalParams         = new BCEvalParams(robotVelocity, robotAngle, shooterVelocity, candidateVelocity);
+        return new BCResult(this.evaluateCandidate(model, evalParams), evalParams);
+    }
+
+    public <M extends BallisticModel<P>, P> BCResult computeSolution(
+        M           model,
+        P           initParam,
+        Velocity3d  robotVelocity,
+        Angle       robotAngle,
+        Velocity3d  shooterVelocity
+    ) {
+        model.paramComputedReset();
+
+        P           bestParam           = initParam;
+        BCResult    bestResult          = this.evalSingleCandidate(model, initParam, robotVelocity, robotAngle, shooterVelocity);
+        boolean     foundImprovement    = true;
+
+        while(bestResult.isValid() && foundImprovement) {
+            // Evaluate all neighbors of the current param
+            foundImprovement = false;
+            for (P param : model.getNeighborParams(bestParam)) {
+                if(model.paramComputed(param)) continue;
+
+                BCResult result = this.evalSingleCandidate(model, param, robotVelocity, robotAngle, shooterVelocity);
+                if(result.penalty < bestResult.penalty) {
+                    bestParam           = param;
+                    bestResult          = result;
+                    foundImprovement    = true;
+                }
+            }
+        }
+        return bestResult;
+    }
+}