/*
 * Copyright (C) Cross The Road Electronics.  All rights reserved.
 * License information can be found in CTRE_LICENSE.txt
 * For support and suggestions contact support@ctr-electronics.com or file
 * an issue tracker at https://github.com/CrossTheRoadElec/Phoenix-Releases
 */
package com.ctre.phoenix6.swerve;

import static edu.wpi.first.units.Units.*;

import java.util.HashMap;

import com.ctre.phoenix6.StatusCode;
import com.ctre.phoenix6.controls.CoastOut;
import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC;
import com.ctre.phoenix6.controls.PositionVoltage;
import com.ctre.phoenix6.controls.VoltageOut;
import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveControlParameters;
import com.ctre.phoenix6.swerve.jni.SwerveJNI;
import com.ctre.phoenix6.swerve.utility.PhoenixPIDController;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.units.measure.*;

/**
 * Container for all the Swerve Requests. Use this to find all applicable swerve
 * drive requests.
 * <p>
 * This is also an interface common to all swerve drive control requests that
 * allow the request to calculate the state to apply to the modules.
 */
public interface SwerveRequest {
    /**
     * In field-centric control, the direction of "forward" is sometimes different
     * depending on perspective. This addresses which forward to use.
     */
    public enum ForwardPerspectiveValue {
        /**
         * "Forward" (positive X) is determined from the operator's perspective. This
         * is important for most teleop driven field-centric requests, where positive
         * X means to drive away from the operator.
         * <p>
         * Important: Users must specify the OperatorPerspective in the SwerveDrivetrain object
         */
        OperatorPerspective(0),
        /**
         * "Forward" (positive X) is always from the perspective of the blue alliance (i.e. towards
         * the red alliance). This is important in situations such as path following where positive
         * X is always from the blue alliance perspective, regardless of where the operator is
         * physically located.
         */
        BlueAlliance(1);

        public final int value;

        ForwardPerspectiveValue(int initValue) {
            this.value = initValue;
        }

        private static HashMap<Integer, ForwardPerspectiveValue> _map = null;
        static {
            _map = new HashMap<Integer, ForwardPerspectiveValue>();
            for (ForwardPerspectiveValue type : ForwardPerspectiveValue.values()) {
                _map.put(type.value, type);
            }
        }

        /**
         * Gets ForwardPerspectiveValue from specified value
         *
         * @param value Value of ForwardPerspectiveValue
         * @return ForwardPerspectiveValue of specified value
         */
        public static ForwardPerspectiveValue valueOf(int value) {
            ForwardPerspectiveValue retval = _map.get(value);
            if (retval != null)
                return retval;
            return ForwardPerspectiveValue.values()[0];
        }
    }

    /**
     * Applies this swerve request to the given modules. This is
     * typically called by the SwerveDrivetrain odometry thread.
     * <p>
     * For native swerve requests, this API can be called from a
     * non-native request's {@code apply} to compose the two together.
     *
     * @param parameters Parameters the control request needs to calculate the module state
     * @param modulesToApply Modules to which the control request is applied
     * @return Status code of sending the request
     */
    public StatusCode apply(SwerveControlParameters parameters, SwerveModule<?, ?, ?>... modulesToApply);

    /** Swerve requests implemented in native code. */
    public interface NativeSwerveRequest extends SwerveRequest {
        /**
         * Applies a native swerve request to the native drivetrain with the provided ID.
         * <p>
         * When this is implemented, the regular {@link #apply} function should do nothing
         * (return OK). Additionally, this cannot be called from another swerve request's
         * apply method, as this overrides the native swerve request of the drivetrain.
         * <p>
         * Unlike {@link #apply}, this function is called every time {@link SwerveDrivetrain#setControl}
         * is run, not every loop of the odometry thread. Instead, the underlying native
         * request is run at the full update frequency of the odometry thread.
         *
         * @param id ID of the native swerve drivetrain
         */
        public void applyNative(int id);
    }

    
    /**
     * Does nothing to the swerve module state. This is the default state of a newly
     * created swerve drive mechanism.
     */
    public static class Idle implements NativeSwerveRequest {
        
        
        
        public StatusCode apply(SwerveControlParameters parameters, SwerveModule<?, ?, ?>... modulesToApply) {
            return StatusCode.valueOf(SwerveJNI.JNI_Request_Apply_Idle(parameters.drivetrainId));
        }
        
        public void applyNative(int id) {
            SwerveJNI.JNI_SetControl_Idle(id);
        }
    }
    
    /**
     * Sets the swerve drive module states to point inward on the robot in an "X"
     * fashion, creating a natural brake which will oppose any motion.
     */
    public static class SwerveDriveBrake implements NativeSwerveRequest {
        /**
         * The type of control request to use for the drive motor.
         */
        public SwerveModule.DriveRequestType DriveRequestType = SwerveModule.DriveRequestType.OpenLoopVoltage;
        /**
         * The type of control request to use for the drive motor.
         */
        public SwerveModule.SteerRequestType SteerRequestType = SwerveModule.SteerRequestType.Position;
        
        /**
         * Modifies the DriveRequestType parameter and returns itself.
         * <p>
         * The type of control request to use for the drive motor.
         *
         * @param newDriveRequestType Parameter to modify
         * @return this object
         */
        public SwerveDriveBrake withDriveRequestType(SwerveModule.DriveRequestType newDriveRequestType) {
            this.DriveRequestType = newDriveRequestType;
            return this;
        }
        
        /**
         * Modifies the SteerRequestType parameter and returns itself.
         * <p>
         * The type of control request to use for the drive motor.
         *
         * @param newSteerRequestType Parameter to modify
         * @return this object
         */
        public SwerveDriveBrake withSteerRequestType(SwerveModule.SteerRequestType newSteerRequestType) {
            this.SteerRequestType = newSteerRequestType;
            return this;
        }
        
        public StatusCode apply(SwerveControlParameters parameters, SwerveModule<?, ?, ?>... modulesToApply) {
            return StatusCode.valueOf(SwerveJNI.JNI_Request_Apply_SwerveDriveBrake(parameters.drivetrainId,
                DriveRequestType.value,
                SteerRequestType.value));
        }
        
        public void applyNative(int id) {
            SwerveJNI.JNI_SetControl_SwerveDriveBrake(id,
                DriveRequestType.value,
                SteerRequestType.value);
        }
    }
    
    /**
     * Drives the swerve drivetrain in a field-centric manner.
     * <p>
     * When users use this request, they specify the direction the robot should
     * travel oriented against the field, and the rate at which their robot should
     * rotate about the center of the robot.
     * <p>
     * An example scenario is that the robot is oriented to the east, the VelocityX
     * is +5 m/s, VelocityY is 0 m/s, and RotationRate is 0.5 rad/s. In this
     * scenario, the robot would drive northward at 5 m/s and turn counterclockwise
     * at 0.5 rad/s.
     */
    public static class FieldCentric implements NativeSwerveRequest {
        /**
         * The velocity in the X direction, in m/s. X is defined as forward according to
         * WPILib convention, so this determines how fast to travel forward.
         */
        public double VelocityX = 0;
        /**
         * The velocity in the Y direction, in m/s. Y is defined as to the left
         * according to WPILib convention, so this determines how fast to travel to the
         * left.
         */
        public double VelocityY = 0;
        /**
         * The angular rate to rotate at, in radians per second. Angular rate is defined
         * as counterclockwise positive, so this determines how fast to turn
         * counterclockwise.
         */
        public double RotationalRate = 0;
        /**
         * The allowable deadband of the request, in m/s.
         */
        public double Deadband = 0;
        /**
         * The rotational deadband of the request, in radians per second.
         */
        public double RotationalDeadband = 0;
        /**
         * The center of rotation the robot should rotate around. This is (0,0) by
         * default, which will rotate around the center of the robot.
         */
        public Translation2d CenterOfRotation = new Translation2d();
        /**
         * The type of control request to use for the drive motor.
         */
        public SwerveModule.DriveRequestType DriveRequestType = SwerveModule.DriveRequestType.OpenLoopVoltage;
        /**
         * The type of control request to use for the drive motor.
         */
        public SwerveModule.SteerRequestType SteerRequestType = SwerveModule.SteerRequestType.Position;
        /**
         * Whether to desaturate wheel speeds before applying. For more information, see
         * the documentation of {@link SwerveDriveKinematics#desaturateWheelSpeeds}.
         */
        public boolean DesaturateWheelSpeeds = true;
        /**
         * The perspective to use when determining which direction is forward.
         */
        public ForwardPerspectiveValue ForwardPerspective = ForwardPerspectiveValue.OperatorPerspective;
        
        /**
         * Modifies the VelocityX parameter and returns itself.
         * <p>
         * The velocity in the X direction, in m/s. X is defined as forward according to
         * WPILib convention, so this determines how fast to travel forward.
         *
         * @param newVelocityX Parameter to modify
         * @return this object
         */
        public FieldCentric withVelocityX(double newVelocityX) {
            this.VelocityX = newVelocityX;
            return this;
        }
        
        /**
         * Modifies the VelocityX parameter and returns itself.
         * <p>
         * The velocity in the X direction, in m/s. X is defined as forward according to
         * WPILib convention, so this determines how fast to travel forward.
         *
         * @param newVelocityX Parameter to modify
         * @return this object
         */
        public FieldCentric withVelocityX(LinearVelocity newVelocityX) {
            this.VelocityX = newVelocityX.in(MetersPerSecond);
            return this;
        }
        
        /**
         * Modifies the VelocityY parameter and returns itself.
         * <p>
         * The velocity in the Y direction, in m/s. Y is defined as to the left
         * according to WPILib convention, so this determines how fast to travel to the
         * left.
         *
         * @param newVelocityY Parameter to modify
         * @return this object
         */
        public FieldCentric withVelocityY(double newVelocityY) {
            this.VelocityY = newVelocityY;
            return this;
        }
        
