VisionSubsystem.java

package frc.robot.subsystems.vision;

import static edu.wpi.first.units.Units.Meters;

import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.units.measure.Distance;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.config.FieldConstants;
import frc.robot.config.RobotConstants;
import frc.robot.config.RobotConstants.Mode;
import frc.robot.robotcontainer.enums.LEDRequest;
import frc.robot.subsystems.drive.AdvantageKitDrive.Drive;
import frc.robot.subsystems.driverio.DriverIOSubsystem;
import frc.robot.subsystems.vision.VisionIO.VisionIOInputs;
import frc.robot.subsystems.vision.VisionIO.VisionIOInputs.CameraInputs;
import frc.robot.util.HubTracker;
import frc.robot.util.LimelightHelpers.PoseEstimate;
import frc.robot.util.LimelightHelpers.RawFiducial;
import java.util.Optional;
import java.util.function.Supplier;
import org.ejml.simple.SimpleMatrix;
import org.littletonrobotics.junction.Logger;

public class VisionSubsystem extends SubsystemBase {

  public final VisionIO io;
  public final Supplier<Pose2d> poseSupplier;
  public final Drive drive;
  public final VisionIOInputs inputs = new VisionIOInputs();

  public VisionSubsystem(VisionIO io, Drive drive, Supplier<Pose2d> poseSupplier) {
    this.io = io;
    this.poseSupplier = poseSupplier;
    this.drive = drive;
    inputs.shooterCamera.cameraName = VisionConstants.shooterCameraConfig.name;
    inputs.rightCamera.cameraName = VisionConstants.rightCameraConfig.name;
    inputs.leftCamera.cameraName = VisionConstants.leftCameraConfig.name;
  }

  private boolean camerasConnected() {
    return inputs.shooterCamera.connected
        && inputs.leftCamera.connected
        && inputs.rightCamera.connected;
  }

  @Override
  /** Periodic update pulling new camera inputs and applying new pose estimates */
  public void periodic() {
    io.updateInputs(inputs);
    if (RobotConstants.currentMode == Mode.SIM) {
      io.updateSim(poseSupplier.get());
    }

    // Controls LEDs when first turned on to signal if all cameras have connected
    if (DriverStation.isDisabled() && this.camerasConnected() && !VisionConstants.seenAllCameras) {
      DriverIOSubsystem.getInstance().requestLED(LEDRequest.CAMERA_ENABLED);
      VisionConstants.seenAllCameras = true;
    }

    logInputs(inputs.shooterCamera, "Vision/" + inputs.shooterCamera.cameraName);
    logInputs(inputs.rightCamera, "Vision/" + inputs.rightCamera.cameraName);
    logInputs(inputs.leftCamera, "Vision/" + inputs.leftCamera.cameraName);
    processCamera(inputs.shooterCamera, "Vision/" + inputs.shooterCamera.cameraName);
    processCamera(inputs.rightCamera, "Vision/" + inputs.rightCamera.cameraName);
    processCamera(inputs.leftCamera, "Vision/" + inputs.leftCamera.cameraName);
    SmartDashboard.putNumber("HUB_DISTANCE", this.getHubDistance().in(Meters));
    SmartDashboard.putNumber("PASS_DISTANCE", this.getPassDistance().in(Meters));
  }

  public void throttleCameras(int frames) {
    this.io.throttleCameras(frames);
  }

  // private VisionFieldPoseEstimate fuseEstimates(
  //     VisionFieldPoseEstimate a, VisionFieldPoseEstimate b) {
  //   // Ensure b is the newer measurement
  //   if (b.getTimestampSeconds() < a.getTimestampSeconds()) {
  //     VisionFieldPoseEstimate tmp = a;
  //     a = b;
  //     b = tmp;
  //   }

  //   // Preview both estimates to the same timestamp
  //   Transform2d a_T_b = state.getFieldToRobot(b.getTimestampSeconds())
  //       .get()
  //       .minus(state.getFieldToRobot(a.getTimestampSeconds()).get());

  //   Pose2d poseA = a.getVisionRobotPoseMeters().transformBy(a_T_b);
  //   Pose2d poseB = b.getVisionRobotPoseMeters();

