001/*
002 * Copyright (C) Cross The Road Electronics.  All rights reserved.
003 * License information can be found in CTRE_LICENSE.txt
004 * For support and suggestions contact support@ctr-electronics.com or file
005 * an issue tracker at https://github.com/CrossTheRoadElec/Phoenix-Releases
006 */
007package com.ctre.phoenix6.wpiutils;
008
009import java.util.concurrent.locks.Lock;
010import java.util.concurrent.locks.ReentrantLock;
011
012import com.ctre.phoenix6.HootReplay;
013
014import edu.wpi.first.hal.AllianceStationID;
015import edu.wpi.first.wpilibj.Notifier;
016import edu.wpi.first.wpilibj.simulation.DriverStationSim;
017
018public final class ReplayAutoEnable implements AutoCloseable {
019    private static final ReplayAutoEnable replayAutoEnable = new ReplayAutoEnable();
020
021    private final Notifier _enableNotifier;
022    private final Lock _lck = new ReentrantLock();
023    private int _startCount = 0;
024
025    public static ReplayAutoEnable getInstance() {
026        return replayAutoEnable;
027    }
028
029    private ReplayAutoEnable() {
030        _enableNotifier = new Notifier(() -> {
031            if (HootReplay.isPlaying()) {
032                var dsAttachedSig = HootReplay.getBoolean("DS:IsDSAttached");
033                if (dsAttachedSig.status.isOK()) {
034                    DriverStationSim.setDsAttached(dsAttachedSig.value);
035                }
036
037                if (DriverStationSim.getDsAttached()) {
038                    var fmsAttachedSig = HootReplay.getBoolean("DS:IsFMSAttached");
039                    if (fmsAttachedSig.status.isOK()) {
040                        DriverStationSim.setFmsAttached(fmsAttachedSig.value);
041                    }
042
043                    var enableSig = HootReplay.getBoolean("RobotEnable");
044                    if (enableSig.status.isOK()) {
045                        DriverStationSim.setEnabled(enableSig.value);
046                    }
047
048                    var robotModeSig = HootReplay.getString("RobotMode");
049                    if (robotModeSig.status.isOK()) {
050                        switch (robotModeSig.value) {
051                            case "Autonomous":
052                                DriverStationSim.setAutonomous(true);
053                                DriverStationSim.setTest(false);
054                                DriverStationSim.setEStop(false);
055                                break;
056                            case "Test":
057                                DriverStationSim.setAutonomous(false);
058                                DriverStationSim.setTest(true);
059                                DriverStationSim.setEStop(false);
060                                break;
061                            case "EStopped":
062                                DriverStationSim.setAutonomous(false);
063                                DriverStationSim.setTest(false);
064                                DriverStationSim.setEStop(true);
065                                break;
066                            default:
067                                DriverStationSim.setAutonomous(false);
068                                DriverStationSim.setTest(false);
069                                DriverStationSim.setEStop(false);
070                                break;
071                        }
072                    }
073
074                    var allianceStationSig = HootReplay.getString("AllianceStation");
075                    if (allianceStationSig.status.isOK()) {
076                        switch (allianceStationSig.value) {
077                            case "Red 1":
078                                DriverStationSim.setAllianceStationId(AllianceStationID.Red1);
079                                break;
080                            case "Red 2":
081                                DriverStationSim.setAllianceStationId(AllianceStationID.Red2);
082                                break;
083                            case "Red 3":
084                                DriverStationSim.setAllianceStationId(AllianceStationID.Red3);
085                                break;
086                            case "Blue 1":
087                                DriverStationSim.setAllianceStationId(AllianceStationID.Blue1);
088                                break;
089                            case "Blue 2":
090                                DriverStationSim.setAllianceStationId(AllianceStationID.Blue2);
091                                break;
092                            case "Blue 3":
093                                DriverStationSim.setAllianceStationId(AllianceStationID.Blue3);
094                                break;
095                            default:
096                                DriverStationSim.setAllianceStationId(AllianceStationID.Unknown);
097                                break;
098                        }
099                    }
100
101                    DriverStationSim.notifyNewData();
102                }
103            }
104        });
105    }
106
107    /**
108     * Starts automatically enabling the robot in replay.
109     */
110    public void start() {
111        _lck.lock();
112        try {
113            if (_startCount < Integer.MAX_VALUE) {
114                if (_startCount++ == 0) {
115                    /* start if we were previously at 0 */
116                    _enableNotifier.startPeriodic(0.010);
117                }
118            }
119        } finally {
120            _lck.unlock();
121        }
122    }
123
124    /**
125     * Stops automatically enabling the robot in replay. The
126     * replay enable will only be stopped when all actuators
127     * have requested to stop the replay enable.
128     */
129    public void stop() {
130        _lck.lock();
131        try {
132            if (_startCount > 0) {
133                if (--_startCount == 0) {
134                    /* stop if we are now at 0 */
135                    _enableNotifier.stop();
136                }
137            }
138        } finally {
139            _lck.unlock();
140        }
141    }
142
143    @Override
144    public void close() {
145        _enableNotifier.close();
146    }
147}