        /**
         * Modifies the VelocityY parameter and returns itself.
         * <p>
         * The velocity in the Y direction, in m/s. Y is defined as to the left
         * according to WPILib convention, so this determines how fast to travel to the
         * left.
         *
         * @param newVelocityY Parameter to modify
         * @return this object
         */
        public FieldCentric withVelocityY(LinearVelocity newVelocityY) {
            this.VelocityY = newVelocityY.in(MetersPerSecond);
            return this;
        }
        
        /**
         * Modifies the RotationalRate parameter and returns itself.
         * <p>
         * The angular rate to rotate at, in radians per second. Angular rate is defined
         * as counterclockwise positive, so this determines how fast to turn
         * counterclockwise.
         *
         * @param newRotationalRate Parameter to modify
         * @return this object
         */
        public FieldCentric withRotationalRate(double newRotationalRate) {
            this.RotationalRate = newRotationalRate;
            return this;
        }
        
        /**
         * Modifies the RotationalRate parameter and returns itself.
         * <p>
         * The angular rate to rotate at, in radians per second. Angular rate is defined
         * as counterclockwise positive, so this determines how fast to turn
         * counterclockwise.
         *
         * @param newRotationalRate Parameter to modify
         * @return this object
         */
        public FieldCentric withRotationalRate(AngularVelocity newRotationalRate) {
            this.RotationalRate = newRotationalRate.in(RadiansPerSecond);
            return this;
        }
        
        /**
         * Modifies the Deadband parameter and returns itself.
         * <p>
         * The allowable deadband of the request, in m/s.
         *
         * @param newDeadband Parameter to modify
         * @return this object
         */
        public FieldCentric withDeadband(double newDeadband) {
            this.Deadband = newDeadband;
            return this;
        }
        
        /**
         * Modifies the Deadband parameter and returns itself.
         * <p>
         * The allowable deadband of the request, in m/s.
         *
         * @param newDeadband Parameter to modify
         * @return this object
         */
        public FieldCentric withDeadband(LinearVelocity newDeadband) {
            this.Deadband = newDeadband.in(MetersPerSecond);
            return this;
        }
        
        /**
         * Modifies the RotationalDeadband parameter and returns itself.
         * <p>
         * The rotational deadband of the request, in radians per second.
         *
         * @param newRotationalDeadband Parameter to modify
         * @return this object
         */
        public FieldCentric withRotationalDeadband(double newRotationalDeadband) {
            this.RotationalDeadband = newRotationalDeadband;
            return this;
        }
        
        /**
         * Modifies the RotationalDeadband parameter and returns itself.
         * <p>
         * The rotational deadband of the request, in radians per second.
         *
         * @param newRotationalDeadband Parameter to modify
         * @return this object
         */
        public FieldCentric withRotationalDeadband(AngularVelocity newRotationalDeadband) {
            this.RotationalDeadband = newRotationalDeadband.in(RadiansPerSecond);
            return this;
        }
        
        /**
         * Modifies the CenterOfRotation parameter and returns itself.
         * <p>
         * The center of rotation the robot should rotate around. This is (0,0) by
         * default, which will rotate around the center of the robot.
         *
         * @param newCenterOfRotation Parameter to modify
         * @return this object
         */
        public FieldCentric withCenterOfRotation(Translation2d newCenterOfRotation) {
            this.CenterOfRotation = newCenterOfRotation;
            return this;
        }
        
        /**
         * Modifies the DriveRequestType parameter and returns itself.
         * <p>
         * The type of control request to use for the drive motor.
         *
         * @param newDriveRequestType Parameter to modify
         * @return this object
         */
        public FieldCentric withDriveRequestType(SwerveModule.DriveRequestType newDriveRequestType) {
            this.DriveRequestType = newDriveRequestType;
            return this;
        }
        
        /**
         * Modifies the SteerRequestType parameter and returns itself.
         * <p>
         * The type of control request to use for the drive motor.
         *
         * @param newSteerRequestType Parameter to modify
         * @return this object
         */
        public FieldCentric withSteerRequestType(SwerveModule.SteerRequestType newSteerRequestType) {
            this.SteerRequestType = newSteerRequestType;
            return this;
        }
        
        /**
         * Modifies the DesaturateWheelSpeeds parameter and returns itself.
         * <p>
         * Whether to desaturate wheel speeds before applying. For more information, see
         * the documentation of {@link SwerveDriveKinematics#desaturateWheelSpeeds}.
         *
         * @param newDesaturateWheelSpeeds Parameter to modify
         * @return this object
         */
        public FieldCentric withDesaturateWheelSpeeds(boolean newDesaturateWheelSpeeds) {
            this.DesaturateWheelSpeeds = newDesaturateWheelSpeeds;
            return this;
        }
        
        /**
         * Modifies the ForwardPerspective parameter and returns itself.
         * <p>
         * The perspective to use when determining which direction is forward.
         *
         * @param newForwardPerspective Parameter to modify
         * @return this object
         */
        public FieldCentric withForwardPerspective(ForwardPerspectiveValue newForwardPerspective) {
            this.ForwardPerspective = newForwardPerspective;
            return this;
        }
        
        public StatusCode apply(SwerveControlParameters parameters, SwerveModule<?, ?, ?>... modulesToApply) {
            return StatusCode.valueOf(SwerveJNI.JNI_Request_Apply_FieldCentric(parameters.drivetrainId,
                VelocityX,
                VelocityY,
                RotationalRate,
                Deadband,
                RotationalDeadband,
                CenterOfRotation.getX(),
                CenterOfRotation.getY(),
                DriveRequestType.value,
                SteerRequestType.value,
                DesaturateWheelSpeeds,
                ForwardPerspective.value));
        }
        
        public void applyNative(int id) {
            SwerveJNI.JNI_SetControl_FieldCentric(id,
                VelocityX,
                VelocityY,
                RotationalRate,
                Deadband,
                RotationalDeadband,
                CenterOfRotation.getX(),
                CenterOfRotation.getY(),
                DriveRequestType.value,
                SteerRequestType.value,
                DesaturateWheelSpeeds,
                ForwardPerspective.value);
        }
    }
    
    /**
     * Drives the swerve drivetrain in a robot-centric manner.
     * <p>
     * When users use this request, they specify the direction the robot should
     * travel oriented against the robot itself, and the rate at which their robot
     * should rotate about the center of the robot.
     * <p>
     * An example scenario is that the robot is oriented to the east, the VelocityX
     * is +5 m/s, VelocityY is 0 m/s, and RotationRate is 0.5 rad/s. In this
     * scenario, the robot would drive eastward at 5 m/s and turn counterclockwise
     * at 0.5 rad/s.
     */
    public static class RobotCentric implements NativeSwerveRequest {
        /**
         * The velocity in the X direction, in m/s. X is defined as forward according to
         * WPILib convention, so this determines how fast to travel forward.
         */
        public double VelocityX = 0;
        /**
         * The velocity in the Y direction, in m/s. Y is defined as to the left
         * according to WPILib convention, so this determines how fast to travel to the
         * left.
         */
        public double VelocityY = 0;
        /**
         * The angular rate to rotate at, in radians per second. Angular rate is defined
         * as counterclockwise positive, so this determines how fast to turn
         * counterclockwise.
         */
        public double RotationalRate = 0;
        /**
         * The allowable deadband of the request, in m/s.
         */
        public double Deadband = 0;
        /**
         * The rotational deadband of the request, in radians per second.
         */
        public double RotationalDeadband = 0;
        /**
         * The center of rotation the robot should rotate around. This is (0,0) by
         * default, which will rotate around the center of the robot.
         */
        public Translation2d CenterOfRotation = new Translation2d();
        /**
         * The type of control request to use for the drive motor.
         */
        public SwerveModule.DriveRequestType DriveRequestType = SwerveModule.DriveRequestType.OpenLoopVoltage;
        /**
         * The type of control request to use for the drive motor.
         */
        public SwerveModule.SteerRequestType SteerRequestType = SwerveModule.SteerRequestType.Position;
        /**
         * Whether to desaturate wheel speeds before applying. For more information, see
         * the documentation of {@link SwerveDriveKinematics#desaturateWheelSpeeds}.
         */
        public boolean DesaturateWheelSpeeds = true;
        
        /**
         * Modifies the VelocityX parameter and returns itself.
         * <p>
         * The velocity in the X direction, in m/s. X is defined as forward according to
         * WPILib convention, so this determines how fast to travel forward.
         *
         * @param newVelocityX Parameter to modify
         * @return this object
         */
        public RobotCentric withVelocityX(double newVelocityX) {
            this.VelocityX = newVelocityX;
            return this;
        }
        
        /**
         * Modifies the VelocityX parameter and returns itself.
         * <p>
         * The velocity in the X direction, in m/s. X is defined as forward according to
         * WPILib convention, so this determines how fast to travel forward.
         *
         * @param newVelocityX Parameter to modify
         * @return this object
         */
        public RobotCentric withVelocityX(LinearVelocity newVelocityX) {
            this.VelocityX = newVelocityX.in(MetersPerSecond);
            return this;
        }
        
        /**
         * Modifies the VelocityY parameter and returns itself.
         * <p>
         * The velocity in the Y direction, in m/s. Y is defined as to the left
         * according to WPILib convention, so this determines how fast to travel to the
         * left.
         *
         * @param newVelocityY Parameter to modify
         * @return this object
         */
        public RobotCentric withVelocityY(double newVelocityY) {
            this.VelocityY = newVelocityY;
            return this;
        }
        
        /**
         * Modifies the VelocityY parameter and returns itself.
         * <p>
         * The velocity in the Y direction, in m/s. Y is defined as to the left
         * according to WPILib convention, so this determines how fast to travel to the
         * left.
         *
         * @param newVelocityY Parameter to modify
         * @return this object
         */
        public RobotCentric withVelocityY(LinearVelocity newVelocityY) {
            this.VelocityY = newVelocityY.in(MetersPerSecond);
            return this;
        }
        
        /**
         * Modifies the RotationalRate parameter and returns itself.
         * <p>
         * The angular rate to rotate at, in radians per second. Angular rate is defined
         * as counterclockwise positive, so this determines how fast to turn
         * counterclockwise.
         *
         * @param newRotationalRate Parameter to modify
         * @return this object
         */
        public RobotCentric withRotationalRate(double newRotationalRate) {
            this.RotationalRate = newRotationalRate;
            return this;
        }
        
