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 static org.wpilib.units.Units.Seconds;
008
009import org.wpilib.math.geometry.Rotation3d;
010import org.wpilib.math.geometry.Transform3d;
011import org.wpilib.units.measure.Time;
012
013import org.littletonrobotics.junction.networktables.LoggedNetworkBoolean;
014
015public interface VisionConstants {
016    public interface VisionSettings {
017        LoggedNetworkBoolean HDR_ENABLED =
018                new LoggedNetworkBoolean("/Tuning/Vision/HDR Enabled", false);
019
020        Time HDR_SWITCHING_INTERVAL = Seconds.of(0.5);
021
022        int RESET_IMU_INDEX = 1;
023
024        // Basic filtering thresholds
025        double MAX_AMBIGUITY = 0.3;
026        double MAX_Z_ERROR = 0.75;
027
028        // Standard deviation baselines, for 1 meter distance and 1 tag
029        // (Adjusted automatically based on distance and # of tags)
030        double LINEAR_STD_DEV_BASELINE = 0.02; // Meters
031        double ANGULAR_STD_DEV_BASELINE = 0.06; // Radians
032
033        // Multipliers to apply for MegaTag 2 observations
034        double LINEAR_STD_DEV_MEGATAG_2_FACTOR = 0.5; // More stable than full 3D solve
035        double ANGULAR_STD_DEV_MEGATAG_2_FACTOR =
036                Double.POSITIVE_INFINITY; // No rotation data available
037
038        double BUZZ_DEBOUNCE = 0.25;
039    }
040
041    record CameraData(String name, Transform3d robotToCamera, double stdDevFactor) {}
042
043    /**
044     * Represents a camera from its physical position and identified by name.
045     *
046     * <p>An constant of this enum looks like this <code>
047     * NAME("detailed name", {@link Transform3d#Transform3d(double, double, double, Rotation3d) new Transform3d(x, y, z, rotation)}, 1.0);
048     * </code>
049     */
050    public enum Camera {
051        ;
052        private final CameraData data;
053
054        /**
055         * Internal constructor to setup a camera with a {@link CameraData} instance
056         *
057         * @param name name of the camera, can be more detailed than its simple enum constant's name
058         * @param robotToCamera transformation from the robot origin to the camera's position
059         * @param stdDevFactor scalar factor to multiply the camrea's standard deviations by
060         */
061        private Camera(String name, Transform3d robotToCamera, double stdDevFactor) {
062            this.data = new CameraData(name, robotToCamera, stdDevFactor);
063        }
064
065        public String getName() {
066            return data.name();
067        }
068
069        public Transform3d getRobotToCamera() {
070            return data.robotToCamera();
071        }
072
073        public double getStdDevFactor() {
074            return data.stdDevFactor();
075        }
076
077        /**
078         * Gets the backing data object
079         *
080         * @return The corresponding {@link CameraData} instance
081         */
082        public CameraData getData() {
083            return data;
084        }
085    }
086
087    public enum Pipeline {
088        HIGH_SUN(0),
089        LOW_SUN(1);
090
091        private final int index;
092
093        private Pipeline(int index) {
094            this.index = index;
095        }
096
097        public int getIndex() {
098            return index;
099        }
100    }
101
102    public enum VisionMode {
103        LIMELIGHT,
104        PHOTONVISION
105    }
106
107    VisionMode VISION_MODE = VisionMode.LIMELIGHT;
108}