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.handoff;
007
008import com.ctre.phoenix6.controls.VoltageOut;
009import com.stuypulse.robot.constants.Field;
010import com.stuypulse.robot.constants.Motors;
011import com.stuypulse.robot.constants.Ports;
012import com.stuypulse.robot.constants.Settings;
013import com.stuypulse.robot.subsystems.shooter.Shooter;
014import com.stuypulse.robot.subsystems.shooter.Shooter.ShooterState;
015import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain;
016import com.stuypulse.robot.util.simulation.TalonFXSimulation;
017import edu.wpi.first.math.system.plant.DCMotor;
018import edu.wpi.first.math.system.plant.LinearSystemId;
019import edu.wpi.first.wpilibj.simulation.DCMotorSim;
020
021public class HandoffSim extends Handoff {
022
023    private final TalonFXSimulation handoffMotor;
024
025    private final DCMotorSim handoffSim;
026
027    private final VoltageOut handoffMotorController;
028
029    public HandoffSim() {
030        handoffSim = new DCMotorSim(
031                LinearSystemId.createDCMotorSystem(
032                        DCMotor.getKrakenX60(1),
033                        Settings.Handoff.J_KG_METERS_SQUARED,
034                        Settings.Handoff.GEAR_RATIO),
035                DCMotor.getKrakenX60(1));
036        handoffMotor = new TalonFXSimulation(Ports.Handoff.HANDOFF_MOTOR, handoffSim);
037        handoffMotor.configure(Motors.Handoff.HANDOFF_MOTOR_CONFIG);
038        handoffMotorController = new VoltageOut(getState().getTargetVoltage()).withEnableFOC(true);
039    }
040
041    @Override
042    protected void stopMotors() {
043        handoffMotor.stopMotor();
044    }
045
046    @Override
047    protected boolean handoffStalling() {
048        return false; // sim doesn't stall
049    };
050
051    @Override
052    public void periodic() {
053        if (Settings.EnabledSubsystems.HANDOFF.get()) {
054            Shooter shooter = Shooter.getInstance();
055            CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance();
056            if (!(swerve.isAlignedToTarget(Field.getHubPose()))
057                    && shooter.getState() == ShooterState.SHOOT) {
058                setState(HandoffState.IDLE);
059            }
060            if (!(swerve.isAlignedToTarget(Field.getFerryZonePose(swerve.getPose().getTranslation())))
061                    && shooter.getState() == ShooterState.FERRY) {
062                setState(HandoffState.IDLE);
063            }
064            
065            handoffMotor.setControl(handoffMotorController.withOutput(getState().getTargetVoltage()));
066        } else {
067            stopMotors();
068        }
069        handoffMotor.update(Settings.DT);
070        super.periodic();
071    }
072}