        /**
         * Modifies the RotationalRate parameter and returns itself.
         * <p>
         * The angular rate to rotate at, in radians per second. Angular rate is defined
         * as counterclockwise positive, so this determines how fast to turn
         * counterclockwise.
         *
         * @param newRotationalRate Parameter to modify
         * @return this object
         */
        public RobotCentric withRotationalRate(AngularVelocity newRotationalRate) {
            this.RotationalRate = newRotationalRate.in(RadiansPerSecond);
            return this;
        }
        
        /**
         * Modifies the Deadband parameter and returns itself.
         * <p>
         * The allowable deadband of the request, in m/s.
         *
         * @param newDeadband Parameter to modify
         * @return this object
         */
        public RobotCentric withDeadband(double newDeadband) {
            this.Deadband = newDeadband;
            return this;
        }
        
        /**
         * Modifies the Deadband parameter and returns itself.
         * <p>
         * The allowable deadband of the request, in m/s.
         *
         * @param newDeadband Parameter to modify
         * @return this object
         */
        public RobotCentric withDeadband(LinearVelocity newDeadband) {
            this.Deadband = newDeadband.in(MetersPerSecond);
            return this;
        }
        
        /**
         * Modifies the RotationalDeadband parameter and returns itself.
         * <p>
         * The rotational deadband of the request, in radians per second.
         *
         * @param newRotationalDeadband Parameter to modify
         * @return this object
         */
        public RobotCentric withRotationalDeadband(double newRotationalDeadband) {
            this.RotationalDeadband = newRotationalDeadband;
            return this;
        }
        
        /**
         * Modifies the RotationalDeadband parameter and returns itself.
         * <p>
         * The rotational deadband of the request, in radians per second.
         *
         * @param newRotationalDeadband Parameter to modify
         * @return this object
         */
        public RobotCentric withRotationalDeadband(AngularVelocity newRotationalDeadband) {
            this.RotationalDeadband = newRotationalDeadband.in(RadiansPerSecond);
            return this;
        }
        
        /**
         * Modifies the CenterOfRotation parameter and returns itself.
         * <p>
         * The center of rotation the robot should rotate around. This is (0,0) by
         * default, which will rotate around the center of the robot.
         *
         * @param newCenterOfRotation Parameter to modify
         * @return this object
         */
        public RobotCentric withCenterOfRotation(Translation2d newCenterOfRotation) {
            this.CenterOfRotation = newCenterOfRotation;
            return this;
        }
        
        /**
         * Modifies the DriveRequestType parameter and returns itself.
         * <p>
         * The type of control request to use for the drive motor.
         *
         * @param newDriveRequestType Parameter to modify
         * @return this object
         */
        public RobotCentric withDriveRequestType(SwerveModule.DriveRequestType newDriveRequestType) {
            this.DriveRequestType = newDriveRequestType;
            return this;
        }
        
        /**
         * Modifies the SteerRequestType parameter and returns itself.
         * <p>
         * The type of control request to use for the drive motor.
         *
         * @param newSteerRequestType Parameter to modify
         * @return this object
         */
        public RobotCentric withSteerRequestType(SwerveModule.SteerRequestType newSteerRequestType) {
            this.SteerRequestType = newSteerRequestType;
            return this;
        }
        
        /**
         * Modifies the DesaturateWheelSpeeds parameter and returns itself.
         * <p>
         * Whether to desaturate wheel speeds before applying. For more information, see
         * the documentation of {@link SwerveDriveKinematics#desaturateWheelSpeeds}.
         *
         * @param newDesaturateWheelSpeeds Parameter to modify
         * @return this object
         */
        public RobotCentric withDesaturateWheelSpeeds(boolean newDesaturateWheelSpeeds) {
            this.DesaturateWheelSpeeds = newDesaturateWheelSpeeds;
            return this;
        }
        
        public StatusCode apply(SwerveControlParameters parameters, SwerveModule<?, ?, ?>... modulesToApply) {
            return StatusCode.valueOf(SwerveJNI.JNI_Request_Apply_RobotCentric(parameters.drivetrainId,
                VelocityX,
                VelocityY,
                RotationalRate,
                Deadband,
                RotationalDeadband,
                CenterOfRotation.getX(),
                CenterOfRotation.getY(),
                DriveRequestType.value,
                SteerRequestType.value,
                DesaturateWheelSpeeds));
        }
        
        public void applyNative(int id) {
            SwerveJNI.JNI_SetControl_RobotCentric(id,
                VelocityX,
                VelocityY,
                RotationalRate,
                Deadband,
                RotationalDeadband,
                CenterOfRotation.getX(),
                CenterOfRotation.getY(),
                DriveRequestType.value,
                SteerRequestType.value,
                DesaturateWheelSpeeds);
        }
    }
    
    /**
     * Sets the swerve drive modules to point to a specified direction.
     */
    public static class PointWheelsAt implements NativeSwerveRequest {
        /**
         * The direction to point the modules toward. This direction is still optimized
         * to what the module was previously at.
         */
        public Rotation2d ModuleDirection = new Rotation2d();
        /**
         * The type of control request to use for the drive motor.
         */
        public SwerveModule.DriveRequestType DriveRequestType = SwerveModule.DriveRequestType.OpenLoopVoltage;
        /**
         * The type of control request to use for the drive motor.
         */
        public SwerveModule.SteerRequestType SteerRequestType = SwerveModule.SteerRequestType.Position;
        
        /**
         * Modifies the ModuleDirection parameter and returns itself.
         * <p>
         * The direction to point the modules toward. This direction is still optimized
         * to what the module was previously at.
         *
         * @param newModuleDirection Parameter to modify
         * @return this object
         */
        public PointWheelsAt withModuleDirection(Rotation2d newModuleDirection) {
            this.ModuleDirection = newModuleDirection;
            return this;
        }
        
        /**
         * Modifies the DriveRequestType parameter and returns itself.
         * <p>
         * The type of control request to use for the drive motor.
         *
         * @param newDriveRequestType Parameter to modify
         * @return this object
         */
        public PointWheelsAt withDriveRequestType(SwerveModule.DriveRequestType newDriveRequestType) {
            this.DriveRequestType = newDriveRequestType;
            return this;
        }
        
        /**
         * Modifies the SteerRequestType parameter and returns itself.
         * <p>
         * The type of control request to use for the drive motor.
         *
         * @param newSteerRequestType Parameter to modify
         * @return this object
         */
        public PointWheelsAt withSteerRequestType(SwerveModule.SteerRequestType newSteerRequestType) {
            this.SteerRequestType = newSteerRequestType;
            return this;
        }
        
        public StatusCode apply(SwerveControlParameters parameters, SwerveModule<?, ?, ?>... modulesToApply) {
            return StatusCode.valueOf(SwerveJNI.JNI_Request_Apply_PointWheelsAt(parameters.drivetrainId,
                ModuleDirection.getRadians(),
                DriveRequestType.value,
                SteerRequestType.value));
        }
        
        public void applyNative(int id) {
            SwerveJNI.JNI_SetControl_PointWheelsAt(id,
                ModuleDirection.getRadians(),
                DriveRequestType.value,
                SteerRequestType.value);
        }
    }

    /**
     * Accepts a generic robot-centric ChassisSpeeds to apply to the drivetrain.
     */
    public static class ApplyRobotSpeeds implements NativeSwerveRequest {
        /**
         * The robot-centric chassis speeds to apply to the drivetrain.
         */
        public ChassisSpeeds Speeds = new ChassisSpeeds();
        /**
         * Robot-centric wheel force feedforwards to apply in the X direction, in
         * newtons. X is defined as forward according to WPILib convention, so this
         * determines the forward forces to apply.
         * <p>
         * These forces should include friction applied to the ground.
         * <p>
         * The order of the forces should match the order of the modules returned from
         * SwerveDrivetrain.
         */
        public double[] WheelForceFeedforwardsX = {};
        /**
         * Robot-centric wheel force feedforwards to apply in the Y direction, in
         * newtons. Y is defined as to the left according to WPILib convention, so this
         * determines the forces to apply to the left.
         * <p>
         * These forces should include friction applied to the ground.
         * <p>
         * The order of the forces should match the order of the modules returned from
         * SwerveDrivetrain.
         */
        public double[] WheelForceFeedforwardsY = {};
        /**
         * The center of rotation the robot should rotate around. This is (0,0) by
         * default, which will rotate around the center of the robot.
         */
        public Translation2d CenterOfRotation = new Translation2d();
        /**
         * The type of control request to use for the drive motor.
         */
        public SwerveModule.DriveRequestType DriveRequestType = SwerveModule.DriveRequestType.OpenLoopVoltage;
        /**
         * The type of control request to use for the drive motor.
         */
        public SwerveModule.SteerRequestType SteerRequestType = SwerveModule.SteerRequestType.Position;
        /**
         * Whether to desaturate wheel speeds before applying. For more information, see
         * the documentation of {@link SwerveDriveKinematics#desaturateWheelSpeeds}.
         */
        public boolean DesaturateWheelSpeeds = true;
    
        /**
         * Modifies the Speeds parameter and returns itself.
         * <p>
         * The robot-centric chassis speeds to apply to the drivetrain.
         *
         * @param newSpeeds Parameter to modify
         * @return this object
         */
        public ApplyRobotSpeeds withSpeeds(ChassisSpeeds newSpeeds) {
            this.Speeds = newSpeeds;
            return this;
        }
        
        /**
         * Modifies the WheelForceFeedforwardsX parameter and returns itself.
         * <p>
         * Robot-centric wheel force feedforwards to apply in the X direction, in
         * newtons. X is defined as forward according to WPILib convention, so this
         * determines the forward forces to apply.
         * <p>
         * These forces should include friction applied to the ground.
         * <p>
         * The order of the forces should match the order of the modules returned from
         * SwerveDrivetrain.
         *
         * @param newWheelForceFeedforwardsX Parameter to modify
         * @return this object
         */
        public ApplyRobotSpeeds withWheelForceFeedforwardsX(double[] newWheelForceFeedforwardsX) {
            this.WheelForceFeedforwardsX = newWheelForceFeedforwardsX;
            return this;
        }
        