  //   // Inverse‑variance weighting
  //   var varianceA =
  // a.getVisionMeasurementStdDevs().elementTimes(a.getVisionMeasurementStdDevs());
  //   var varianceB =
  // b.getVisionMeasurementStdDevs().elementTimes(b.getVisionMeasurementStdDevs());

  //   Rotation2d fusedHeading = poseB.getRotation();
  //   if (varianceA.get(2, 0) < VisionConstants.kLargeVariance
  //       && varianceB.get(2, 0) < VisionConstants.kLargeVariance) {
  //     fusedHeading = new Rotation2d(
  //         poseA.getRotation().getCos() / varianceA.get(2, 0)
  //             + poseB.getRotation().getCos() / varianceB.get(2, 0),
  //         poseA.getRotation().getSin() / varianceA.get(2, 0)
  //             + poseB.getRotation().getSin() / varianceB.get(2, 0));
  //   }

  //   double weightAx = 1.0 / varianceA.get(0, 0);
  //   double weightAy = 1.0 / varianceA.get(1, 0);
  //   double weightBx = 1.0 / varianceB.get(0, 0);
  //   double weightBy = 1.0 / varianceB.get(1, 0);

  //   Pose2d fusedPose = new Pose2d(
  //       new Translation2d(
  //           (poseA.getTranslation().getX() * weightAx
  //               + poseB.getTranslation().getX() * weightBx)
  //               / (weightAx + weightBx),
  //           (poseA.getTranslation().getY() * weightAy
  //               + poseB.getTranslation().getY() * weightBy)
  //               / (weightAy + weightBy)),
  //       fusedHeading);

  //   Matrix<N3, N1> fusedStdDev = VecBuilder.fill(
  //       Math.sqrt(1.0 / (weightAx + weightBx)),
  //       Math.sqrt(1.0 / (weightAy + weightBy)),
  //       Math.sqrt(1.0 / (1.0 / varianceA.get(2, 0) + 1.0 / varianceB.get(2,
  //           0))));

  //   int numTags = a.getNumTags() + b.getNumTags();
  //   double time = b.getTimestampSeconds();

  //   return new VisionFieldPoseEstimate(fusedPose, time, fusedStdDev, numTags);
  // }

  /**
   * Construct a final PoseEstimate object for a single camera if applicable. Utilizes MegaTag2
   * criteria
   *
   * @param cameraInputs Camera to process
   * @param logPrefix Camera name to log with
   * @return An Optional containing a scaled PoseEstimate or nothing if the pose is determined to be
   *     invalid
   */
  public Optional<PoseEstimate> processPoseEstimate(CameraInputs cameraInputs, String logPrefix) {
    PoseEstimate estimate = cameraInputs.poseEstimate;

    ChassisSpeeds chassisSpeeds = this.drive.getChassisSpeeds();
    double angular = chassisSpeeds.omegaRadiansPerSecond;

    if (Math.abs(angular) > Math.toRadians(60)) {
      Logger.recordOutput(logPrefix + "/rejectReason", 1);
      return Optional.empty();
    } else if (HubTracker.isAuton()
        && ((HubTracker.isBlue()
                && estimate.pose.getX()
                    > FieldConstants.fieldLength - FieldConstants.Bump.FRONT_BUMP_EDGE)
            || (!HubTracker.isBlue()
                && estimate.pose.getX() < FieldConstants.Bump.FRONT_BUMP_EDGE))) {
      Logger.recordOutput(logPrefix + "/rejectReason", 2);
      return Optional.empty();
    } else if (estimate.tagCount <= 1) {
      Logger.recordOutput(logPrefix + "/rejectReason", 3);
      return Optional.empty();
    } else if (Math.abs(estimate.pose.getZ()) > 0.5) {
      Logger.recordOutput(logPrefix + "/rejectReason", 4);
      return Optional.empty();
    } else if (estimate.tagCount == 1) {
      double avgAmbiguity = 0.0;
      for (RawFiducial raw : estimate.rawFiducials) {
        avgAmbiguity += raw.ambiguity;
      }
      avgAmbiguity /= estimate.rawFiducials.length;

      if (avgAmbiguity > 0.2) {
        Logger.recordOutput(logPrefix + "/rejectReason", 5);
        return Optional.empty();
      }
    } else if (estimate.pose.getX() < 0.0
        || estimate.pose.getX() > FieldConstants.fieldLength
        || estimate.pose.getY() < 0.0
        || estimate.pose.getY() > FieldConstants.fieldWidth) {
      Logger.recordOutput(logPrefix + "/rejectReason", 6);
      return Optional.empty();
    } else if (estimate.avgTagDist > 5.5) {
      Logger.recordOutput(logPrefix + "/rejectReason", 7);
      return Optional.empty();
    } else if (frc.robot.robotcontainer.RobotState.shooting
        && cameraInputs.cameraName != this.inputs.shooterCamera.cameraName) {
      Logger.recordOutput(logPrefix + "/rejectReason", 8);
      return Optional.empty();
    } else if (Math.abs(
            estimate
                .pose
                .getRotation()
                .toRotation2d()
                .minus(this.drive.getPose().getRotation())
                .getDegrees())
        > VisionConstants.ANGLE_THRESHOLD) {
      Logger.recordOutput(logPrefix + "/rejectReason", 9);
      return Optional.empty();
    }
    Logger.recordOutput(logPrefix + "/rejectReason", -1);

    cameraInputs.stdDev[6] *= 0.35;
    cameraInputs.stdDev[7] *= 0.35;
    // if (cameraInputs.cameraName == this.inputs.climberRightCamera.cameraName) {
    // cameraInputs.stdDev[6] *= 0.3;
    // cameraInputs.stdDev[7] *= 0.3;
    // }

    if (estimate.tagCount >= 3) {
      if (cameraInputs.cameraName == this.inputs.shooterCamera.cameraName) {
        cameraInputs.stdDev[6] *= VisionConstants.SHOOTER_TRIPLE_TAG_SCALING;
        cameraInputs.stdDev[7] *= VisionConstants.SHOOTER_TRIPLE_TAG_SCALING;
      } else {
        cameraInputs.stdDev[6] *= VisionConstants.DEFAULT_TRIPLE_TAG_SCALING;
        cameraInputs.stdDev[7] *= VisionConstants.DEFAULT_TRIPLE_TAG_SCALING;
      }
    }

    return Optional.of(estimate);
  }

