TeleopSwerve

package frc.robot.commands.swerve;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.filter.SlewRateLimiter;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.Constants.JoystickConstants;
import frc.robot.Constants.Swerve.DriveConstants;
import frc.robot.Robot;
import frc.robot.subsystems.Infrastructure.robotPlacementState;
import frc.robot.subsystems.IntakeSubsystem.IntakeStates;
import frc.robot.subsystems.Swerve;
import frc.robot.subsystems.Swerve.DriveState;
import java.util.function.BooleanSupplier;
import java.util.function.DoubleSupplier;

public class TeleopSwerve extends CommandBase {
    private Swerve s_Swerve;
    private DoubleSupplier translationSup;
    private DoubleSupplier strafeSup;
    private DoubleSupplier rotationSup;
    private BooleanSupplier robotCentricSup;
    private final SlewRateLimiter xLimiter, yLimiter, rotationLimiter;

    public TeleopSwerve(
            Swerve s_Swerve,
            DoubleSupplier translationSup,
            DoubleSupplier strafeSup,
            DoubleSupplier rotationSup,
            BooleanSupplier robotCentricSup) {
        this.s_Swerve = s_Swerve;
        addRequirements(s_Swerve);
        this.translationSup = translationSup;
        this.strafeSup = strafeSup;
        this.rotationSup = rotationSup;
        this.robotCentricSup = robotCentricSup;
        xLimiter = new SlewRateLimiter(DriveConstants.kMaxAccelerationUnitsPerSecond);
        yLimiter = new SlewRateLimiter(DriveConstants.kMaxAccelerationUnitsPerSecond);
        rotationLimiter = new SlewRateLimiter(DriveConstants.kMaxAngularAccelerationUnitsPerSecond);
    }

    @Override
    public void execute() {
        /* Get Values, Deadband*/
        double translationVal =
                MathUtil.applyDeadband(translationSup.getAsDouble(), JoystickConstants.stickDeadband);
        double strafeVal =
                MathUtil.applyDeadband(strafeSup.getAsDouble(), JoystickConstants.stickDeadband);
        double rotationVal =
                MathUtil.applyDeadband(rotationSup.getAsDouble(), JoystickConstants.stickDeadband);

        translationVal = xLimiter.calculate(translationVal) * DriveConstants.kMaxSpeedMetersPerSecond;
        strafeVal = yLimiter.calculate(strafeVal) * DriveConstants.kMaxSpeedMetersPerSecond;
        rotationVal =
                rotationLimiter.calculate(rotationVal) * DriveConstants.kMaxAngularSpeedRadiansPerSecond;

        translationVal = Robot.currentAlliance == Alliance.Blue ? translationVal : -translationVal;
        strafeVal = Robot.currentAlliance == Alliance.Blue ? strafeVal : -strafeVal;
        /* Drive */
        if (s_Swerve.driveState == DriveState.JOYSTICK) {
            if (Robot.m_robotContainer.m_infrastructure.getRobotPlacementState()
                            == robotPlacementState.HIGH
                    || Robot.m_robotContainer.m_infrastructure.getRobotPlacementState()
                            == robotPlacementState.TOP) {
                s_Swerve.drive(
                        new Translation2d(translationVal * 0.65, strafeVal * 0.65),
                        rotationVal * 0.65,
                        !robotCentricSup.getAsBoolean(),
                        true);
            } else if (Robot.m_robotContainer.m_infrastructure.getRobotPlacementState()
                            == robotPlacementState.MID
                    || Robot.m_robotContainer.m_infrastructure.getRobotPlacementState()
                            == robotPlacementState.LOW) {
                s_Swerve.drive(
                        new Translation2d(translationVal * 0.8, strafeVal * 0.8),
                        rotationVal * 0.8,
                        !robotCentricSup.getAsBoolean(),
                        true);
            } else {
                if(Robot.m_robotContainer.m_intakeSubsystem.m_intakeState != IntakeStates.STOPPED){
                        s_Swerve.drive(
                                new Translation2d(translationVal * 0.45, strafeVal * 0.45),
                                rotationVal,
                                !robotCentricSup.getAsBoolean(),
                                true);
                }
                else{
                        s_Swerve.drive(
                                new Translation2d(translationVal, strafeVal),
                                rotationVal,
                                !robotCentricSup.getAsBoolean(),
                                true);
                }

            }
        }
    }
}