        /**
         * Modifies the WheelForceFeedforwardsX parameter and returns itself.
         * <p>
         * Robot-centric wheel force feedforwards to apply in the X direction, in
         * newtons. X is defined as forward according to WPILib convention, so this
         * determines the forward forces to apply.
         * <p>
         * These forces should include friction applied to the ground.
         * <p>
         * The order of the forces should match the order of the modules returned from
         * SwerveDrivetrain.
         *
         * @param newWheelForceFeedforwardsX Parameter to modify
         * @return this object
         */
        public ApplyRobotSpeeds withWheelForceFeedforwardsX(Force[] newWheelForceFeedforwardsX) {
            if (this.WheelForceFeedforwardsX.length != newWheelForceFeedforwardsX.length) {
                this.WheelForceFeedforwardsX = new double[newWheelForceFeedforwardsX.length];
            }
            for (int i = 0; i < this.WheelForceFeedforwardsX.length; ++i) {
                this.WheelForceFeedforwardsX[i] = newWheelForceFeedforwardsX[i].in(Newtons);
            }

            return this;
        }
        
        /**
         * Modifies the WheelForceFeedforwardsY parameter and returns itself.
         * <p>
         * Robot-centric wheel force feedforwards to apply in the Y direction, in
         * newtons. Y is defined as to the left according to WPILib convention, so this
         * determines the forces to apply to the left.
         * <p>
         * These forces should include friction applied to the ground.
         * <p>
         * The order of the forces should match the order of the modules returned from
         * SwerveDrivetrain.
         *
         * @param newWheelForceFeedforwardsY Parameter to modify
         * @return this object
         */
        public ApplyRobotSpeeds withWheelForceFeedforwardsY(double[] newWheelForceFeedforwardsY) {
            this.WheelForceFeedforwardsY = newWheelForceFeedforwardsY;
            return this;
        }
        
        /**
         * Modifies the WheelForceFeedforwardsY parameter and returns itself.
         * <p>
         * Robot-centric wheel force feedforwards to apply in the Y direction, in
         * newtons. Y is defined as to the left according to WPILib convention, so this
         * determines the forces to apply to the left.
         * <p>
         * These forces should include friction applied to the ground.
         * <p>
         * The order of the forces should match the order of the modules returned from
         * SwerveDrivetrain.
         *
         * @param newWheelForceFeedforwardsY Parameter to modify
         * @return this object
         */
        public ApplyRobotSpeeds withWheelForceFeedforwardsY(Force[] newWheelForceFeedforwardsY) {
            if (this.WheelForceFeedforwardsY.length != newWheelForceFeedforwardsY.length) {
                this.WheelForceFeedforwardsY = new double[newWheelForceFeedforwardsY.length];
            }
            for (int i = 0; i < this.WheelForceFeedforwardsY.length; ++i) {
                this.WheelForceFeedforwardsY[i] = newWheelForceFeedforwardsY[i].in(Newtons);
            }

            return this;
        }
        
        /**
         * Modifies the CenterOfRotation parameter and returns itself.
         * <p>
         * The center of rotation the robot should rotate around. This is (0,0) by
         * default, which will rotate around the center of the robot.
         *
         * @param newCenterOfRotation Parameter to modify
         * @return this object
         */
        public ApplyRobotSpeeds withCenterOfRotation(Translation2d newCenterOfRotation) {
            this.CenterOfRotation = newCenterOfRotation;
            return this;
        }
        
        /**
         * Modifies the DriveRequestType parameter and returns itself.
         * <p>
         * The type of control request to use for the drive motor.
         *
         * @param newDriveRequestType Parameter to modify
         * @return this object
         */
        public ApplyRobotSpeeds withDriveRequestType(SwerveModule.DriveRequestType newDriveRequestType) {
            this.DriveRequestType = newDriveRequestType;
            return this;
        }
        
        /**
         * Modifies the SteerRequestType parameter and returns itself.
         * <p>
         * The type of control request to use for the drive motor.
         *
         * @param newSteerRequestType Parameter to modify
         * @return this object
         */
        public ApplyRobotSpeeds withSteerRequestType(SwerveModule.SteerRequestType newSteerRequestType) {
            this.SteerRequestType = newSteerRequestType;
            return this;
        }
        
        /**
         * Modifies the DesaturateWheelSpeeds parameter and returns itself.
         * <p>
         * Whether to desaturate wheel speeds before applying. For more information, see
         * the documentation of {@link SwerveDriveKinematics#desaturateWheelSpeeds}.
         *
         * @param newDesaturateWheelSpeeds Parameter to modify
         * @return this object
         */
        public ApplyRobotSpeeds withDesaturateWheelSpeeds(boolean newDesaturateWheelSpeeds) {
            this.DesaturateWheelSpeeds = newDesaturateWheelSpeeds;
            return this;
        }
        
        public StatusCode apply(SwerveControlParameters parameters, SwerveModule<?, ?, ?>... modulesToApply) {
            return StatusCode.valueOf(SwerveJNI.JNI_Request_Apply_ApplyRobotSpeeds(parameters.drivetrainId,
                Speeds.vxMetersPerSecond,
                Speeds.vyMetersPerSecond,
                Speeds.omegaRadiansPerSecond,
                WheelForceFeedforwardsX,
                WheelForceFeedforwardsY,
                CenterOfRotation.getX(),
                CenterOfRotation.getY(),
                DriveRequestType.value,
                SteerRequestType.value,
                DesaturateWheelSpeeds));
        }
        
        public void applyNative(int id) {
            SwerveJNI.JNI_SetControl_ApplyRobotSpeeds(id,
                Speeds.vxMetersPerSecond,
                Speeds.vyMetersPerSecond,
                Speeds.omegaRadiansPerSecond,
                WheelForceFeedforwardsX,
                WheelForceFeedforwardsY,
                CenterOfRotation.getX(),
                CenterOfRotation.getY(),
                DriveRequestType.value,
                SteerRequestType.value,
                DesaturateWheelSpeeds);
        }
    }

    /**
     * Accepts a generic robot-centric ChassisSpeeds to apply to the drivetrain.
     *
     * @deprecated Use {@link ApplyRobotSpeeds} instead
     */
    @Deprecated(forRemoval = true, since = "2025")
    public static class ApplyChassisSpeeds extends ApplyRobotSpeeds {}

    /**
     * Accepts a generic field-centric ChassisSpeeds to apply to the drivetrain.
     */
    public static class ApplyFieldSpeeds implements NativeSwerveRequest {
        /**
         * The field-centric chassis speeds to apply to the drivetrain.
         */
        public ChassisSpeeds Speeds = new ChassisSpeeds();
        /**
         * Field-centric wheel force feedforwards to apply in the X direction, in
         * newtons. X is defined as forward according to WPILib convention, so this
         * determines the forward forces to apply.
         * <p>
         * These forces should include friction applied to the ground.
         * <p>
         * The order of the forces should match the order of the modules returned from
         * SwerveDrivetrain.
         */
        public double[] WheelForceFeedforwardsX = {};
        /**
         * Field-centric wheel force feedforwards to apply in the Y direction, in
         * newtons. Y is defined as to the left according to WPILib convention, so this
         * determines the forces to apply to the left.
         * <p>
         * These forces should include friction applied to the ground.
         * <p>
         * The order of the forces should match the order of the modules returned from
         * SwerveDrivetrain.
         */
        public double[] WheelForceFeedforwardsY = {};
        /**
         * The center of rotation the robot should rotate around. This is (0,0) by
         * default, which will rotate around the center of the robot.
         */
        public Translation2d CenterOfRotation = new Translation2d();
        /**
         * The type of control request to use for the drive motor.
         */
        public SwerveModule.DriveRequestType DriveRequestType = SwerveModule.DriveRequestType.OpenLoopVoltage;
        /**
         * The type of control request to use for the drive motor.
         */
        public SwerveModule.SteerRequestType SteerRequestType = SwerveModule.SteerRequestType.Position;
        /**
         * Whether to desaturate wheel speeds before applying. For more information, see
         * the documentation of {@link SwerveDriveKinematics#desaturateWheelSpeeds}.
         */
        public boolean DesaturateWheelSpeeds = true;
        /**
         * The perspective to use when determining which direction is forward.
         */
        public ForwardPerspectiveValue ForwardPerspective = ForwardPerspectiveValue.BlueAlliance;
    
        /**
         * Modifies the Speeds parameter and returns itself.
         * <p>
         * The field-centric chassis speeds to apply to the drivetrain.
         *
         * @param newSpeeds Parameter to modify
         * @return this object
         */
        public ApplyFieldSpeeds withSpeeds(ChassisSpeeds newSpeeds) {
            this.Speeds = newSpeeds;
            return this;
        }
        
        /**
         * Modifies the WheelForceFeedforwardsX parameter and returns itself.
         * <p>
         * Field-centric wheel force feedforwards to apply in the X direction, in
         * newtons. X is defined as forward according to WPILib convention, so this
         * determines the forward forces to apply.
         * <p>
         * These forces should include friction applied to the ground.
         * <p>
         * The order of the forces should match the order of the modules returned from
         * SwerveDrivetrain.
         *
         * @param newWheelForceFeedforwardsX Parameter to modify
         * @return this object
         */
        public ApplyFieldSpeeds withWheelForceFeedforwardsX(double[] newWheelForceFeedforwardsX) {
            this.WheelForceFeedforwardsX = newWheelForceFeedforwardsX;
            return this;
        }
        