  /**
   * Process a camera PoseEstimate utilizing MegaTag1 criteria
   *
   * @param cameraInputs
   * @param logPrefix
   * @return
   */
  public Optional<PoseEstimate> processPoseEstimateMtag1(
      CameraInputs cameraInputs, String logPrefix) {
    PoseEstimate estimate = cameraInputs.poseEstimateMtag1;

    ChassisSpeeds chassisSpeeds = this.drive.getChassisSpeeds();
    double angular = chassisSpeeds.omegaRadiansPerSecond;

    if (angular > Math.toRadians(90)) {
      Logger.recordOutput(logPrefix + "/rejectMtag1", 1);
      return Optional.empty();
      // } else if (forward > 2.0 || sideways > 2.0) {
      // Logger.recordOutput(logPrefix + "/rejectReason", 2);
      // return Optional.empty();
    } else if (estimate.tagCount <= 1) {
      Logger.recordOutput(logPrefix + "/rejectMtag1", 3);
      return Optional.empty();
    } else if (Math.abs(estimate.pose.getZ()) > 0.5) {
      Logger.recordOutput(logPrefix + "/rejectMtag1", 4);
      return Optional.empty();
    } else if (estimate.tagCount == 1) {
      double avgAmbiguity = 0.0;
      for (RawFiducial raw : estimate.rawFiducials) {
        avgAmbiguity += raw.ambiguity;
      }
      avgAmbiguity /= estimate.rawFiducials.length;

      if (avgAmbiguity > 0.2) {
        Logger.recordOutput(logPrefix + "/rejectMtag1", 5);
        return Optional.empty();
      }
    } else if (estimate.pose.getX() < 0.0
        || estimate.pose.getX() > FieldConstants.fieldLength
        || estimate.pose.getY() < 0.0
        || estimate.pose.getY() > FieldConstants.fieldWidth) {
      Logger.recordOutput(logPrefix + "/rejectMtag1", 6);
      return Optional.empty();
    } else if (estimate.avgTagDist > 5.5) {
      Logger.recordOutput(logPrefix + "/rejectMtag1", 7);
      return Optional.empty();
    }
    Logger.recordOutput(logPrefix + "/rejectMtag1", -1);

    cameraInputs.stdDev[5] = 0;

    return Optional.of(estimate);
  }

