fixed best algae method

README.md

@@ -173,6 +173,10 @@ To undo the going back:
 
 ## CppCheck Warnings
 ```
-src\frc846\cpp\frc846\math\collection.cc:25:0: warning: The function 'VerticalDeadband' is never used. [unusedFunction]
-src\frc846\cpp\frc846\math\collection.cc:52:0: warning: The function 'CoterminalSum' is never used. [unusedFunction]
+src/y2025/cpp/commands/teleop/drive_command.cc:20:24: warning: Variable 'targeting_algae' is assigned a value that is never used. [unreadVariable]
+src/y2025/cpp/subsystems/abstract/gpd.cc:30:7: warning: The scope of the variable 'closest_algae_index' can be reduced. [variableScope]
+src/y2025/cpp/subsystems/abstract/gpd.cc:30:27: warning: Variable 'closest_algae_index' is assigned a value that is never used. [unreadVariable]
+src/y2025/cpp/subsystems/abstract/gpd.cc:40:27: warning: Variable 'closest_algae_index' is assigned a value that is never used. [unreadVariable]
+src/frc846/cpp/frc846/math/collection.cc:25:0: warning: The function 'VerticalDeadband' is never used. [unusedFunction]
+src/frc846/cpp/frc846/math/collection.cc:52:0: warning: The function 'CoterminalSum' is never used. [unusedFunction]
 ```

src/y2025/cpp/subsystems/abstract/gpd.cc

@@ -23,6 +23,28 @@ bool GPDSubsystem::VerifyHardware() { return true; }
 
 void GPDSubsystem::Setup() {}
 
+frc846::math::Vector2D GPDSubsystem::getBestGP(
+    const std::vector<frc846::math::Vector2D> algae,
+    const frc846::math::VectorND<units::feet_per_second, 2> robot_velocity) {
+  if (algae.empty()) { return {}; }
+  frc846::math::Vector2D closest_algae;
+  int closest_algae_index = -1;
+  units::degree_t min_angle = 180_deg;
+
+  for (size_t i = 0; i < algae.size(); i++) {
+    frc846::math::Vector2D relative_note = algae.at(i);
+    units::degree_t angle = robot_velocity.angleTo(relative_note, true);
+
+    if (angle < min_angle) {
+      min_angle = angle;
+      closest_algae = relative_note;
+      closest_algae_index = i;
+    }
+
+    return closest_algae;
+  }
+}
+
 GPDReadings GPDSubsystem::ReadFromHardware() {
   GPDReadings readings;
   frc846::robot::swerve::DrivetrainReadings drivetrain_readings =

src/y2025/include/subsystems/abstract/control_input.h

@@ -21,6 +21,8 @@ struct ControlInputReadings {
   bool lock_right_reef;
 
   bool zero_bearing;
+
+  bool targeting_algae;
 };
 
 struct ControlInputTarget {

src/y2025/include/subsystems/abstract/gpd.h

@@ -23,6 +23,10 @@ class GPDSubsystem
 public:
   GPDSubsystem(frc846::robot::swerve::DrivetrainSubsystem* drivetrain);
 
+  frc846::math::Vector2D getBestGP(
+      const std::vector<frc846::math::Vector2D> algae,
+      const frc846::math::VectorND<units::feet_per_second, 2> robot_velocity);
+
   void Setup() override;
 
   GPDTarget ZeroTarget() const override;