        /**
         * Modifies the WheelForceFeedforwardsX parameter and returns itself.
         * <p>
         * Field-centric wheel force feedforwards to apply in the X direction, in
         * newtons. X is defined as forward according to WPILib convention, so this
         * determines the forward forces to apply.
         * <p>
         * These forces should include friction applied to the ground.
         * <p>
         * The order of the forces should match the order of the modules returned from
         * SwerveDrivetrain.
         *
         * @param newWheelForceFeedforwardsX Parameter to modify
         * @return this object
         */
        public ApplyFieldSpeeds withWheelForceFeedforwardsX(Force[] newWheelForceFeedforwardsX) {
            if (this.WheelForceFeedforwardsX.length != newWheelForceFeedforwardsX.length) {
                this.WheelForceFeedforwardsX = new double[newWheelForceFeedforwardsX.length];
            }
            for (int i = 0; i < this.WheelForceFeedforwardsX.length; ++i) {
                this.WheelForceFeedforwardsX[i] = newWheelForceFeedforwardsX[i].in(Newtons);
            }

            return this;
        }
        
        /**
         * Modifies the WheelForceFeedforwardsY parameter and returns itself.
         * <p>
         * Field-centric wheel force feedforwards to apply in the Y direction, in
         * newtons. Y is defined as to the left according to WPILib convention, so this
         * determines the forces to apply to the left.
         * <p>
         * These forces should include friction applied to the ground.
         * <p>
         * The order of the forces should match the order of the modules returned from
         * SwerveDrivetrain.
         *
         * @param newWheelForceFeedforwardsY Parameter to modify
         * @return this object
         */
        public ApplyFieldSpeeds withWheelForceFeedforwardsY(double[] newWheelForceFeedforwardsY) {
            this.WheelForceFeedforwardsY = newWheelForceFeedforwardsY;
            return this;
        }
        
        /**
         * Modifies the WheelForceFeedforwardsY parameter and returns itself.
         * <p>
         * Field-centric wheel force feedforwards to apply in the Y direction, in
         * newtons. Y is defined as to the left according to WPILib convention, so this
         * determines the forces to apply to the left.
         * <p>
         * These forces should include friction applied to the ground.
         * <p>
         * The order of the forces should match the order of the modules returned from
         * SwerveDrivetrain.
         *
         * @param newWheelForceFeedforwardsY Parameter to modify
         * @return this object
         */
        public ApplyFieldSpeeds withWheelForceFeedforwardsY(Force[] newWheelForceFeedforwardsY) {
            if (this.WheelForceFeedforwardsY.length != newWheelForceFeedforwardsY.length) {
                this.WheelForceFeedforwardsY = new double[newWheelForceFeedforwardsY.length];
            }
            for (int i = 0; i < this.WheelForceFeedforwardsY.length; ++i) {
                this.WheelForceFeedforwardsY[i] = newWheelForceFeedforwardsY[i].in(Newtons);
            }

            return this;
        }
        
        /**
         * Modifies the CenterOfRotation parameter and returns itself.
         * <p>
         * The center of rotation the robot should rotate around. This is (0,0) by
         * default, which will rotate around the center of the robot.
         *
         * @param newCenterOfRotation Parameter to modify
         * @return this object
         */
        public ApplyFieldSpeeds withCenterOfRotation(Translation2d newCenterOfRotation) {
            this.CenterOfRotation = newCenterOfRotation;
            return this;
        }
        
        /**
         * Modifies the DriveRequestType parameter and returns itself.
         * <p>
         * The type of control request to use for the drive motor.
         *
         * @param newDriveRequestType Parameter to modify
         * @return this object
         */
        public ApplyFieldSpeeds withDriveRequestType(SwerveModule.DriveRequestType newDriveRequestType) {
            this.DriveRequestType = newDriveRequestType;
            return this;
        }
        
        /**
         * Modifies the SteerRequestType parameter and returns itself.
         * <p>
         * The type of control request to use for the drive motor.
         *
         * @param newSteerRequestType Parameter to modify
         * @return this object
         */
        public ApplyFieldSpeeds withSteerRequestType(SwerveModule.SteerRequestType newSteerRequestType) {
            this.SteerRequestType = newSteerRequestType;
            return this;
        }
        
        /**
         * Modifies the DesaturateWheelSpeeds parameter and returns itself.
         * <p>
         * Whether to desaturate wheel speeds before applying. For more information, see
         * the documentation of {@link SwerveDriveKinematics#desaturateWheelSpeeds}.
         *
         * @param newDesaturateWheelSpeeds Parameter to modify
         * @return this object
         */
        public ApplyFieldSpeeds withDesaturateWheelSpeeds(boolean newDesaturateWheelSpeeds) {
            this.DesaturateWheelSpeeds = newDesaturateWheelSpeeds;
            return this;
        }
        
        /**
         * Modifies the ForwardPerspective parameter and returns itself.
         * <p>
         * The perspective to use when determining which direction is forward.
         *
         * @param newForwardPerspective Parameter to modify
         * @return this object
         */
        public ApplyFieldSpeeds withForwardPerspective(ForwardPerspectiveValue newForwardPerspective) {
            this.ForwardPerspective = newForwardPerspective;
            return this;
        }
        
        public StatusCode apply(SwerveControlParameters parameters, SwerveModule<?, ?, ?>... modulesToApply) {
            return StatusCode.valueOf(SwerveJNI.JNI_Request_Apply_ApplyFieldSpeeds(parameters.drivetrainId,
                Speeds.vxMetersPerSecond,
                Speeds.vyMetersPerSecond,
                Speeds.omegaRadiansPerSecond,
                WheelForceFeedforwardsX,
                WheelForceFeedforwardsY,
                CenterOfRotation.getX(),
                CenterOfRotation.getY(),
                DriveRequestType.value,
                SteerRequestType.value,
                DesaturateWheelSpeeds,
                ForwardPerspective.value));
        }
        
        public void applyNative(int id) {
            SwerveJNI.JNI_SetControl_ApplyFieldSpeeds(id,
                Speeds.vxMetersPerSecond,
                Speeds.vyMetersPerSecond,
                Speeds.omegaRadiansPerSecond,
                WheelForceFeedforwardsX,
                WheelForceFeedforwardsY,
                CenterOfRotation.getX(),
                CenterOfRotation.getY(),
                DriveRequestType.value,
                SteerRequestType.value,
                DesaturateWheelSpeeds,
                ForwardPerspective.value);
        }
    }

    /**
     * Drives the swerve drivetrain in a field-centric manner, maintaining a
     * specified heading angle to ensure the robot is facing the desired direction
     * <p>
     * When users use this request, they specify the direction the robot should
     * travel oriented against the field, and the direction the robot should be facing.
     * <p>
     * An example scenario is that the robot is oriented to the east, the VelocityX
     * is +5 m/s, VelocityY is 0 m/s, and TargetDirection is 180 degrees.
     * In this scenario, the robot would drive northward at 5 m/s and turn clockwise
     * to a target of 180 degrees.
     * <p>
     * This control request is especially useful for autonomous control, where the
     * robot should be facing a changing direction throughout the motion.
     */
    public static class FieldCentricFacingAngle implements SwerveRequest {
        /**
         * The velocity in the X direction, in m/s.
         * X is defined as forward according to WPILib convention,
         * so this determines how fast to travel forward.
         */
        public double VelocityX = 0;
        /**
         * The velocity in the Y direction, in m/s.
         * Y is defined as to the left according to WPILib convention,
         * so this determines how fast to travel to the left.
         */
        public double VelocityY = 0;
        /**
         * The desired direction to face.
         * 0 Degrees is defined as in the direction of the X axis.
         * As a result, a TargetDirection of 90 degrees will point along
         * the Y axis, or to the left.
         */
        public Rotation2d TargetDirection = new Rotation2d();
        /**
         * The rotational rate feedforward to add to the output of the heading
         * controller, in radians per second. When using a motion profile for the
         * target direction, this can be set to the current velocity reference of
         * the profile.
         */
        public double TargetRateFeedforward = 0;

        /**
         * The allowable deadband of the request, in m/s.
         */
        public double Deadband = 0;
        /**
         * The rotational deadband of the request, in radians per second.
         */
        public double RotationalDeadband = 0;
        /**
         * The maximum absolute rotational rate to allow, in radians per second.
         * Setting this to 0 results in no cap to rotational rate.
         */
        public double MaxAbsRotationalRate = 0;
        /**
         * The center of rotation the robot should rotate around.
         * This is (0,0) by default, which will rotate around the center of the robot.
         */
        public Translation2d CenterOfRotation = new Translation2d();

        /**
         * The type of control request to use for the drive motor.
         */
        public SwerveModule.DriveRequestType DriveRequestType = SwerveModule.DriveRequestType.OpenLoopVoltage;
        /**
         * The type of control request to use for the steer motor.
         */
        public SwerveModule.SteerRequestType SteerRequestType = SwerveModule.SteerRequestType.Position;
        /**
         * Whether to desaturate wheel speeds before applying.
         * For more information, see the documentation of {@link SwerveDriveKinematics#desaturateWheelSpeeds}.
         */
        public boolean DesaturateWheelSpeeds = true;

        /**
         * The perspective to use when determining which direction is forward.
         */
        public ForwardPerspectiveValue ForwardPerspective = ForwardPerspectiveValue.OperatorPerspective;

        /**
         * The PID controller used to maintain the desired heading.
         * Users can specify the PID gains to change how aggressively to maintain
         * heading.
         * <p>
         * This PID controller operates on heading radians and outputs a target
         * rotational rate in radians per second. Note that continuous input should
         * be enabled on the range [-pi, pi].
         */
        public PhoenixPIDController HeadingController = new PhoenixPIDController(0, 0, 0);

        private final FieldCentric m_fieldCentric = new FieldCentric();

        public FieldCentricFacingAngle() {
            HeadingController.enableContinuousInput(-Math.PI, Math.PI);
        }