  public Distance getHubDistance() {

    return Meters.of(
        new Pose2d(
                FieldConstants.Hub.topCenterPoint.getX(),
                FieldConstants.Hub.topCenterPoint.getY(),
                Rotation2d.kZero)
            .minus(drive.getPose())
            .getTranslation()
            .getNorm());

    // return Meters.of(this.getHubPose2d().getTranslation().getNorm());
  }

  public Pose2d getHubPose2d() {
    return HubTracker.isBlue()
        ? new Pose2d(
            FieldConstants.Hub.topCenterPoint.getX(),
            FieldConstants.Hub.topCenterPoint.getY(),
            Rotation2d.kZero)
        : new Pose2d(
            FieldConstants.fieldLength - FieldConstants.Hub.topCenterPoint.getX(),
            FieldConstants.Hub.topCenterPoint.getY(),
            Rotation2d.kZero);
  }

  public Pose2d getHubPose2dSingleTag() {
    // red
    // front
    // Translation2d translation2dTagID10 =
    // new Translation2d(
    // inputs.shooterCamera.tagDistance.getX() - Inches.of(23.22).in(Meters),
    // inputs.shooterCamera.tagDistance.getY()); // Red: -23.22 inches to x (no y
    // offset)
    // Translation2d translation2dTagID9 =
    // new Translation2d(
    // inputs.shooterCamera.tagDistance.getX() - Inches.of(23.22).in(Meters),
    // inputs.shooterCamera.tagDistance.getY()
    // + Inches.of(14).in(Meters)); // Red: -23.22 inches to x (positive y offset)
    // // left side
    // Translation2d translation2dTagID5 =
    // new Translation2d(
    // inputs.shooterCamera.tagDistance.getX(),
    // inputs.shooterCamera.tagDistance.getY()
    // + Inches.of(23.22).in(Meters)); // Blue: +23.22 inches to x (negative y
    // offset)
    // Translation2d translation2dTagID8 =
    // new Translation2d(
    // inputs.shooterCamera.tagDistance.getX() - Inches.of(14).in(Meters),
    // inputs.shooterCamera.tagDistance.getY()
    // + Inches.of(23.22).in(Meters)); // Blue: +23.22 inches to x (negative y
    // offset)
    // // right side
    // Translation2d translation2dTagID2 =
    // new Translation2d(
    // inputs.shooterCamera.tagDistance.getX(),
    // inputs.shooterCamera.tagDistance.getY()
    // - Inches.of(23.22).in(Meters)); // Blue: +23.22 inches to x (negative y
    // offset)
    // Translation2d translation2dTagID11 =
    // new Translation2d(
    // inputs.shooterCamera.tagDistance.getX() - Inches.of(14).in(Meters),
    // inputs.shooterCamera.tagDistance.getY()
    // - Inches.of(23.22).in(Meters)); // Blue: +23.22 inches to x (negative y
    // offset)
    // // blue
    // // front
    // Translation2d translation2dTagID26 =
    // new Translation2d(
    // inputs.shooterCamera.tagDistance.getX() + Inches.of(23.22).in(Meters),
    // inputs.shooterCamera.tagDistance.getY()); // Blue: +23.22 inches to x (no y
    // offset)
    // Translation2d translation2dTagID25 =
    // new Translation2d(
    // inputs.shooterCamera.tagDistance.getX() + Inches.of(23.22).in(Meters),
    // inputs.shooterCamera.tagDistance.getY()
    // - Inches.of(14).in(Meters)); // Blue: +23.22 inches to x (negative y offset)
    // // left of hub
    // Translation2d translation2dTagID21 =
    // new Translation2d(
    // inputs.shooterCamera.tagDistance.getX(),
    // inputs.shooterCamera.tagDistance.getY()
    // - Inches.of(23.22).in(Meters)); // Blue: +23.22 inches to x (negative y
    // offset)
    // Translation2d translation2dTagID24 =
    // new Translation2d(
    // inputs.shooterCamera.tagDistance.getX() + Inches.of(14).in(Meters),
    // inputs.shooterCamera.tagDistance.getY()
    // - Inches.of(23.22).in(Meters)); // Blue: +23.22 inches to x (negative y
    // offset)
    // // right side of hub
    // Translation2d translation2dTagID18 =
    // new Translation2d(
    // inputs.shooterCamera.tagDistance.getX(),
    // inputs.shooterCamera.tagDistance.getY()
    // + Inches.of(14).in(Meters)); // Blue: +23.22 inches to x (negative y offset)
    // Translation2d translation2dTagID27 =
    // new Translation2d(
    // inputs.shooterCamera.tagDistance.getX() + Inches.of(14).in(Meters),
    // inputs.shooterCamera.tagDistance.getY()
    // + Inches.of(23.22).in(Meters)); // Blue: +23.22 inches to x (negative y
    // offset)

    // if (inputs.shooterCamera.tagID == 10) {
    // return new Pose2d(translation2dTagID10, new Rotation2d());
    // } else if (inputs.shooterCamera.tagID == 26) {
    // return new Pose2d(translation2dTagID26, new Rotation2d());
    // } else if (inputs.shooterCamera.tagID == 9) {
    // return new Pose2d(translation2dTagID9, new Rotation2d());
    // } else if (inputs.shooterCamera.tagID == 25) {
    // return new Pose2d(translation2dTagID25, new Rotation2d());
    // } else if (inputs.shooterCamera.tagID == 27) {
    // return new Pose2d(translation2dTagID27, new Rotation2d());
    // } else if (inputs.shooterCamera.tagID == 18) {
    // return new Pose2d(translation2dTagID18, new Rotation2d());
    // } else if (inputs.shooterCamera.tagID == 24) {
    // return new Pose2d(translation2dTagID24, new Rotation2d());
    // } else if (inputs.shooterCamera.tagID == 21) {
    // return new Pose2d(translation2dTagID21, new Rotation2d());
    // } else if (inputs.shooterCamera.tagID == 11) {
    // return new Pose2d(translation2dTagID11, new Rotation2d());
    // } else if (inputs.shooterCamera.tagID == 2) {
    // return new Pose2d(translation2dTagID2, new Rotation2d());
    // } else if (inputs.shooterCamera.tagID == 8) {
    // return new Pose2d(translation2dTagID8, new Rotation2d());
    // } else if (inputs.shooterCamera.tagID == 5) {
    // return new Pose2d(translation2dTagID5, new Rotation2d());
    // }
    return Pose2d.kZero;
  }

