001/************************* PROJECT RON *************************/
002/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */
003/* Use of this source code is governed by an MIT-style license */
004/* that can be found in the repository LICENSE file.           */
005/***************************************************************/
006package com.stuypulse.robot.subsystems.shooter;
007
008import static edu.wpi.first.units.Units.RPM;
009
010import java.util.function.DoubleSupplier;
011
012import com.ctre.phoenix6.hardware.TalonFX;
013import com.stuypulse.robot.Robot;
014import com.stuypulse.robot.constants.Settings;
015import com.stuypulse.robot.util.shooter.InterpolationCalculator;
016import dev.doglog.DogLog;
017import edu.wpi.first.units.measure.AngularVelocity;
018import edu.wpi.first.units.measure.Voltage;
019import edu.wpi.first.wpilibj2.command.SubsystemBase;
020import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
021
022public abstract class Shooter extends SubsystemBase {
023
024    private static final Shooter instance;
025
026    private static AngularVelocity bonusVelocity;
027
028    private ShooterState state;
029
030    protected int gainSlot;
031
032    static {
033        if (Robot.isReal()) {
034            instance = new ShooterImpl();
035        } else {
036            instance = new ShooterSim();
037        }
038    }
039
040    public static Shooter getInstance() {
041        return instance;
042    }
043
044    protected Shooter() {
045        setState(ShooterState.SHOOT);
046
047        setGainSlot(0);
048
049        bonusVelocity = RPM.of(0);
050    }
051
052    public void setState(ShooterState state) {
053        this.state = state;
054    }
055
056    public ShooterState getState() {
057        return this.state;
058    }
059
060    public void setGainSlot(int slot) {
061        this.gainSlot = slot;
062    }
063
064    public void logMotor(String motorName, TalonFX motor) {
065        String stem = "Shooter/Motors/" + motorName + "/";
066        DogLog.log(stem + "MotorVoltage", motor.getMotorVoltage().getValueAsDouble());
067        DogLog.log(stem + "SupplyCurrent", motor.getSupplyCurrent().getValueAsDouble());
068        DogLog.log(stem + "StatorCurrent", motor.getStatorCurrent().getValueAsDouble());
069        DogLog.log(stem + "RPM", motor.getVelocity().getValue().in(RPM));
070    }
071
072    /** Enum representing the different possible states of the shooter. */
073    public enum ShooterState {
074
075        // SOTM(() -> 0.0), 
076        // FOTM(() -> 0.0),
077        /** Shooter doesn't run. */
078        IDLE(() -> 0.0),
079        /** Shooter wheels spin at it's target RPM, interpolated based on distance to hub. */
080        // SHOOT(Settings.Shooter.SHOOT_TUNING_RPM), //TODO:Replace with interpolated RPM after data is gathered
081        /** Shooter wheels spin at it's target RPM, interpolated based on distance to ferry zone. */
082        // FERRY(Settings.Shooter.FERRY_TUNING_RPM),
083        SHOOT(() -> InterpolationCalculator.interpolateShotInfo().targetRPM()),
084        FERRY(() -> InterpolationCalculator.interpolateFerryingInfo().targetRPM()),
085        /** Shooter wheels spin at a predetermined constant rate without interpolation. */
086        MANUAL_HUB(Settings.Shooter.MANUAL_HUB_RPM);
087
088        /** The supplier for the target RPM of the shooter in the corresponding state. */
089        private DoubleSupplier RPMSupplier;
090
091        /**
092         * Constructs a ShooterState with the given supplier for the target RPM of the shooter.
093         * @param RPMSupplier the supplier for the target RPM of the shooter in the corresponding state
094         */
095        private ShooterState(DoubleSupplier RPMSupplier) {
096            this.RPMSupplier = RPMSupplier;
097        }
098
099        /**
100         * Gets the target angular velocity of the shooter in the corresponding state by converting the target RPM from the supplier to an AngularVelocity.
101         * @return the target angular velocity of the shooter
102         */
103        public AngularVelocity getTargetAngularVelocity() {
104            return RPM.of(RPMSupplier.getAsDouble()).plus(bonusVelocity); // adding here allows target RPM readings to be accurate
105        }
106    }
107
108    public void addToBonusVelocity(double velocity) {
109        bonusVelocity = bonusVelocity.plus(RPM.of(velocity));
110    }
111
112    public void resetBonusVelocity() {
113        bonusVelocity = RPM.of(0);
114    }
115
116    public abstract AngularVelocity getCurrentAngularVelocity();
117
118    protected abstract void stopMotors();
119
120    public abstract SysIdRoutine getShooterSysIdRoutine();
121
122    public abstract void setVoltageOverride(Voltage voltage);
123
124    public boolean shooterSpunUp() {
125        return getCurrentAngularVelocity().gte(getState().getTargetAngularVelocity().minus(Settings.Shooter.SHOOTER_SPUN_UP_TOLERANCE));
126    }
127
128    @Override
129    public void periodic() {
130        final ShooterState currentState = getState();
131        DogLog.log("Shooter/Bonus Velocity", bonusVelocity);
132        DogLog.log("Shooter/Target RPM", (int) currentState.getTargetAngularVelocity().in(RPM));
133        DogLog.log("Shooter/Current RPM", (int) getCurrentAngularVelocity().in(RPM));
134        DogLog.log("Shooter/State", currentState.name());
135        DogLog.forceNt.log("States/Shooter", currentState.name());
136        DogLog.forceNt.log("Shooter/At Target RPM", shooterSpunUp());
137    }
138}