        public StatusCode apply(SwerveControlParameters parameters, SwerveModule<?, ?, ?>... modulesToApply) {
            Rotation2d angleToFace = TargetDirection;
            if (ForwardPerspective == ForwardPerspectiveValue.OperatorPerspective) {
                /* If we're operator perspective, rotate the direction we want to face by the angle */
                angleToFace = angleToFace.rotateBy(parameters.operatorForwardDirection);
            }

            double toApplyOmega = TargetRateFeedforward +
                HeadingController.calculate(
                    parameters.currentPose.getRotation().getRadians(),
                    angleToFace.getRadians(),
                    parameters.timestamp
                );
            if (MaxAbsRotationalRate > 0.0) {
                if (toApplyOmega > MaxAbsRotationalRate) {
                    toApplyOmega = MaxAbsRotationalRate;
                } else if (toApplyOmega < -MaxAbsRotationalRate) {
                    toApplyOmega = -MaxAbsRotationalRate;
                }
            }

            return m_fieldCentric
                .withVelocityX(VelocityX)
                .withVelocityY(VelocityY)
                .withRotationalRate(toApplyOmega)
                .withDeadband(Deadband)
                .withRotationalDeadband(RotationalDeadband)
                .withCenterOfRotation(CenterOfRotation)
                .withDriveRequestType(DriveRequestType)
                .withSteerRequestType(SteerRequestType)
                .withDesaturateWheelSpeeds(DesaturateWheelSpeeds)
                .withForwardPerspective(ForwardPerspective)
                .apply(parameters, modulesToApply);
        }

        /**
         * Modifies the PID gains of the HeadingController parameter and returns itself.
         * <p>
         * Sets the proportional, integral, and differential coefficients used to maintain
         * the desired heading. Users can specify the PID gains to change how aggressively to
         * maintain heading.
         * <p>
         * This PID controller operates on heading radians and outputs a target
         * rotational rate in radians per second.
         *
         * @param kP The proportional coefficient; must be >= 0
         * @param kI The integral coefficient; must be >= 0
         * @param kD The differential coefficient; must be >= 0
         * @return this object
         */
        public FieldCentricFacingAngle withHeadingPID(double kP, double kI, double kD)
        {
            this.HeadingController.setPID(kP, kI, kD);
            return this;
        }

        /**
         * Modifies the VelocityX parameter and returns itself.
         * <p>
         * The velocity in the X direction, in m/s. X is defined as forward according to
         * WPILib convention, so this determines how fast to travel forward.
         *
         * @param newVelocityX Parameter to modify
         * @return this object
         */
        public FieldCentricFacingAngle withVelocityX(double newVelocityX) {
            this.VelocityX = newVelocityX;
            return this;
        }

        /**
         * Modifies the VelocityX parameter and returns itself.
         * <p>
         * The velocity in the X direction, in m/s. X is defined as forward according to
         * WPILib convention, so this determines how fast to travel forward.
         *
         * @param newVelocityX Parameter to modify
         * @return this object
         */
        public FieldCentricFacingAngle withVelocityX(LinearVelocity newVelocityX) {
            this.VelocityX = newVelocityX.in(MetersPerSecond);
            return this;
        }

        /**
         * Modifies the VelocityY parameter and returns itself.
         * <p>
         * The velocity in the Y direction, in m/s. Y is defined as to the left
         * according to WPILib convention, so this determines how fast to travel to the
         * left.
         *
         * @param newVelocityY Parameter to modify
         * @return this object
         */
        public FieldCentricFacingAngle withVelocityY(double newVelocityY) {
            this.VelocityY = newVelocityY;
            return this;
        }

        /**
         * Modifies the VelocityY parameter and returns itself.
         * <p>
         * The velocity in the Y direction, in m/s. Y is defined as to the left
         * according to WPILib convention, so this determines how fast to travel to the
         * left.
         *
         * @param newVelocityY Parameter to modify
         * @return this object
         */
        public FieldCentricFacingAngle withVelocityY(LinearVelocity newVelocityY) {
            this.VelocityY = newVelocityY.in(MetersPerSecond);
            return this;
        }

        /**
         * Modifies the TargetDirection parameter and returns itself.
         * <p>
         * The desired direction to face. 0 Degrees is defined as in the direction of
         * the X axis. As a result, a TargetDirection of 90 degrees will point along
         * the Y axis, or to the left.
         *
         * @param newTargetDirection Parameter to modify
         * @return this object
         */
        public FieldCentricFacingAngle withTargetDirection(Rotation2d newTargetDirection) {
            this.TargetDirection = newTargetDirection;
            return this;
        }

        /**
         * Modifies the TargetRateFeedforward parameter and returns itself.
         * <p>
         * The rotational rate feedforward to add to the output of the heading
         * controller, in radians per second. When using a motion profile for the
         * target direction, this can be set to the current velocity reference of
         * the profile.
         *
         * @param newTargetRateFeedforward Parameter to modify
         * @return this object
         */
        public FieldCentricFacingAngle withTargetRateFeedforward(double newTargetRateFeedforward) {
            this.TargetRateFeedforward = newTargetRateFeedforward;
            return this;
        }
        /**
         * Modifies the TargetRateFeedforward parameter and returns itself.
         * <p>
         * The rotational rate feedforward to add to the output of the heading
         * controller, in radians per second. When using a motion profile for the
         * target direction, this can be set to the current velocity reference of
         * the profile.
         *
         * @param newTargetRateFeedforward Parameter to modify
         * @return this object
         */
        public FieldCentricFacingAngle withTargetRateFeedforward(AngularVelocity newTargetRateFeedforward) {
            this.TargetRateFeedforward = newTargetRateFeedforward.in(RadiansPerSecond);
            return this;
        }

        /**
         * Modifies the Deadband parameter and returns itself.
         * <p>
         * The allowable deadband of the request, in m/s.
         *
         * @param newDeadband Parameter to modify
         * @return this object
         */
        public FieldCentricFacingAngle withDeadband(double newDeadband) {
            this.Deadband = newDeadband;
            return this;
        }

        /**
         * Modifies the Deadband parameter and returns itself.
         * <p>
         * The allowable deadband of the request, in m/s.
         *
         * @param newDeadband Parameter to modify
         * @return this object
         */
        public FieldCentricFacingAngle withDeadband(LinearVelocity newDeadband) {
            this.Deadband = newDeadband.in(MetersPerSecond);
            return this;
        }

        /**
         * Modifies the RotationalDeadband parameter and returns itself.
         * <p>
         * The rotational deadband of the request, in radians per second.
         *
         * @param newRotationalDeadband Parameter to modify
         * @return this object
         */
        public FieldCentricFacingAngle withRotationalDeadband(double newRotationalDeadband) {
            this.RotationalDeadband = newRotationalDeadband;
            return this;
        }

        /**
         * Modifies the RotationalDeadband parameter and returns itself.
         * <p>
         * The rotational deadband of the request, in radians per second.
         *
         * @param newRotationalDeadband Parameter to modify
         * @return this object
         */
        public FieldCentricFacingAngle withRotationalDeadband(AngularVelocity newRotationalDeadband) {
            this.RotationalDeadband = newRotationalDeadband.in(RadiansPerSecond);
            return this;
        }

        /**
         * Modifies the MaxAbsRotationalRate parameter and returns itself.
         * <p>
         * The maximum absolute rotational rate to allow, in radians per second.
         * Setting this to 0 results in no cap to rotational rate.
         *
         * @param newMaxAbsRotationalRate Parameter to modify
         * @return this object
         */
        public FieldCentricFacingAngle withMaxAbsRotationalRate(double newMaxAbsRotationalRate) {
            this.MaxAbsRotationalRate = newMaxAbsRotationalRate;
            return this;
        }

        /**
         * Modifies the MaxAbsRotationalRate parameter and returns itself.
         * <p>
         * The maximum absolute rotational rate to allow, in radians per second.
         * Setting this to 0 results in no cap to rotational rate.
         *
         * @param newMaxAbsRotationalRate Parameter to modify
         * @return this object
         */
        public FieldCentricFacingAngle withMaxAbsRotationalRate(AngularVelocity newMaxAbsRotationalRate) {
            this.MaxAbsRotationalRate = newMaxAbsRotationalRate.in(RadiansPerSecond);
            return this;
        }

        /**
         * Modifies the CenterOfRotation parameter and returns itself.
         * <p>
         * The center of rotation the robot should rotate around. This is (0,0) by
         * default, which will rotate around the center of the robot.
         *
         * @param newCenterOfRotation Parameter to modify
         * @return this object
         */
        public FieldCentricFacingAngle withCenterOfRotation(Translation2d newCenterOfRotation) {
            this.CenterOfRotation = newCenterOfRotation;
            return this;
        }

        /**
         * Modifies the DriveRequestType parameter and returns itself.
         * <p>
         * The type of control request to use for the drive motor.
         *
         * @param newDriveRequestType Parameter to modify
         * @return this object
         */
        public FieldCentricFacingAngle withDriveRequestType(SwerveModule.DriveRequestType newDriveRequestType) {
            this.DriveRequestType = newDriveRequestType;
            return this;
        }

        /**
         * Modifies the SteerRequestType parameter and returns itself.
         * <p>
         * The type of control request to use for the drive motor.
         *
         * @param newSteerRequestType Parameter to modify
         * @return this object
         */
        public FieldCentricFacingAngle withSteerRequestType(SwerveModule.SteerRequestType newSteerRequestType) {
            this.SteerRequestType = newSteerRequestType;
            return this;
        }

        /**
         * Modifies the DesaturateWheelSpeeds parameter and returns itself.
         * <p>
         * Whether to desaturate wheel speeds before applying. For more information, see
         * the documentation of {@link SwerveDriveKinematics#desaturateWheelSpeeds}.
         *
         * @param newDesaturateWheelSpeeds Parameter to modify
         * @return this object
         */
        public FieldCentricFacingAngle withDesaturateWheelSpeeds(boolean newDesaturateWheelSpeeds) {
            this.DesaturateWheelSpeeds = newDesaturateWheelSpeeds;
            return this;
        }

        /**
         * Modifies the ForwardPerspective parameter and returns itself.
         * <p>
         * The perspective to use when determining which direction is forward.
         *
         * @param newForwardPerspective Parameter to modify
         * @return this object
         */
        public FieldCentricFacingAngle withForwardPerspective(ForwardPerspectiveValue newForwardPerspective) {
            this.ForwardPerspective = newForwardPerspective;
            return this;
        }
    }

