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.constants.GlobalSettings;
009import com.stuypulse.robot.generated.BuildConstants;
010import com.stuypulse.robot.util.PeriodicSubsystem;
011
012import org.wpilib.command3.Scheduler;
013import org.wpilib.driverstation.Alliance;
014import org.wpilib.driverstation.MatchState;
015import org.wpilib.util.Alert;
016
017import java.util.Optional;
018import org.littletonrobotics.junction.LogFileUtil;
019import org.littletonrobotics.junction.LoggedRobot;
020import org.littletonrobotics.junction.Logger;
021import org.littletonrobotics.junction.networktables.NT4Publisher;
022import org.littletonrobotics.junction.wpilog.WPILOGReader;
023import org.littletonrobotics.junction.wpilog.WPILOGWriter;
024
025/**
026 * Robot Class
027 *
028 * <p>This is the main class for robot code, instantiated in {@link com.stuypulse.robot.Main} It
029 * extends TimedRobot, meaning that the methods in this class are called automatically during
030 * specific states of the robot.
031 */
032public class Robot extends LoggedRobot {
033    private static final Alert allianceMissing =
034            new Alert(
035                    "MissingAllianceWarning",
036                    "Alliance optional has no value present",
037                    Alert.Level.MEDIUM);
038
039    /**
040     * Checks the alliance the robot is on
041     *
042     * @return true if the robot is on the blue alliance, false if the robot is on the red alliance,
043     *     and false if alliance cannot be determined.
044     */
045    public static boolean isBlue() {
046        final Optional<Alliance> alliance = MatchState.getAlliance();
047        if (alliance.isPresent()) {
048            allianceMissing.set(false);
049            return alliance.get() == Alliance.BLUE;
050        } else {
051            allianceMissing.set(true);
052        }
053        return false;
054    }
055
056    private final RobotContainer robot;
057    private final Scheduler defaultScheduler;
058    private Auton auto;
059
060    public Robot() {
061        robot = new RobotContainer();
062        defaultScheduler = Scheduler.getDefault();
063
064        logGitInfo();
065        switch (GlobalSettings.CURRENT_MODE) {
066            case REAL -> {
067                // Running on a real robot, log to a USB stick ("/U/logs")
068                Logger.addDataReceiver(new WPILOGWriter());
069                Logger.addDataReceiver(new NT4Publisher());
070            }
071
072            case SIM -> {
073                // Running a physics simulator, log to NT
074                Logger.addDataReceiver(new NT4Publisher());
075            }
076
077            case REPLAY -> {
078                // Replaying a log, set up replay source
079                setUseTiming(false); // Run as fast as possible
080                String logPath = LogFileUtil.findReplayLog();
081                Logger.setReplaySource(new WPILOGReader(logPath));
082                Logger.addDataReceiver(
083                        new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_replay")));
084            }
085        }
086
087        Logger.start();
088    }
089
090    private void logGitInfo() {
091        Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME);
092        Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE);
093        Logger.recordMetadata("GitCommitMessage", BuildConstants.GIT_MESSAGE);
094        if (!BuildConstants.GIT_LINK.isBlank()) {
095            Logger.recordMetadata("GitCommitLink", BuildConstants.GIT_LINK);
096        }
097        Logger.recordMetadata(
098                "GitPushStatus",
099                switch (BuildConstants.GIT_PUSH_STATUS) {
100                    case "PUSHED" -> "Commit pushed to remote repository";
101                    case "NOT_PUSHED" -> "Local commit not pushed to remote repository";
102                    case "STALE" -> "Could not verify push status. Cached information may be stale";
103                    default -> "Unable to determine commit push status";
104                });
105        Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA);
106        Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE);
107        Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH);
108        Logger.recordMetadata(
109                "GitDirty",
110                switch (BuildConstants.DIRTY) {
111                    case 0 -> "All changes committed";
112                    case 1 -> "Uncommitted changes";
113                    default -> "Unknown";
114                });
115    }
116
117    /*************************/
118    /*** ROBOT SCHEDULEING ***/
119    /*************************/
120
121    @Override
122    public void robotPeriodic() {
123        defaultScheduler.run();
124        PeriodicSubsystem.runAllPeriodicAfterScheduler();
125    }
126
127    /*********************/
128    /*** DISABLED MODE ***/
129    /*********************/
130
131    @Override
132    public void disabledInit() {}
133
134    @Override
135    public void disabledPeriodic() {
136        if (auto != robot.getAutonomousCommand() && auto != null) {
137            auto.clearPaths();
138
139            auto = robot.getAutonomousCommand();
140
141            auto.displayPaths();
142        }
143
144        if (auto == null) {
145            auto = robot.getAutonomousCommand();
146            if (auto != null) {
147                auto.displayPaths();
148            }
149        }
150    }
151
152    /***********************/
153    /*** AUTONOMOUS MODE ***/
154    /***********************/
155
156    @Override
157    public void autonomousInit() {
158        if (auto != null) {
159            auto.clearPaths();
160        }
161
162        auto = robot.getAutonomousCommand();
163
164        if (auto != null) {
165            defaultScheduler.schedule(auto.getCommand());
166        }
167    }
168
169    @Override
170    public void autonomousPeriodic() {}
171
172    @Override
173    public void autonomousExit() {}
174
175    /*******************/
176    /*** TELEOP MODE ***/
177    /*******************/
178
179    @Override
180    public void teleopInit() {
181        if (auto != null) {
182            auto.clearPaths();
183            defaultScheduler.cancel(auto.getCommand());
184        }
185    }
186
187    @Override
188    public void teleopPeriodic() {}
189
190    @Override
191    public void teleopExit() {}
192
193    /*************************/
194    /*** UTILITY/TEST MODE ***/
195    /*************************/
196
197    @Override
198    public void utilityInit() {
199        defaultScheduler.cancelAll();
200    }
201
202    @Override
203    public void utilityPeriodic() {}
204
205    @Override
206    public void utilityExit() {}
207}