From b289ce0097c169196ad475bdd0ab29df4f5ad57a Mon Sep 17 00:00:00 2001
From: Bradley Bickford <bradleybickford@gmail.com>
Date: Sat, 13 Jan 2024 12:37:40 -0500
Subject: [PATCH] More changes to IAprilTagProvider and PhotonVision

---
 .../robot/interfaces/IAprilTagProvider.java   |  1 +
 .../frc/robot/utilities/PhotonVision.java     | 22 +++++++++++++++++++
 2 files changed, 23 insertions(+)

diff --git a/src/main/java/frc/robot/interfaces/IAprilTagProvider.java b/src/main/java/frc/robot/interfaces/IAprilTagProvider.java
index 3546b53..be6f10e 100644
--- a/src/main/java/frc/robot/interfaces/IAprilTagProvider.java
+++ b/src/main/java/frc/robot/interfaces/IAprilTagProvider.java
@@ -3,6 +3,7 @@ package frc.robot.interfaces;
 import java.util.OptionalDouble;
 
 public interface IAprilTagProvider {
+    public OptionalDouble getTagOffsetFromCameraCenterByID(int id, double cameraHalfXRes);
     public OptionalDouble getTagDistanceFromCameraByID(int id, double targetHeightMeters);
     public OptionalDouble getTagPitchByID(int id);
     public OptionalDouble getTagYawByID(int id);
diff --git a/src/main/java/frc/robot/utilities/PhotonVision.java b/src/main/java/frc/robot/utilities/PhotonVision.java
index 591e219..b6f9f2d 100644
--- a/src/main/java/frc/robot/utilities/PhotonVision.java
+++ b/src/main/java/frc/robot/utilities/PhotonVision.java
@@ -12,6 +12,7 @@ import org.photonvision.PhotonUtils;
 import org.photonvision.PhotonPoseEstimator.PoseStrategy;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
+import org.photonvision.targeting.TargetCorner;
 
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
@@ -61,6 +62,27 @@ public class PhotonVision implements IAprilTagProvider,IVisualPoseProvider {
         );
     }
 
+    @Override
+    public OptionalDouble getTagOffsetFromCameraCenterByID(int id, double cameraHalfXRes) {
+        PhotonPipelineResult result = camera.getLatestResult();
+        
+        if (!result.hasTargets()) {
+            return OptionalDouble.empty();
+        }
+
+        Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(result.getTargets(), id);
+
+        if (desiredTarget.isEmpty()) {
+            return OptionalDouble.empty();
+        }
+
+        List<TargetCorner> corners = desiredTarget.get().getDetectedCorners();
+
+        return OptionalDouble.of(
+            corners.get(2).x - corners.get(3).x - cameraHalfXRes
+        );
+    }
+
     @Override
     public OptionalDouble getTagDistanceFromCameraByID(int id, double targetHeightMeters) {
         PhotonPipelineResult result = camera.getLatestResult();