    /**
     * Drives the swerve drivetrain in a robot-centric manner, maintaining a
     * specified heading angle to ensure the robot is facing the desired direction
     * <p>
     * When users use this request, they specify the direction the robot should
     * travel oriented against the robot itself, and the direction the robot should
     * be facing relative to the field.
     * <p>
     * An example scenario is that the robot is oriented to the east, the VelocityX
     * is +5 m/s, VelocityY is 0 m/s, and TargetDirection is 180 degrees.
     * In this scenario, the robot would drive forward at 5 m/s and turn clockwise
     * to a target of 180 degrees.
     * <p>
     * This control request is especially useful for vision control, where the
     * robot should be facing a vision target throughout the motion.
     */
    public static class RobotCentricFacingAngle implements SwerveRequest {
        /**
         * The velocity in the X direction, in m/s.
         * X is defined as forward according to WPILib convention,
         * so this determines how fast to travel forward.
         */
        public double VelocityX = 0;
        /**
         * The velocity in the Y direction, in m/s.
         * Y is defined as to the left according to WPILib convention,
         * so this determines how fast to travel to the left.
         */
        public double VelocityY = 0;
        /**
         * The desired direction to face.
         * 0 Degrees is defined as in the direction of the X axis.
         * As a result, a TargetDirection of 90 degrees will point along
         * the Y axis, or to the left.
         */
        public Rotation2d TargetDirection = new Rotation2d();
        /**
         * The rotational rate feedforward to add to the output of the heading
         * controller, in radians per second. When using a motion profile for the
         * target direction, this can be set to the current velocity reference of
         * the profile.
         */
        public double TargetRateFeedforward = 0;

        /**
         * The allowable deadband of the request, in m/s.
         */
        public double Deadband = 0;
        /**
         * The rotational deadband of the request, in radians per second.
         */
        public double RotationalDeadband = 0;
        /**
         * The maximum absolute rotational rate to allow, in radians per second.
         * Setting this to 0 results in no cap to rotational rate.
         */
        public double MaxAbsRotationalRate = 0;
        /**
         * The center of rotation the robot should rotate around.
         * This is (0,0) by default, which will rotate around the center of the robot.
         */
        public Translation2d CenterOfRotation = new Translation2d();

        /**
         * The type of control request to use for the drive motor.
         */
        public SwerveModule.DriveRequestType DriveRequestType = SwerveModule.DriveRequestType.OpenLoopVoltage;
        /**
         * The type of control request to use for the steer motor.
         */
        public SwerveModule.SteerRequestType SteerRequestType = SwerveModule.SteerRequestType.Position;
        /**
         * Whether to desaturate wheel speeds before applying.
         * For more information, see the documentation of {@link SwerveDriveKinematics#desaturateWheelSpeeds}.
         */
        public boolean DesaturateWheelSpeeds = true;

        /**
         * The perspective to use when determining which direction is forward
         * for the target heading.
         */
        public ForwardPerspectiveValue ForwardPerspective = ForwardPerspectiveValue.OperatorPerspective;

        /**
         * The PID controller used to maintain the desired heading.
         * Users can specify the PID gains to change how aggressively to maintain
         * heading.
         * <p>
         * This PID controller operates on heading radians and outputs a target
         * rotational rate in radians per second. Note that continuous input should
         * be enabled on the range [-pi, pi].
         */
        public PhoenixPIDController HeadingController = new PhoenixPIDController(0, 0, 0);

        private final RobotCentric m_robotCentric = new RobotCentric();

        public RobotCentricFacingAngle() {
            HeadingController.enableContinuousInput(-Math.PI, Math.PI);
        }

        public StatusCode apply(SwerveControlParameters parameters, SwerveModule<?, ?, ?>... modulesToApply) {
            Rotation2d angleToFace = TargetDirection;
            if (ForwardPerspective == ForwardPerspectiveValue.OperatorPerspective) {
                /* If we're operator perspective, rotate the direction we want to face by the angle */
                angleToFace = angleToFace.rotateBy(parameters.operatorForwardDirection);
            }

            double toApplyOmega = TargetRateFeedforward +
                HeadingController.calculate(
                    parameters.currentPose.getRotation().getRadians(),
                    angleToFace.getRadians(),
                    parameters.timestamp
                );
            if (MaxAbsRotationalRate > 0.0) {
                if (toApplyOmega > MaxAbsRotationalRate) {
                    toApplyOmega = MaxAbsRotationalRate;
                } else if (toApplyOmega < -MaxAbsRotationalRate) {
                    toApplyOmega = -MaxAbsRotationalRate;
                }
            }

            return m_robotCentric
                .withVelocityX(VelocityX)
                .withVelocityY(VelocityY)
                .withRotationalRate(toApplyOmega)
                .withDeadband(Deadband)
                .withRotationalDeadband(RotationalDeadband)
                .withCenterOfRotation(CenterOfRotation)
                .withDriveRequestType(DriveRequestType)
                .withSteerRequestType(SteerRequestType)
                .withDesaturateWheelSpeeds(DesaturateWheelSpeeds)
                .apply(parameters, modulesToApply);
        }

        /**
         * Modifies the PID gains of the HeadingController parameter and returns itself.
         * <p>
         * Sets the proportional, integral, and differential coefficients used to maintain
         * the desired heading. Users can specify the PID gains to change how aggressively to
         * maintain heading.
         * <p>
         * This PID controller operates on heading radians and outputs a target
         * rotational rate in radians per second.
         *
         * @param kP The proportional coefficient; must be >= 0
         * @param kI The integral coefficient; must be >= 0
         * @param kD The differential coefficient; must be >= 0
         * @return this object
         */
        public RobotCentricFacingAngle withHeadingPID(double kP, double kI, double kD)
        {
            this.HeadingController.setPID(kP, kI, kD);
            return this;
        }

        /**
         * Modifies the VelocityX parameter and returns itself.
         * <p>
         * The velocity in the X direction, in m/s. X is defined as forward according to
         * WPILib convention, so this determines how fast to travel forward.
         *
         * @param newVelocityX Parameter to modify
         * @return this object
         */
        public RobotCentricFacingAngle withVelocityX(double newVelocityX) {
            this.VelocityX = newVelocityX;
            return this;
        }

        /**
         * Modifies the VelocityX parameter and returns itself.
         * <p>
         * The velocity in the X direction, in m/s. X is defined as forward according to
         * WPILib convention, so this determines how fast to travel forward.
         *
         * @param newVelocityX Parameter to modify
         * @return this object
         */
        public RobotCentricFacingAngle withVelocityX(LinearVelocity newVelocityX) {
            this.VelocityX = newVelocityX.in(MetersPerSecond);
            return this;
        }

        /**
         * Modifies the VelocityY parameter and returns itself.
         * <p>
         * The velocity in the Y direction, in m/s. Y is defined as to the left
         * according to WPILib convention, so this determines how fast to travel to the
         * left.
         *
         * @param newVelocityY Parameter to modify
         * @return this object
         */
        public RobotCentricFacingAngle withVelocityY(double newVelocityY) {
            this.VelocityY = newVelocityY;
            return this;
        }

        /**
         * Modifies the VelocityY parameter and returns itself.
         * <p>
         * The velocity in the Y direction, in m/s. Y is defined as to the left
         * according to WPILib convention, so this determines how fast to travel to the
         * left.
         *
         * @param newVelocityY Parameter to modify
         * @return this object
         */
        public RobotCentricFacingAngle withVelocityY(LinearVelocity newVelocityY) {
            this.VelocityY = newVelocityY.in(MetersPerSecond);
            return this;
        }

        /**
         * Modifies the TargetDirection parameter and returns itself.
         * <p>
         * The desired direction to face. 0 Degrees is defined as in the direction of
         * the X axis. As a result, a TargetDirection of 90 degrees will point along
         * the Y axis, or to the left.
         *
         * @param newTargetDirection Parameter to modify
         * @return this object
         */
        public RobotCentricFacingAngle withTargetDirection(Rotation2d newTargetDirection) {
            this.TargetDirection = newTargetDirection;
            return this;
        }

        /**
         * Modifies the TargetRateFeedforward parameter and returns itself.
         * <p>
         * The rotational rate feedforward to add to the output of the heading
         * controller, in radians per second. When using a motion profile for the
         * target direction, this can be set to the current velocity reference of
         * the profile.
         *
         * @param newTargetRateFeedforward Parameter to modify
         * @return this object
         */
        public RobotCentricFacingAngle withTargetRateFeedforward(double newTargetRateFeedforward) {
            this.TargetRateFeedforward = newTargetRateFeedforward;
            return this;
        }
        /**
         * Modifies the TargetRateFeedforward parameter and returns itself.
         * <p>
         * The rotational rate feedforward to add to the output of the heading
         * controller, in radians per second. When using a motion profile for the
         * target direction, this can be set to the current velocity reference of
         * the profile.
         *
         * @param newTargetRateFeedforward Parameter to modify
         * @return this object
         */
        public RobotCentricFacingAngle withTargetRateFeedforward(AngularVelocity newTargetRateFeedforward) {
            this.TargetRateFeedforward = newTargetRateFeedforward.in(RadiansPerSecond);
            return this;
        }

        /**
         * Modifies the Deadband parameter and returns itself.
         * <p>
         * The allowable deadband of the request, in m/s.
         *
         * @param newDeadband Parameter to modify
         * @return this object
         */
        public RobotCentricFacingAngle withDeadband(double newDeadband) {
            this.Deadband = newDeadband;
            return this;
        }

        /**
         * Modifies the Deadband parameter and returns itself.
         * <p>
         * The allowable deadband of the request, in m/s.
         *
         * @param newDeadband Parameter to modify
         * @return this object
         */
        public RobotCentricFacingAngle withDeadband(LinearVelocity newDeadband) {
            this.Deadband = newDeadband.in(MetersPerSecond);
            return this;
        }

        /**
         * Modifies the RotationalDeadband parameter and returns itself.
         * <p>
         * The rotational deadband of the request, in radians per second.
         *
         * @param newRotationalDeadband Parameter to modify
         * @return this object
         */
        public RobotCentricFacingAngle withRotationalDeadband(double newRotationalDeadband) {
            this.RotationalDeadband = newRotationalDeadband;
            return this;
        }

