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;
006
007import com.stuypulse.robot.commands.auton.Auton;
008import com.stuypulse.robot.commands.auton.AutonFactory;
009import com.stuypulse.robot.commands.subsystems.DriveCommands;
010import com.stuypulse.robot.constants.DriverConstants;
011import com.stuypulse.robot.constants.FieldConstants;
012import com.stuypulse.robot.constants.GlobalSettings;
013import com.stuypulse.robot.subsystems.swerve.GyroIO;
014import com.stuypulse.robot.subsystems.swerve.GyroIOPigeon2;
015import com.stuypulse.robot.subsystems.swerve.ModuleIO;
016import com.stuypulse.robot.subsystems.swerve.ModuleIOSim;
017import com.stuypulse.robot.subsystems.swerve.ModuleIOTalonFX;
018import com.stuypulse.robot.subsystems.swerve.Swerve;
019import com.stuypulse.robot.subsystems.swerve.TunerConstants;
020import com.stuypulse.robot.subsystems.vision.Vision;
021import com.stuypulse.robot.subsystems.vision.VisionConstants;
022import com.stuypulse.robot.subsystems.vision.VisionConstants.Camera;
023import com.stuypulse.robot.subsystems.vision.VisionConstants.VisionMode;
024import com.stuypulse.robot.subsystems.vision.VisionIO;
025import com.stuypulse.robot.subsystems.vision.VisionIOLimelight;
026import com.stuypulse.robot.subsystems.vision.VisionIOPhoton;
027import com.stuypulse.robot.subsystems.vision.VisionIOPhotonSim;
028
029import org.wpilib.command3.button.CommandGamepad;
030import org.wpilib.math.geometry.Rotation2d;
031import org.wpilib.tunable.Tunables;
032
033import java.util.EnumMap;
034import org.littletonrobotics.junction.networktables.LoggedNetworkChooser;
035
036public class RobotContainer {
037
038    // Gamepads
039    private final CommandGamepad driver =
040            new CommandGamepad(DriverConstants.Driver.DRIVER_INDEX); // model-agnostic controller
041    // public final CommandGamepad operator = new CommandGamepad(GlobalPorts.Gamepad.OPERATOR);
042
043    // Subsystems
044    private final Swerve swerve;
045    private final Vision vision;
046    // Autons
047    private final LoggedNetworkChooser<Auton> autonChooser = new LoggedNetworkChooser<>("Auton");
048
049    // Robot container
050
051    public RobotContainer() {
052        EnumMap<Camera, VisionIO> visionIOMap = new EnumMap<>(Camera.class);
053        switch (GlobalSettings.CURRENT_MODE) {
054            case REAL -> {
055                swerve =
056                        new Swerve(
057                                new GyroIOPigeon2(),
058                                new ModuleIOTalonFX(TunerConstants.FrontLeft),
059                                new ModuleIOTalonFX(TunerConstants.FrontRight),
060                                new ModuleIOTalonFX(TunerConstants.BackLeft),
061                                new ModuleIOTalonFX(TunerConstants.BackRight));
062                for (Camera camera : VisionConstants.Camera.values()) {
063                    visionIOMap.put(
064                            camera,
065                            VisionConstants.VISION_MODE == VisionMode.LIMELIGHT
066                                    ? new VisionIOLimelight(
067                                            camera.name(),
068                                            camera.getRobotToCamera(),
069                                            swerve::getRotation)
070                                    : new VisionIOPhoton(camera.name(), camera.getRobotToCamera()));
071                }
072            }
073
074            case SIM -> {
075                swerve =
076                        new Swerve(
077                                new GyroIO() {},
078                                new ModuleIOSim(TunerConstants.FrontLeft),
079                                new ModuleIOSim(TunerConstants.FrontRight),
080                                new ModuleIOSim(TunerConstants.BackLeft),
081                                new ModuleIOSim(TunerConstants.BackRight));
082                for (Camera camera : Camera.values()) {
083                    visionIOMap.put(
084                            camera,
085                            new VisionIOPhotonSim(
086                                    camera.name(), camera.getRobotToCamera(), swerve::getPose));
087                }
088            }
089
090            default -> {
091                swerve =
092                        new Swerve(
093                                new GyroIO() {},
094                                new ModuleIO() {},
095                                new ModuleIO() {},
096                                new ModuleIO() {},
097                                new ModuleIO() {});
098                for (Camera camera : Camera.values()) {
099                    visionIOMap.put(camera, new VisionIO() {});
100                }
101            }
102        }
103        vision = new Vision(swerve.visionConsumer, visionIOMap);
104
105        configureLogging();
106        configureDefaultCommands();
107        configureButtonBindings();
108        configureAutons();
109
110        Tunables.publish("Field", FieldConstants.FIELD_2D);
111    }
112
113    /***************/
114    /*** LOGGING ***/
115    /***************/
116
117    private void configureLogging() {}
118
119    /****************/
120    /*** DEFAULTS ***/
121    /****************/
122
123    private void configureDefaultCommands() {
124        swerve.setDefaultCommand(DriveCommands.joystickDrive(swerve, driver));
125    }
126
127    /***************/
128    /*** BUTTONS ***/
129    /***************/
130
131    private void configureButtonBindings() {
132        driver.dpadUp().onTrue(DriveCommands.resetHeading(swerve, Rotation2d.ZERO));
133    }
134
135    /**************/
136    /*** AUTONS ***/
137    /**************/
138
139    private void configureAutons() {
140        AutonFactory autonFactory = new AutonFactory();
141
142        autonChooser.addDefault("Do Nothing", autonFactory.doNothingAuton());
143    }
144
145    /**
146     * Use this to pass the autonomous command to the main {@link Robot} class.
147     *
148     * @return The command to run in autonomous
149     */
150    public Auton getAutonomousCommand() {
151        return autonChooser.getSelected();
152    }
153}