  public Distance getPassDistance() {
    return Meters.of(this.getPassPose2d().getTranslation().getNorm());
  }

  public Pose2d getPassPose2d() {
    double xOffEdge = RobotConstants.passXOffset;
    double yOffEdge = RobotConstants.passYOffset;

    if (!HubTracker.isBlue()) {
      return drive.getPose().getY() >= FieldConstants.fieldWidth / 2
          ? new Pose2d(
              FieldConstants.fieldLength - xOffEdge,
              FieldConstants.fieldWidth - yOffEdge,
              Rotation2d.k180deg)
          : new Pose2d(FieldConstants.fieldLength - xOffEdge, yOffEdge, Rotation2d.k180deg);
    }

    return drive.getPose().getY() >= FieldConstants.fieldWidth / 2
        ? new Pose2d(xOffEdge, FieldConstants.fieldWidth - yOffEdge, Rotation2d.kZero)
        : new Pose2d(xOffEdge, yOffEdge, Rotation2d.kZero);
  }

  /**
   * Update logs with detailed information and apply pose estimate TODO: Should lock drive and
   * vision data while doing all 3. Better yet fuse, but probably a next year project.
   */
  public void processCamera(CameraInputs inputs, String logPrefix) {
    Optional<PoseEstimate> poseEstimate = Optional.empty();
    Optional<PoseEstimate> poseEstimateMtag1 = Optional.empty();
    if (inputs.newPose) {
      poseEstimate = processPoseEstimate(inputs, logPrefix);
    }

    if (inputs.newPoseMtag1) {
      poseEstimateMtag1 = processPoseEstimateMtag1(inputs, logPrefix);
    }

    if (poseEstimate.isPresent()) { // should apply
      PoseEstimate estimate = poseEstimate.get();
      inputs.postLastCycle = true;
      Logger.recordOutput(logPrefix + "/poseAccepted", true);
      // Logger.recordOutput(logPrefix + "/latency", estimate.latency);
      Logger.recordOutput(logPrefix + "/tagCount", estimate.tagCount);
      // Logger.recordOutput(logPrefix + "/tagSpan", estimate.tagSpan);
      Logger.recordOutput(logPrefix + "/avgTagDist", estimate.avgTagDist);
      // Logger.recordOutput(logPrefix + "/avgTagArea", estimate.avgTagArea);
      // Logger.recordOutput(logPrefix + "/isMegaTag2", estimate.isMegaTag2);

      // add to odometry; later on might return instead and fuse them before applying
      this.drive.addVisionMeasurement(
          estimate.pose.toPose2d(),
          estimate.timestampSeconds,
          new Matrix<N3, N1>(
              new SimpleMatrix(3, 1, true, inputs.stdDev[6], inputs.stdDev[7], Integer.MAX_VALUE)));

    } else if (true) {
      Logger.recordOutput(logPrefix + "/poseAccepted", false);
      // Logger.recordOutput(logPrefix + "/latency", -1.0);
      Logger.recordOutput(logPrefix + "/tagCount", -1);
      // Logger.recordOutput(logPrefix + "/tagSpan", -1.0);
      Logger.recordOutput(logPrefix + "/avgTagDist", -1.0);
      // Logger.recordOutput(logPrefix + "/avgTagArea", -1.0);
      // Logger.recordOutput(logPrefix + "/isMegaTag2", false);
    }

    if (inputs.newPoseMtag1
        && poseEstimateMtag1.isPresent()
        && RobotConstants.isMegatag1Enabled
        && this.inputs.shooterCamera.cameraName == inputs.cameraName) {
      // RobotConstants.hasResetWithMTag1 = true;
      PoseEstimate estimate = poseEstimateMtag1.get();
      Logger.recordOutput(logPrefix + "/poseMtag1Accepted", true);
      // Logger.recordOutput(logPrefix + "/latencyMtag1", estimate.latency);
      // Logger.recordOutput(logPrefix + "/tagCountMtag1", estimate.tagCount);
      // Logger.recordOutput(logPrefix + "/tagSpanMtag1", estimate.tagSpan);
      // Logger.recordOutput(logPrefix + "/avgTagDistMtag1", estimate.avgTagDist);
      // Logger.recordOutput(logPrefix + "/avgTagAreaMtag1", estimate.avgTagArea);
      DriverIOSubsystem.getInstance().requestLED(LEDRequest.USED_MEGATAG);

      // Add gyro estimate to odometry
      this.drive.addVisionMeasurement(
          estimate.pose.toPose2d(),
          estimate.timestampSeconds,
          new Matrix<N3, N1>(
              new SimpleMatrix(
                  3, 1, true, Integer.MAX_VALUE, Integer.MAX_VALUE, inputs.stdDev[5])));
    } else if (DriverStation.isDisabled()) {
      Logger.recordOutput(logPrefix + "/poseMtag1Accepted", false);
      // Logger.recordOutput(logPrefix + "/latencyMtag1", -1.0);
      // Logger.recordOutput(logPrefix + "/tagCountMtag1", -1);
      // Logger.recordOutput(logPrefix + "/tagSpanMtag1", -1.0);
      // Logger.recordOutput(logPrefix + "/avgTagDistMtag1", -1.0);
      // Logger.recordOutput(logPrefix + "/avgTagAreaMtag1", -1.0);
    }
  }

  public void logInputs(CameraInputs inputs, String logPrefix) {
    // Logger.recordOutput(logPrefix + "/seesTarget", inputs.seesTarget);
    Logger.recordOutput(logPrefix + "/poseEstimate", inputs.poseEstimate.pose.toPose2d());
    Logger.recordOutput(logPrefix + "/stdDev/", inputs.stdDev);
    Logger.recordOutput(logPrefix + "/newPose", inputs.newPose);
    if (DriverStation.isDisabled()) {
      Logger.recordOutput(logPrefix + "/mTag1Estimate", inputs.poseEstimateMtag1.pose);
      Logger.recordOutput(logPrefix + "/newMtag1Pose", inputs.newPoseMtag1);
    }
    // Logger.recordOutput(logPrefix + "/tagID", inputs.tagID);
    // Logger.recordOutput(logPrefix + "/txValue", inputs.tx);
    // Logger.recordOutput(logPrefix + "/tagDistance", inputs.tagDistance);
  }
}