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 org.wpilib.math.geometry.Pose3d;
008import org.wpilib.math.geometry.Rotation2d;
009
010import org.littletonrobotics.junction.AutoLog;
011import org.littletonrobotics.junction.AutoLogOutput;
012
013public interface VisionIO {
014    @AutoLog
015    class VisionIOInputs {
016        public boolean connected = false;
017        public TargetObservation latestTargetObservation =
018                new TargetObservation(Rotation2d.ZERO, Rotation2d.ZERO);
019        public PoseObservation[] poseObservations = new PoseObservation[0];
020        public int[] tagIds = new int[0];
021        public int currentPipeline = 0;
022    }
023
024    class VisionIOOutputs {
025        @AutoLogOutput(key = "Vision/MegaTag Mode")
026        public MegaTagMode megaTagMode = MegaTagMode.MEGATAG_1;
027
028        @AutoLogOutput(key = "Vision/Target Pipeline Index")
029        public int pipeline = 0;
030
031        @AutoLogOutput(key = "Vision/AprilTag Whitelist")
032        public int[] aprilTagWhitelist = new int[0];
033
034        @AutoLogOutput(key = "Vision/IMU Mode")
035        public int imuMode = 1;
036    }
037
038    /** Represents the angle to a simple target, not used for pose estimation. */
039    record TargetObservation(Rotation2d tx, Rotation2d ty) {}
040
041    /** Represents a robot pose sample used for pose estimation. */
042    record PoseObservation(
043            double timestamp,
044            Pose3d pose,
045            double ambiguity,
046            int tagCount,
047            double averageTagDistance,
048            PoseObservationType type) {}
049
050    enum PoseObservationType {
051        MEGATAG_1,
052        MEGATAG_2,
053        PHOTONVISION
054    }
055
056    enum MegaTagMode {
057        MEGATAG_1,
058        MEGATAG_2
059    }
060
061    public default void updateInputs(VisionIOInputs inputs) {}
062
063    public default void applyOutputs(VisionIOOutputs outputs) {}
064}