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    static {
031        if (Robot.isReal()) {
032            instance = new ShooterImpl();
033        } else {
034            instance = new ShooterSim();
035        }
036    }
037
038    public static Shooter getInstance() {
039        return instance;
040    }
041
042    protected Shooter() {
043        setState(ShooterState.SHOOT);
044
045        bonusVelocity = RPM.of(0);
046    }
047
048    public void setState(ShooterState state) {
049        this.state = state;
050    }
051
052    public ShooterState getState() {
053        return this.state;
054    }
055
056    public void logMotor(String motorName, TalonFX motor) {
057        String stem = "Shooter/Motors/" + motorName + "/";
058        DogLog.log(stem + "MotorVoltage", motor.getMotorVoltage().getValueAsDouble());
059        DogLog.log(stem + "SupplyCurrent", motor.getSupplyCurrent().getValueAsDouble());
060        DogLog.log(stem + "StatorCurrent", motor.getStatorCurrent().getValueAsDouble());
061        DogLog.log(stem + "RPM", motor.getVelocity().getValue().in(RPM));
062    }
063
064    /** Enum representing the different possible states of the shooter. */
065    public enum ShooterState {
066
067        // SOTM(() -> 0.0), 
068        // FOTM(() -> 0.0),
069        /** Shooter doesn't run. */
070        IDLE(() -> 0.0),
071        /** Shooter wheels spin at it's target RPM, interpolated based on distance to hub. */
072        // SHOOT(Settings.Shooter.SHOOT_TUNING_RPM), //TODO:Replace with interpolated RPM after data is gathered
073        /** Shooter wheels spin at it's target RPM, interpolated based on distance to ferry zone. */
074        // FERRY(Settings.Shooter.FERRY_TUNING_RPM),
075        SHOOT(() -> InterpolationCalculator.interpolateShotInfo().targetRPM()),
076        FERRY(() -> InterpolationCalculator.interpolateFerryingInfo().targetRPM()),
077        /** Shooter wheels spin at a predetermined constant rate without interpolation. */
078        MANUAL_HUB(Settings.Shooter.MANUAL_HUB_RPM);
079
080        /** The supplier for the target RPM of the shooter in the corresponding state. */
081        private DoubleSupplier RPMSupplier;
082
083        /**
084         * Constructs a ShooterState with the given supplier for the target RPM of the shooter.
085         * @param RPMSupplier the supplier for the target RPM of the shooter in the corresponding state
086         */
087        private ShooterState(DoubleSupplier RPMSupplier) {
088            this.RPMSupplier = RPMSupplier;
089        }
090
091        /**
092         * Gets the target angular velocity of the shooter in the corresponding state by converting the target RPM from the supplier to an AngularVelocity.
093         * @return the target angular velocity of the shooter
094         */
095        public AngularVelocity getTargetAngularVelocity() {
096            return RPM.of(RPMSupplier.getAsDouble()).plus(bonusVelocity); // adding here allows target RPM readings to be accurate
097        }
098    }
099
100    public void addToBonusVelocity(double velocity) {
101        bonusVelocity = bonusVelocity.plus(RPM.of(velocity));
102    }
103
104    public void resetBonusVelocity() {
105        bonusVelocity = RPM.of(0);
106    }
107
108    public abstract AngularVelocity getCurrentAngularVelocity();
109
110    protected abstract void stopMotors();
111
112    public abstract SysIdRoutine getShooterSysIdRoutine();
113
114    public abstract void setVoltageOverride(Voltage voltage);
115
116    public boolean shooterSpunUp() {
117        return getCurrentAngularVelocity().gte(getState().getTargetAngularVelocity().minus(Settings.Shooter.SHOOTER_SPUN_UP_TOLERANCE));
118    }
119
120    @Override
121    public void periodic() {
122        final ShooterState currentState = getState();
123        DogLog.log("Shooter/Bonus Velocity", bonusVelocity);
124        DogLog.log("Shooter/Target RPM", (int) currentState.getTargetAngularVelocity().in(RPM));
125        DogLog.log("Shooter/Current RPM", (int) getCurrentAngularVelocity().in(RPM));
126        DogLog.log("Shooter/State", currentState.name());
127        DogLog.forceNt.log("States/Shooter", currentState.name());
128        DogLog.forceNt.log("Shooter/At Target RPM", shooterSpunUp());
129    }
130}