        /**
         * Modifies the RotationalDeadband parameter and returns itself.
         * <p>
         * The rotational deadband of the request, in radians per second.
         *
         * @param newRotationalDeadband Parameter to modify
         * @return this object
         */
        public RobotCentricFacingAngle withRotationalDeadband(AngularVelocity newRotationalDeadband) {
            this.RotationalDeadband = newRotationalDeadband.in(RadiansPerSecond);
            return this;
        }

        /**
         * Modifies the MaxAbsRotationalRate parameter and returns itself.
         * <p>
         * The maximum absolute rotational rate to allow, in radians per second.
         * Setting this to 0 results in no cap to rotational rate.
         *
         * @param newMaxAbsRotationalRate Parameter to modify
         * @return this object
         */
        public RobotCentricFacingAngle withMaxAbsRotationalRate(double newMaxAbsRotationalRate) {
            this.MaxAbsRotationalRate = newMaxAbsRotationalRate;
            return this;
        }

        /**
         * Modifies the MaxAbsRotationalRate parameter and returns itself.
         * <p>
         * The maximum absolute rotational rate to allow, in radians per second.
         * Setting this to 0 results in no cap to rotational rate.
         *
         * @param newMaxAbsRotationalRate Parameter to modify
         * @return this object
         */
        public RobotCentricFacingAngle withMaxAbsRotationalRate(AngularVelocity newMaxAbsRotationalRate) {
            this.MaxAbsRotationalRate = newMaxAbsRotationalRate.in(RadiansPerSecond);
            return this;
        }

        /**
         * Modifies the CenterOfRotation parameter and returns itself.
         * <p>
         * The center of rotation the robot should rotate around. This is (0,0) by
         * default, which will rotate around the center of the robot.
         *
         * @param newCenterOfRotation Parameter to modify
         * @return this object
         */
        public RobotCentricFacingAngle withCenterOfRotation(Translation2d newCenterOfRotation) {
            this.CenterOfRotation = newCenterOfRotation;
            return this;
        }

        /**
         * Modifies the DriveRequestType parameter and returns itself.
         * <p>
         * The type of control request to use for the drive motor.
         *
         * @param newDriveRequestType Parameter to modify
         * @return this object
         */
        public RobotCentricFacingAngle withDriveRequestType(SwerveModule.DriveRequestType newDriveRequestType) {
            this.DriveRequestType = newDriveRequestType;
            return this;
        }

        /**
         * Modifies the SteerRequestType parameter and returns itself.
         * <p>
         * The type of control request to use for the drive motor.
         *
         * @param newSteerRequestType Parameter to modify
         * @return this object
         */
        public RobotCentricFacingAngle withSteerRequestType(SwerveModule.SteerRequestType newSteerRequestType) {
            this.SteerRequestType = newSteerRequestType;
            return this;
        }

        /**
         * Modifies the DesaturateWheelSpeeds parameter and returns itself.
         * <p>
         * Whether to desaturate wheel speeds before applying. For more information, see
         * the documentation of {@link SwerveDriveKinematics#desaturateWheelSpeeds}.
         *
         * @param newDesaturateWheelSpeeds Parameter to modify
         * @return this object
         */
        public RobotCentricFacingAngle withDesaturateWheelSpeeds(boolean newDesaturateWheelSpeeds) {
            this.DesaturateWheelSpeeds = newDesaturateWheelSpeeds;
            return this;
        }

        /**
         * Modifies the ForwardPerspective parameter and returns itself.
         * <p>
         * The perspective to use when determining which direction is forward
         * for the target heading.
         *
         * @param newForwardPerspective Parameter to modify
         * @return this object
         */
        public RobotCentricFacingAngle withForwardPerspective(ForwardPerspectiveValue newForwardPerspective) {
            this.ForwardPerspective = newForwardPerspective;
            return this;
        }
    }

    /**
     * SysId-specific SwerveRequest to characterize the translational
     * characteristics of a swerve drivetrain.
     */
    public static class SysIdSwerveTranslation implements SwerveRequest {
        /**
         * Voltage to apply to drive wheels.
         */
        public double VoltsToApply = 0;

        /** Local reference to a voltage request for the drive motors */
        private final VoltageOut m_driveRequest = new VoltageOut(0);
        /** Local reference to a position voltage request for the steer motors */
        private final PositionVoltage m_steerRequest_Voltage = new PositionVoltage(0);
        /** Local reference to a position torque current request for the steer motors */
        private final PositionTorqueCurrentFOC m_steerRequest_TorqueCurrent = new PositionTorqueCurrentFOC(0);

        public StatusCode apply(SwerveControlParameters parameters, SwerveModule<?, ?, ?>... modulesToApply) {
            for (int i = 0; i < modulesToApply.length; ++i) {
                switch (modulesToApply[i].getSteerClosedLoopOutputType()) {
                    case Voltage:
                        modulesToApply[i].apply(
                            m_driveRequest.withOutput(VoltsToApply),
                            m_steerRequest_Voltage.withPosition(0)
                        );
                        break;
                    case TorqueCurrentFOC:
                        modulesToApply[i].apply(
                            m_driveRequest.withOutput(VoltsToApply),
                            m_steerRequest_TorqueCurrent.withPosition(0)
                        );
                        break;
                }
            }
            return StatusCode.OK;
        }

        /**
         * Sets the voltage to apply to the drive wheels.
         *
         * @param volts Voltage to apply
         * @return this request
         */
        public SysIdSwerveTranslation withVolts(double volts) {
            VoltsToApply = volts;
            return this;
        }
        /**
         * Sets the voltage to apply to the drive wheels.
         *
         * @param volts Voltage to apply
         * @return this request
         */
        public SysIdSwerveTranslation withVolts(Voltage volts) {
            VoltsToApply = volts.in(Volts);
            return this;
        }
    }

    /**
     * SysId-specific SwerveRequest to characterize the rotational
     * characteristics of a swerve drivetrain. This is useful to
     * characterize the heading controller for FieldCentricFacingAngle.
     * <p>
     * The RotationalRate of this swerve request should be logged.
     * When importing the log to SysId, set the "voltage" to
     * RotationalRate, "position" to the Pigeon 2 Yaw, and "velocity"
     * to the Pigeon 2 AngularVelocityZWorld. Note that the position
     * and velocity will both need to be scaled by pi/180.
     * <p>
     * Alternatively, the MotorVoltage of one of the drive motors can
     * be loaded into the SysId "voltage" field, which can be useful
     * when determining the MOI of the robot.
     */
    public static class SysIdSwerveRotation implements SwerveRequest {
        /**
         * The angular rate to rotate at, in radians per second.
         */
        public double RotationalRate = 0;

        /** Local reference to a voltage request for the drive motors */
        private final VoltageOut m_driveRequest = new VoltageOut(0);
        /** Local reference to a position voltage request for the steer motors */
        private final PositionVoltage m_steerRequest_Voltage = new PositionVoltage(0);
        /** Local reference to a position torque current request for the steer motors */
        private final PositionTorqueCurrentFOC m_steerRequest_TorqueCurrent = new PositionTorqueCurrentFOC(0);

        public StatusCode apply(SwerveControlParameters parameters, SwerveModule<?, ?, ?>... modulesToApply) {
            for (int i = 0; i < modulesToApply.length; ++i) {
                var speedMps = RotationalRate * parameters.moduleLocations[i].getNorm();
                var driveVoltage = speedMps / parameters.kMaxSpeedMps * 12.0;

                var angle = parameters.moduleLocations[i].getAngle().plus(Rotation2d.kCCW_90deg);

                switch (modulesToApply[i].getSteerClosedLoopOutputType()) {
                    case Voltage:
                        modulesToApply[i].apply(
                            m_driveRequest.withOutput(driveVoltage),
                            m_steerRequest_Voltage.withPosition(angle.getRotations())
                        );
                        break;
                    case TorqueCurrentFOC:
                        modulesToApply[i].apply(
                            m_driveRequest.withOutput(driveVoltage),
                            m_steerRequest_TorqueCurrent.withPosition(angle.getRotations())
                        );
                        break;
                }
            }
            return StatusCode.OK;
        }

        /**
         * Update the angular rate to rotate at, in radians per second.
         *
         * @param newRotationalRate Angular rate to rotate at
         * @return this request
         */
        public SysIdSwerveRotation withRotationalRate(double newRotationalRate) {
            RotationalRate = newRotationalRate;
            return this;
        }
        /**
         * Update the angular rate to rotate at.
         *
         * @param newRotationalRate The angular rate to rotate at
         * @return this request
         */
        public SysIdSwerveRotation withRotationalRate(AngularVelocity newRotationalRate) {
            RotationalRate = newRotationalRate.in(RadiansPerSecond);
            return this;
        }
    }

    /**
     * SysId-specific SwerveRequest to characterize the steer module
     * characteristics of a swerve drivetrain.
     */
    public static class SysIdSwerveSteerGains implements SwerveRequest {
        /**
         * Voltage to apply to steer motors.
         */
        public double VoltsToApply = 0;

        /** Local reference to a coast request for the drive motors */
        private final CoastOut m_driveRequest = new CoastOut();
        /** Local reference to a voltage request for the steer motors */
        private final VoltageOut m_steerRequest = new VoltageOut(0);

        public StatusCode apply(SwerveControlParameters parameters, SwerveModule<?, ?, ?>... modulesToApply) {
            for (int i = 0; i < modulesToApply.length; ++i) {
                modulesToApply[i].apply(m_driveRequest, m_steerRequest.withOutput(VoltsToApply));
            }
            return StatusCode.OK;
        }

        /**
         * Sets the voltage to apply to the steer motors.
         *
         * @param volts Voltage to apply
         * @return this request
         */
        public SysIdSwerveSteerGains withVolts(double volts) {
            VoltsToApply = volts;
            return this;
        }
        /**
         * Sets the voltage to apply to the steer motors.
         *
         * @param volts Voltage to apply
         * @return this request
         */
        public SysIdSwerveSteerGains withVolts(Voltage volts) {
            VoltsToApply = volts.in(Volts);
            return this;
        }
    }
}