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.feeder;
007
008import static edu.wpi.first.units.Units.*;
009
010import com.stuypulse.robot.Robot;
011import com.stuypulse.robot.constants.Settings;
012import dev.doglog.DogLog;
013import edu.wpi.first.units.measure.*;
014import edu.wpi.first.wpilibj2.command.SubsystemBase;
015
016public abstract class Feeder extends SubsystemBase {
017
018    private static final Feeder instance;
019
020    private FeederState state;
021
022    static {
023        if (Robot.isReal()) {
024            instance = new FeederImpl();
025        } else {
026            instance = new FeederSim();
027        }
028    }
029
030    public static Feeder getInstance() {
031        return instance;
032    }
033
034    /** Enum representing the different possible states of the feeder. */
035    public enum FeederState {
036        /** Feeder is stopped. */
037        IDLE(Volts.of(0.0)),
038        /** Motors run forward to feed to the shooter. */
039        FORWARD(Settings.Feeder.FORWARD_VOLTAGE),
040        /** Motors run backward to work with the intake to outtake fuel from the robot. */
041        REVERSE(Settings.Feeder.REVERSE_VOLTAGE);
042
043        /** The target voltage of the feeder motors. */
044        private Voltage targetVoltage;
045
046        /**
047         * Constructs a FeederState with the given target voltage.
048         * @param targetVoltage the target voltage of the feeder motors in the corresponding state.
049         */
050        private FeederState(Voltage targetVoltage) {
051            this.targetVoltage = targetVoltage;
052        }
053
054        /** 
055         * Gets the target voltage of the feeder motors in the corresponding state.
056         * @return the target voltage of the feeder motors
057         */
058        public Voltage getTargetVoltage() {
059            return this.targetVoltage;
060        }
061    }
062
063    protected Feeder() {
064        this.state = FeederState.IDLE;
065    }
066
067    public void setState(FeederState state) {
068        this.state = state;
069    }
070
071    public FeederState getState() {
072        return state;
073    }
074
075    public abstract AngularVelocity getCurrentAngularVelocity();
076
077    protected abstract void stopMotors();
078
079    @Override
080    public void periodic() {
081        final FeederState currentState = getState();
082        // Logging
083        DogLog.log("Feeder/Target Voltage", currentState.getTargetVoltage());
084        DogLog.log("Feeder/Current RPM", getCurrentAngularVelocity().in(RPM));
085        DogLog.log("Feeder/State", currentState.name());
086        DogLog.forceNt.log("States/Feeder", currentState.name());
087    }
088}