import org.photonvision.PhotonCamera;
import org.photonvision.PhotonPoseEstimator;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.photonvision.EstimatedRobotPose;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;

public class VisionSubsystem extends SubsystemBase {

    private final PhotonCamera m_camera;
    private final PhotonPoseEstimator m_poseEstimator;
    private Optional<EstimatedRobotPose> m_latestEstimate = Optional.empty();

    public VisionSubsystem() {
        // Camera name must match PhotonVision dashboard exactly
        m_camera = new PhotonCamera("FrontCamera");

        // Load the current season's field layout
        AprilTagFieldLayout layout =
            AprilTagFields.k2025ReefScapeV2.loadAprilTagLayoutField();

        // Create the estimator — one per camera
        m_poseEstimator = new PhotonPoseEstimator(
            layout,
            PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
            m_camera,
            VisionConstants.ROBOT_TO_FRONT_CAMERA   // Transform3d from VisionConstants
        );

        // Configure fallback strategy for when only 1 tag is visible
        m_poseEstimator.setMultiTagFallbackStrategy(
            PoseStrategy.LOWEST_AMBIGUITY);
    }

    @Override
    public void periodic() {
        // update() reads the latest result from the camera and runs the estimator.
        // Returns Optional.empty() if:
        //   - No targets were detected
        //   - Only 1 target was detected and ambiguity was too high
        //   - The camera is disconnected
        m_latestEstimate = m_poseEstimator.update();

        // Publish to SmartDashboard for debugging
        m_latestEstimate.ifPresent(est -> {
            SmartDashboard.putNumber("Vision/PoseX", est.estimatedPose.getX());
            SmartDashboard.putNumber("Vision/PoseY", est.estimatedPose.getY());
            SmartDashboard.putNumber("Vision/TagCount", est.targetsUsed.size());
        });
        SmartDashboard.putBoolean("Vision/HasEstimate", m_latestEstimate.isPresent());
    }

    /**
     * Returns the latest robot pose estimate.
     * Returns Optional.empty() if no valid estimate is available.
     * Consumers MUST handle the empty case — never call .get() without
     * checking .isPresent() first.
     */
    public Optional<EstimatedRobotPose> getLatestEstimate() {
        return m_latestEstimate;
    }
}

Optional<EstimatedRobotPose> estimateOpt = visionSubsystem.getLatestEstimate();

if (estimateOpt.isPresent()) {
    EstimatedRobotPose estimate = estimateOpt.get();

    // ── 1. estimatedPose — the computed robot field pose ──────────────────────
    // Pose3d of the robot on the field.
    // Call .toPose2d() to get a Pose2d for most downstream uses.
    Pose3d  pose3d = estimate.estimatedPose;
    Pose2d  pose2d = pose3d.toPose2d();

    // ── 2. timestampSeconds — when the image was CAPTURED ────────────────────
    // This is the camera capture timestamp — NOT the current time.
    // Pass this to SwerveDrivePoseEstimator.addVisionMeasurement() as the
    // timestamp argument. Lesson 6 covers how this is used for latency
    // compensation in the Kalman filter.
    double captureTime = estimate.timestampSeconds;

    // ── 3. targetsUsed — which tags contributed to this estimate ─────────────
    // List of the detections used.
    // Use .size() to distinguish single-tag vs. multi-tag estimates.
    // Multi-tag estimates (size >= 2) are more accurate and should
    // be weighted more heavily in the pose estimator (Lesson 7).
    int  numTagsUsed = estimate.targetsUsed.size();
    boolean isMultiTag = numTagsUsed >= 2;

    // Pass to the drivetrain for fusion (Lesson 6)
    m_driveSubsystem.addVisionMeasurement(pose2d, captureTime);
}

public class VisionSubsystem extends SubsystemBase {

    private final PhotonCamera m_frontCam = new PhotonCamera("FrontCamera");
    private final PhotonCamera m_backCam  = new PhotonCamera("BackCamera");

    private final PhotonPoseEstimator m_frontEstimator;
    private final PhotonPoseEstimator m_backEstimator;

    public VisionSubsystem() {
        AprilTagFieldLayout layout =
            AprilTagFields.k2025ReefScapeV2.loadAprilTagLayoutField();

        m_frontEstimator = new PhotonPoseEstimator(layout,
            PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, m_frontCam,
            VisionConstants.ROBOT_TO_FRONT_CAMERA);
        m_frontEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);

        m_backEstimator = new PhotonPoseEstimator(layout,
            PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, m_backCam,
            VisionConstants.ROBOT_TO_BACK_CAMERA);
        m_backEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
    }

    @Override
    public void periodic() {
        // Update both estimators independently every loop
        Optional<EstimatedRobotPose> frontEst = m_frontEstimator.update();
        Optional<EstimatedRobotPose> backEst  = m_backEstimator.update();

        // Each estimate is passed to the drivetrain independently.
        // The drive subsystem's addVisionMeasurement() handles fusing them.
        // Both can be present in the same loop — this is fine and expected.
        frontEst.ifPresent(est -> m_drive.addVisionMeasurement(
            est.estimatedPose.toPose2d(), est.timestampSeconds));
        backEst.ifPresent(est -> m_drive.addVisionMeasurement(
            est.estimatedPose.toPose2d(), est.timestampSeconds));
    }
}