001/************************ PROJECT ZIXI ************************/
002/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved.*/
003/* This work is licensed under the terms of the MIT license.  */
004/**************************************************************/
005package com.stuypulse.robot.subsystems.vision;
006
007import com.stuypulse.robot.constants.FieldConstants;
008
009import org.wpilib.math.geometry.Pose2d;
010import org.wpilib.math.geometry.Transform3d;
011
012import java.util.function.Supplier;
013import org.photonvision.simulation.PhotonCameraSim;
014import org.photonvision.simulation.SimCameraProperties;
015import org.photonvision.simulation.VisionSystemSim;
016
017/** IO implementation for physics sim using PhotonVision simulator. */
018public class VisionIOPhotonSim extends VisionIOPhoton {
019    private static VisionSystemSim visionSim;
020
021    private final Supplier<Pose2d> poseSupplier;
022    private final PhotonCameraSim cameraSim;
023
024    /**
025     * Creates a new VisionIOPhotonVisionSim.
026     *
027     * @param name The name of the camera.
028     * @param robotToCamera The 3D position of the camera relative to the robot.
029     * @param poseSupplier Supplier for the robot pose to use in simulation.
030     */
031    public VisionIOPhotonSim(
032            String name, Transform3d robotToCamera, Supplier<Pose2d> poseSupplier) {
033        super(name, robotToCamera);
034        this.poseSupplier = poseSupplier;
035
036        // Initialize vision sim
037        if (visionSim == null) {
038            visionSim = new VisionSystemSim("main");
039            visionSim.addAprilTags(FieldConstants.FIELD_LAYOUT);
040        }
041
042        // Add sim camera
043        SimCameraProperties cameraProperties = new SimCameraProperties();
044        cameraSim = new PhotonCameraSim(camera, cameraProperties, FieldConstants.FIELD_LAYOUT);
045        visionSim.addCamera(cameraSim, robotToCamera);
046    }
047
048    @Override
049    public void updateInputs(VisionIOInputs inputs) {
050        visionSim.update(poseSupplier.get());
051        super.updateInputs(inputs);
052    }
053}