/*
 * 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.Newtons;

import java.util.HashMap;
import java.util.concurrent.locks.Lock;
import java.util.concurrent.locks.ReentrantLock;

import com.ctre.phoenix6.StatusCode;
import com.ctre.phoenix6.configs.CANcoderConfiguration;
import com.ctre.phoenix6.configs.CANdiConfiguration;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.configs.TalonFXSConfiguration;
import com.ctre.phoenix6.controls.ControlRequest;
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.hardware.CANdi;
import com.ctre.phoenix6.hardware.ParentDevice;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.hardware.TalonFXS;
import com.ctre.phoenix6.hardware.traits.CommonTalon;
import com.ctre.phoenix6.signals.BrushedMotorWiringValue;
import com.ctre.phoenix6.signals.ExternalFeedbackSensorSourceValue;
import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.MotorArrangementValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
import com.ctre.phoenix6.signals.SensorDirectionValue;
import com.ctre.phoenix6.signals.SensorPhaseValue;
import com.ctre.phoenix6.swerve.SwerveDrivetrain.DeviceConstructor;
import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType;
import com.ctre.phoenix6.swerve.SwerveModuleConstants.DriveMotorArrangement;
import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement;
import com.ctre.phoenix6.swerve.jni.SwerveJNI;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.units.measure.Force;
import edu.wpi.first.wpilibj.DriverStation;

/**
 * Swerve Module class that encapsulates a swerve module powered by CTR
 * Electronics devices.
 * <p>
 * This class handles the hardware devices and configures them for
 * swerve module operation using the Phoenix 6 API.
 * <p>
 * This class constructs hardware devices internally, so the user
 * only specifies the constants (IDs, PID gains, gear ratios, etc).
 * Getters for these hardware devices are available.
 */
public class SwerveModule<
    DriveMotorT extends CommonTalon,
    SteerMotorT extends CommonTalon,
    EncoderT extends ParentDevice
> {
    /**
     * All possible control requests for the module steer motor.
     */
    public enum SteerRequestType {
        /**
         * Control the drive motor using a Motion Magic® Expo request.
         * The control output type is determined by {@link SwerveModuleConstants#SteerMotorClosedLoopOutput}
         */
        MotionMagicExpo(0),
        /**
         * Control the drive motor using an unprofiled position request.
         * The control output type is determined by {@link SwerveModuleConstants#SteerMotorClosedLoopOutput}
         */
        Position(1);

        public final int value;

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

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

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

    /**
     * All possible control requests for the module drive motor.
     */
    public enum DriveRequestType {
        /**
         * Control the drive motor using an open-loop voltage request.
         */
        OpenLoopVoltage(0),
        /**
         * Control the drive motor using a velocity closed-loop request.
         * The control output type is determined by {@link SwerveModuleConstants#DriveMotorClosedLoopOutput}
         */
        Velocity(1);

        public final int value;

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

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

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

    /**
     * Contains everything the swerve module needs to apply a request.
     */
    public static class ModuleRequest {
        /**
         * Unoptimized speed and direction the module should target.
         */
        public SwerveModuleState State = new SwerveModuleState();

        /**
         * Robot-centric wheel force feedforward to apply in the
         * X direction. X is defined as forward according to WPILib
         * convention, so this determines the forward force to apply.
         * <p>
         * This force should include friction.
         */
        public double WheelForceFeedforwardX = 0.0;
        /**
         * Robot-centric wheel force feedforward to apply in the
         * Y direction. Y is defined as to the left according to WPILib
         * convention, so this determines the force to apply to the left.
         * <p>
         * This force should include friction.
         */
        public double WheelForceFeedforwardY = 0.0;

        /**
         * The type of control request to use for the drive motor.
         */
        public DriveRequestType DriveRequest = DriveRequestType.OpenLoopVoltage;
        /**
         * The type of control request to use for the steer motor.
         */
        public SteerRequestType SteerRequest = SteerRequestType.Position;

        /**
         * The update period of the module request. Setting this to a
         * non-zero value adds a velocity feedforward to the steer motor.
         */
        public double UpdatePeriod = 0.0;

        /**
         * When using Voltage-based control, set to true (default) to use FOC commutation
         * (requires Phoenix Pro), which increases peak power by ~15%. Set to false to
         * use trapezoidal commutation. This is ignored when using Torque-based control,
         * which always uses FOC.
         * <p>
         * FOC improves motor performance by leveraging torque (current) control. 
         * However, this may be inconvenient for applications that require specifying
         * duty cycle or voltage.  CTR-Electronics has developed a hybrid method that
         * combines the performances gains of FOC while still allowing applications to
         * provide duty cycle or voltage demand.  This not to be confused with simple
         * sinusoidal control or phase voltage control which lacks the performance
         * gains.
         */
        public boolean EnableFOC = true;

        /**
         * Modifies the State parameter and returns itself.
         * <p>
         * Unoptimized speed and direction the module should target.
         *
         * @param newState Parameter to modify
         * @return Itself
         */
        public ModuleRequest withState(SwerveModuleState newState) {
            this.State = newState;
            return this;
        }

        /**
         * Modifies the WheelForceFeedforwardX parameter and returns itself.
         * <p>
         * Robot-centric wheel force feedforward to apply in the
         * X direction. X is defined as forward according to WPILib
         * convention, so this determines the forward force to apply.
         * <p>
         * This force should include friction applied to the ground.
         *
         * @param newWheelForceFeedforwardX Parameter to modify
         * @return Itself
         */
        public ModuleRequest withWheelForceFeedforwardX(double newWheelForceFeedforwardX) {
            this.WheelForceFeedforwardX = newWheelForceFeedforwardX;
            return this;
        }
        /**
         * Modifies the WheelForceFeedforwardX parameter and returns itself.
         * <p>
         * Robot-centric wheel force feedforward to apply in the
         * X direction. X is defined as forward according to WPILib
         * convention, so this determines the forward force to apply.
         * <p>
         * This force should include friction applied to the ground.
         *
         * @param newWheelForceFeedforwardX Parameter to modify
         * @return Itself
         */
        public ModuleRequest withWheelForceFeedforwardX(Force newWheelForceFeedforwardX) {
            this.WheelForceFeedforwardX = newWheelForceFeedforwardX.in(Newtons);
            return this;
        }
        /**
         * Helper method to get the WheelForceFeedforwardX parameter
         * as a unit type. If not using the Java units library,
         * {@link #WheelForceFeedforwardX} can be accessed directly instead.
         * <p>
         * Robot-centric wheel force feedforward to apply in the
         * X direction. X is defined as forward according to WPILib
         * convention, so this determines the forward force to apply.
         * <p>
         * This force should include friction applied to the ground.
         *
         * @return WheelForceFeedforwardX
         */
        public Force getWheelForceFeedforwardXMeasure() {
            return Newtons.of(WheelForceFeedforwardX);
        }

        /**
         * Modifies the WheelForceFeedforwardY parameter and returns itself.
         * <p>
         * Robot-centric wheel force feedforward to apply in the
         * Y direction. Y is defined as to the left according to WPILib
         * convention, so this determines the force to apply to the left.
         * <p>
         * This force should include friction applied to the ground.
         *
         * @param newWheelForceFeedforwardY Parameter to modify
         * @return Itself
         */
        public ModuleRequest withWheelForceFeedforwardY(double newWheelForceFeedforwardY) {
            this.WheelForceFeedforwardY = newWheelForceFeedforwardY;
            return this;
        }
        /**
         * Modifies the WheelForceFeedforwardY parameter and returns itself.
         * <p>
         * Robot-centric wheel force feedforward to apply in the
         * Y direction. Y is defined as to the left according to WPILib
         * convention, so this determines the force to apply to the left.
         * <p>
         * This force should include friction applied to the ground.
         *
         * @param newWheelForceFeedforwardY Parameter to modify
         * @return Itself
         */
        public ModuleRequest withWheelForceFeedforwardY(Force newWheelForceFeedforwardY) {
            this.WheelForceFeedforwardY = newWheelForceFeedforwardY.in(Newtons);
            return this;
        }
        /**
         * Helper method to get the WheelForceFeedforwardY parameter
         * as a unit type. If not using the Java units library,
         * {@link #WheelForceFeedforwardY} can be accessed directly instead.
         * <p>
         * Robot-centric wheel force feedforward to apply in the
         * Y direction. Y is defined as to the left according to WPILib
         * convention, so this determines the force to apply to the left.
         * <p>
         * This force should include friction applied to the ground.
         *
         * @return WheelForceFeedforwardY
         */
        public Force getWheelForceFeedforwardYMeasure() {
            return Newtons.of(WheelForceFeedforwardY);
        }

        /**
         * Modifies the DriveRequest parameter and returns itself.
         * <p>
         * The type of control request to use for the drive motor.
         *
         * @param newDriveRequest Parameter to modify
         * @return Itself
         */
        public ModuleRequest withDriveRequest(DriveRequestType newDriveRequest) {
            this.DriveRequest = newDriveRequest;
            return this;
        }

        /**
         * Modifies the SteerRequest parameter and returns itself.
         * <p>
         * The type of control request to use for the steer motor.
         *
         * @param newSteerRequest Parameter to modify
         * @return Itself
         */
        public ModuleRequest withSteerRequest(SteerRequestType newSteerRequest) {
            this.SteerRequest = newSteerRequest;
            return this;
        }

        /**
         * Modifies the UpdatePeriod parameter and returns itself.
         * <p>
         * The update period of the module request. Setting this to a
         * non-zero value adds a velocity feedforward to the steer motor.
         *
         * @param newUpdatePeriod Parameter to modify
         * @return Itself
         */
        public ModuleRequest withUpdatePeriod(double newUpdatePeriod) {
            this.UpdatePeriod = newUpdatePeriod;
            return this;
        }

        /**
         * Modifies the EnableFOC parameter and returns itself.
         * <p>
         * When using Voltage-based control, set to true (default) to use FOC commutation
         * (requires Phoenix Pro), which increases peak power by ~15%. Set to false to
         * use trapezoidal commutation. This is ignored when using Torque-based control,
         * which always uses FOC.
         * <p>
         * FOC improves motor performance by leveraging torque (current) control. 
         * However, this may be inconvenient for applications that require specifying
         * duty cycle or voltage.  CTR-Electronics has developed a hybrid method that
         * combines the performances gains of FOC while still allowing applications to
         * provide duty cycle or voltage demand.  This not to be confused with simple
         * sinusoidal control or phase voltage control which lacks the performance
         * gains.
         *
         * @param newEnableFOC Parameter to modify
         * @return Itself
         */
        public ModuleRequest withEnableFOC(boolean newEnableFOC) {
            this.EnableFOC = newEnableFOC;
            return this;
        }
    }

    /** Number of times to attempt config applies. */
    protected static final int kNumConfigAttempts = 2;

    /** ID of the native drivetrain instance, used for JNI calls. */
    protected final int m_drivetrainId;
    /** Index of this module in the native drivetrain, used for JNI calls. */
    protected final int m_moduleIdx;
    /** JNI instance to use for non-static JNI calls. This object is not thread-safe. */
    protected final SwerveJNI m_jni = new SwerveJNI();

    private final DriveMotorT m_driveMotor;
    private final SteerMotorT m_steerMotor;
    private final EncoderT m_encoder;

    private final ClosedLoopOutputType m_driveClosedLoopOutput;
    private final ClosedLoopOutputType m_steerClosedLoopOutput;

    private final SwerveModulePosition m_currentPosition = new SwerveModulePosition();
    private final SwerveModuleState m_currentState = new SwerveModuleState();
    private final SwerveModuleState m_targetState = new SwerveModuleState();
    /** Lock protecting the swerve module state. */
    private final Lock m_stateLock = new ReentrantLock();

    boolean m_isValid = true;

    /**
     * Construct a SwerveModule with the specified constants.
     *
     * @param driveMotorConstructor Constructor for the drive motor, such as {@code TalonFX::new}
     * @param steerMotorConstructor Constructor for the steer motor, such as {@code TalonFX::new}
     * @param encoderConstructor    Constructor for the azimuth encoder, such as {@code CANcoder::new}
     * @param constants             Constants used to construct the module
     * @param canbusName            The name of the CAN bus this module is on
     * @param drivetrainId          ID of the swerve drivetrain
     * @param index                 Index of this swerve module
     */
    public SwerveModule(
        DeviceConstructor<DriveMotorT> driveMotorConstructor,
        DeviceConstructor<SteerMotorT> steerMotorConstructor,
        DeviceConstructor<EncoderT> encoderConstructor,
        SwerveModuleConstants<?, ?, ?> constants,
        String canbusName,
        int drivetrainId,
        int index
    ) {
        m_drivetrainId = drivetrainId;
        m_moduleIdx = index;

        m_driveMotor = driveMotorConstructor.create(constants.DriveMotorId, canbusName);
        m_steerMotor = steerMotorConstructor.create(constants.SteerMotorId, canbusName);
        m_encoder = encoderConstructor.create(constants.EncoderId, canbusName);

        m_driveClosedLoopOutput = constants.DriveMotorClosedLoopOutput;
        m_steerClosedLoopOutput = constants.SteerMotorClosedLoopOutput;

        StatusCode response = StatusCode.OK;

        if (
            m_driveMotor instanceof TalonFX driveMotor &&
            (constants.DriveMotorInitialConfigs == null || constants.DriveMotorInitialConfigs instanceof TalonFXConfiguration)
        ) {
            TalonFXConfiguration driveConfigs = (TalonFXConfiguration)constants.DriveMotorInitialConfigs;
            if (driveConfigs == null) {
                driveConfigs = new TalonFXConfiguration();
            }
            
            driveConfigs.MotorOutput.NeutralMode = NeutralModeValue.Brake;

            driveConfigs.Slot0 = constants.DriveMotorGains;
            driveConfigs.TorqueCurrent.PeakForwardTorqueCurrent = constants.SlipCurrent;
            driveConfigs.TorqueCurrent.PeakReverseTorqueCurrent = -constants.SlipCurrent;
            driveConfigs.CurrentLimits.StatorCurrentLimit = constants.SlipCurrent;
            driveConfigs.CurrentLimits.StatorCurrentLimitEnable = true;

            if (constants.DriveMotorType != DriveMotorArrangement.TalonFX_Integrated) {
                System.out.println(
                    "Drive motor Talon FX ID " + driveMotor.getDeviceID() + " only supports TalonFX_Integrated."
                );
            }

            driveConfigs.MotorOutput.Inverted = constants.DriveMotorInverted
                    ? InvertedValue.Clockwise_Positive
                    : InvertedValue.CounterClockwise_Positive;
            for (int i = 0; i < kNumConfigAttempts; ++i) {
                response = driveMotor.getConfigurator().apply(driveConfigs);
                if (response.isOK()) break;
            }
            if (!response.isOK()) {
                System.out.println(
                    "Talon ID " + driveMotor.getDeviceID() + " failed config with error " + response.toString()
                );
            }
        } else if (
            m_driveMotor instanceof TalonFXS driveMotor &&
            (constants.DriveMotorInitialConfigs == null || constants.DriveMotorInitialConfigs instanceof TalonFXSConfiguration)
        ) {
            TalonFXSConfiguration driveConfigs = (TalonFXSConfiguration)constants.DriveMotorInitialConfigs;
            if (driveConfigs == null) {
                driveConfigs = new TalonFXSConfiguration();
            }
            
            driveConfigs.MotorOutput.NeutralMode = NeutralModeValue.Brake;

            driveConfigs.Slot0 = constants.DriveMotorGains;
            driveConfigs.CurrentLimits.StatorCurrentLimit = constants.SlipCurrent;
            driveConfigs.CurrentLimits.StatorCurrentLimitEnable = true;

            switch (constants.DriveMotorType) {
                case TalonFX_Integrated:
                    System.out.println(
                        "Cannot use TalonFX_Integrated drive motor type on Talon FXS ID " + driveMotor.getDeviceID() + ". TalonFX_Integrated is only supported on Talon FX."
                    );
                    break;

                case TalonFXS_NEO_JST:
                    driveConfigs.Commutation.MotorArrangement = MotorArrangementValue.NEO_JST;
                    break;
                case TalonFXS_VORTEX_JST:
                    driveConfigs.Commutation.MotorArrangement = MotorArrangementValue.VORTEX_JST;
                    break;
            }

            driveConfigs.MotorOutput.Inverted = constants.DriveMotorInverted
                    ? InvertedValue.Clockwise_Positive
                    : InvertedValue.CounterClockwise_Positive;
            for (int i = 0; i < kNumConfigAttempts; ++i) {
                response = driveMotor.getConfigurator().apply(driveConfigs);
                if (response.isOK()) break;
            }
            if (!response.isOK()) {
                System.out.println(
                    "Talon ID " + driveMotor.getDeviceID() + " failed config with error " + response.toString()
                );
            }
        } else {
            DriverStation.reportError(
                "[phoenix] SwerveDrivetrain drive motor type does not match the SwerveModuleConstants drive initial configs type. Ensure that the SwerveDrivetrain and SwerveModuleConstants instances are both constructed with the same drive motor type (TalonFX/TalonFXConfiguration, TalonFXS/TalonFXSConfiguration, etc.).",
                true
            );
            m_isValid = false;
        }

        if (
            m_steerMotor instanceof TalonFX steerMotor &&
            (constants.SteerMotorInitialConfigs == null || constants.SteerMotorInitialConfigs instanceof TalonFXConfiguration)
        ) {
            TalonFXConfiguration steerConfigs = (TalonFXConfiguration)constants.SteerMotorInitialConfigs;
            if (steerConfigs == null) {
                steerConfigs = new TalonFXConfiguration();
            }

            steerConfigs.MotorOutput.NeutralMode = NeutralModeValue.Brake;

            steerConfigs.Slot0 = constants.SteerMotorGains;

            if (constants.SteerMotorType != SteerMotorArrangement.TalonFX_Integrated) {
                System.out.println(
                    "Steer motor Talon FX ID " + steerMotor.getDeviceID() + " only supports TalonFX_Integrated."
                );
            }

            /* Modify configuration to use remote encoder setting */
            steerConfigs.Feedback.FeedbackRemoteSensorID = constants.EncoderId;
            switch (constants.FeedbackSource) {
                case FusedCANcoder:
                    steerConfigs.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder;
                    break;
                case SyncCANcoder:
                    steerConfigs.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.SyncCANcoder;
                    break;
                case RemoteCANcoder:
                    steerConfigs.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder;
                    break;

                case FusedCANdiPWM1:
                    steerConfigs.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANdiPWM1;
                    break;
                case FusedCANdiPWM2:
                    steerConfigs.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANdiPWM2;
                    break;
                case SyncCANdiPWM1:
                    steerConfigs.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.SyncCANdiPWM1;
                    break;
                case SyncCANdiPWM2:
                    steerConfigs.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.SyncCANdiPWM2;
                    break;
                case RemoteCANdiPWM1:
                    steerConfigs.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANdiPWM1;
                    break;
                case RemoteCANdiPWM2:
                    steerConfigs.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANdiPWM2;
                    break;

                case TalonFXS_PulseWidth:
                    System.out.println(
                        "Cannot use Pulse Width steer feedback type on Talon FX ID " + steerMotor.getDeviceID() + ". Pulse Width is only supported on Talon FXS."
                    );
                    break;
            }
            steerConfigs.Feedback.RotorToSensorRatio = constants.SteerMotorGearRatio;
    
            steerConfigs.MotionMagic.MotionMagicExpo_kV = 0.12 * constants.SteerMotorGearRatio;
            steerConfigs.MotionMagic.MotionMagicExpo_kA = 0.8 / constants.SteerMotorGearRatio;
    
            steerConfigs.ClosedLoopGeneral.ContinuousWrap = true; // Enable continuous wrap for swerve modules
    
            steerConfigs.MotorOutput.Inverted = constants.SteerMotorInverted
                    ? InvertedValue.Clockwise_Positive
                    : InvertedValue.CounterClockwise_Positive;
            for (int i = 0; i < kNumConfigAttempts; ++i) {
                response = steerMotor.getConfigurator().apply(steerConfigs);
                if (response.isOK()) break;
            }
            if (!response.isOK()) {
                System.out.println(
                    "Talon ID " + steerMotor.getDeviceID() + " failed config with error: " + response.toString()
                );
            }
        } else if (
            m_steerMotor instanceof TalonFXS steerMotor &&
            (constants.SteerMotorInitialConfigs == null || constants.SteerMotorInitialConfigs instanceof TalonFXSConfiguration)
        ) {
            TalonFXSConfiguration steerConfigs = (TalonFXSConfiguration)constants.SteerMotorInitialConfigs;
            if (steerConfigs == null) {
                steerConfigs = new TalonFXSConfiguration();
            }

            steerConfigs.MotorOutput.NeutralMode = NeutralModeValue.Brake;

            steerConfigs.Slot0 = constants.SteerMotorGains;

            switch (constants.SteerMotorType) {
                case TalonFX_Integrated:
                    System.out.println(
                        "Cannot use TalonFX_Integrated steer motor type on Talon FXS ID " + steerMotor.getDeviceID() + ". TalonFX_Integrated is only supported on Talon FX."
                    );
                    break;

                case TalonFXS_Minion_JST:
                    steerConfigs.Commutation.MotorArrangement = MotorArrangementValue.Minion_JST;
                    break;
                case TalonFXS_NEO_JST:
                    steerConfigs.Commutation.MotorArrangement = MotorArrangementValue.NEO_JST;
                    break;
                case TalonFXS_VORTEX_JST:
                    steerConfigs.Commutation.MotorArrangement = MotorArrangementValue.VORTEX_JST;
                    break;
                case TalonFXS_NEO550_JST:
                    steerConfigs.Commutation.MotorArrangement = MotorArrangementValue.NEO550_JST;
                    break;
                case TalonFXS_Brushed_AB:
                    steerConfigs.Commutation.MotorArrangement = MotorArrangementValue.Brushed_DC;
                    steerConfigs.Commutation.BrushedMotorWiring = BrushedMotorWiringValue.Leads_A_and_B;
                    break;
                case TalonFXS_Brushed_AC:
                    steerConfigs.Commutation.MotorArrangement = MotorArrangementValue.Brushed_DC;
                    steerConfigs.Commutation.BrushedMotorWiring = BrushedMotorWiringValue.Leads_A_and_C;
                    break;
                case TalonFXS_Brushed_BC:
                    steerConfigs.Commutation.MotorArrangement = MotorArrangementValue.Brushed_DC;
                    steerConfigs.Commutation.BrushedMotorWiring = BrushedMotorWiringValue.Leads_B_and_C;
                    break;
            }

            /* Modify configuration to use remote encoder setting */
            steerConfigs.ExternalFeedback.FeedbackRemoteSensorID = constants.EncoderId;
            switch (constants.FeedbackSource) {
                case FusedCANcoder:
                    steerConfigs.ExternalFeedback.ExternalFeedbackSensorSource = ExternalFeedbackSensorSourceValue.FusedCANcoder;
                    break;
                case SyncCANcoder:
                    steerConfigs.ExternalFeedback.ExternalFeedbackSensorSource = ExternalFeedbackSensorSourceValue.SyncCANcoder;
                    break;
                case RemoteCANcoder:
                    steerConfigs.ExternalFeedback.ExternalFeedbackSensorSource = ExternalFeedbackSensorSourceValue.RemoteCANcoder;
                    break;

                case FusedCANdiPWM1:
                    steerConfigs.ExternalFeedback.ExternalFeedbackSensorSource = ExternalFeedbackSensorSourceValue.FusedCANdiPWM1;
                    break;
                case FusedCANdiPWM2:
                    steerConfigs.ExternalFeedback.ExternalFeedbackSensorSource = ExternalFeedbackSensorSourceValue.FusedCANdiPWM2;
                    break;
                case SyncCANdiPWM1:
                    steerConfigs.ExternalFeedback.ExternalFeedbackSensorSource = ExternalFeedbackSensorSourceValue.SyncCANdiPWM1;
                    break;
                case SyncCANdiPWM2:
                    steerConfigs.ExternalFeedback.ExternalFeedbackSensorSource = ExternalFeedbackSensorSourceValue.SyncCANdiPWM2;
                    break;
                case RemoteCANdiPWM1:
                    steerConfigs.ExternalFeedback.ExternalFeedbackSensorSource = ExternalFeedbackSensorSourceValue.RemoteCANdiPWM1;
                    break;
                case RemoteCANdiPWM2:
                    steerConfigs.ExternalFeedback.ExternalFeedbackSensorSource = ExternalFeedbackSensorSourceValue.RemoteCANdiPWM2;
                    break;

                case TalonFXS_PulseWidth:
                    steerConfigs.ExternalFeedback.ExternalFeedbackSensorSource = ExternalFeedbackSensorSourceValue.PulseWidth;
                    steerConfigs.ExternalFeedback.AbsoluteSensorOffset = constants.EncoderOffset;
                    steerConfigs.ExternalFeedback.SensorPhase = constants.EncoderInverted
                        ? SensorPhaseValue.Opposed
                        : SensorPhaseValue.Aligned;
                    break;
            }
            steerConfigs.ExternalFeedback.RotorToSensorRatio = constants.SteerMotorGearRatio;
    
            steerConfigs.MotionMagic.MotionMagicExpo_kV = 0.12 * constants.SteerMotorGearRatio;
            steerConfigs.MotionMagic.MotionMagicExpo_kA = 0.8 / constants.SteerMotorGearRatio;
    
            steerConfigs.ClosedLoopGeneral.ContinuousWrap = true; // Enable continuous wrap for swerve modules
    
            steerConfigs.MotorOutput.Inverted = constants.SteerMotorInverted
                    ? InvertedValue.Clockwise_Positive
                    : InvertedValue.CounterClockwise_Positive;
            for (int i = 0; i < kNumConfigAttempts; ++i) {
                response = steerMotor.getConfigurator().apply(steerConfigs);
                if (response.isOK()) break;
            }
            if (!response.isOK()) {
                System.out.println(
                    "Talon ID " + steerMotor.getDeviceID() + " failed config with error: " + response.toString()
                );
            }
        } else {
            DriverStation.reportError(
                "[phoenix] SwerveDrivetrain steer motor type does not match the SwerveModuleConstants steer initial configs type. Ensure that the SwerveDrivetrain and SwerveModuleConstants instances are both constructed with the same steer motor type (TalonFX/TalonFXConfiguration, TalonFXS/TalonFXSConfiguration, etc.).",
                true
            );
            m_isValid = false;
        }

        if (
            m_encoder instanceof CANcoder encoder &&
            (constants.EncoderInitialConfigs == null || constants.EncoderInitialConfigs instanceof CANcoderConfiguration)
        ) {
            CANcoderConfiguration encoderConfigs = (CANcoderConfiguration)constants.EncoderInitialConfigs;
            if (encoderConfigs == null) {
                encoderConfigs = new CANcoderConfiguration();
            }

            encoderConfigs.MagnetSensor.MagnetOffset = constants.EncoderOffset;
            encoderConfigs.MagnetSensor.SensorDirection = constants.EncoderInverted
                    ? SensorDirectionValue.Clockwise_Positive
                    : SensorDirectionValue.CounterClockwise_Positive;
            for (int i = 0; i < kNumConfigAttempts; ++i) {
                response = encoder.getConfigurator().apply(encoderConfigs);
                if (response.isOK()) break;
            }
            if (!response.isOK()) {
                System.out.println(
                    "Encoder ID " + encoder.getDeviceID() + " failed config with error: " + response.toString()
                );
            }
        } else if (
            m_encoder instanceof CANdi encoder &&
            (constants.EncoderInitialConfigs == null || constants.EncoderInitialConfigs instanceof CANdiConfiguration)
        ) {
            CANdiConfiguration encoderConfigs = (CANdiConfiguration)constants.EncoderInitialConfigs;
            if (encoderConfigs == null) {
                encoderConfigs = new CANdiConfiguration();
            }

            for (int i = 0; i < kNumConfigAttempts; ++i) {
                response = encoder.getConfigurator().apply(encoderConfigs.DigitalInputs);
                if (response.isOK()) break;
            }
            if (!response.isOK()) {
                System.out.println(
                    "Encoder ID " + encoder.getDeviceID() + " failed config with error: " + response.toString()
                );
            }

            for (int i = 0; i < kNumConfigAttempts; ++i) {
                response = encoder.getConfigurator().apply(encoderConfigs.CustomParams);
                if (response.isOK()) break;
            }
            if (!response.isOK()) {
                System.out.println(
                    "Encoder ID " + encoder.getDeviceID() + " failed config with error: " + response.toString()
                );
            }

            switch (constants.FeedbackSource) {
                case FusedCANdiPWM1:
                case SyncCANdiPWM1:
                case RemoteCANdiPWM1:
                    encoderConfigs.PWM1.AbsoluteSensorOffset = constants.EncoderOffset;
                    encoderConfigs.PWM1.SensorDirection = constants.EncoderInverted;

                    for (int i = 0; i < kNumConfigAttempts; ++i) {
                        response = encoder.getConfigurator().apply(encoderConfigs.PWM1);
                        if (response.isOK()) break;
                    }
                    if (!response.isOK()) {
                        System.out.println(
                            "Encoder ID " + encoder.getDeviceID() + " failed config with error: " + response.toString()
                        );
                    }
                    break;

                case FusedCANdiPWM2:
                case SyncCANdiPWM2:
                case RemoteCANdiPWM2:
                    encoderConfigs.PWM2.AbsoluteSensorOffset = constants.EncoderOffset;
                    encoderConfigs.PWM2.SensorDirection = constants.EncoderInverted;

                    for (int i = 0; i < kNumConfigAttempts; ++i) {
                        response = encoder.getConfigurator().apply(encoderConfigs.PWM2);
                        if (response.isOK()) break;
                    }
                    if (!response.isOK()) {
                        System.out.println(
                            "Encoder ID " + encoder.getDeviceID() + " failed config with error: " + response.toString()
                        );
                    }
                    break;

                default:
                    break;
            }
        } else if (
            m_encoder instanceof TalonFXS &&
            (constants.EncoderInitialConfigs == null || constants.EncoderInitialConfigs instanceof TalonFXSConfiguration)
        ) {
            /* do nothing if using encoder on a Talon FXS, configs are applied when initializing the steer motor */
        } else {
            DriverStation.reportError(
                "[phoenix] SwerveDrivetrain encoder type does not match the SwerveModuleConstants encoder initial configs type. Ensure that the SwerveDrivetrain and SwerveModuleConstants instances are both constructed with the same encoder type (CANcoder/CANcoderConfiguration, CANdi/CANdiConfiguration, etc.).",
                true
            );
            m_isValid = false;
        }
    }

    /**
     * Applies the desired SwerveModuleState to this module.
     *
     * @param moduleRequest The request to apply to this module
     */
    public void apply(ModuleRequest moduleRequest) {
        m_jni.moduleApplyParams.state.speed = moduleRequest.State.speedMetersPerSecond;
        m_jni.moduleApplyParams.state.angle = moduleRequest.State.angle.getRadians();
        m_jni.moduleApplyParams.wheelForceFeedforwardX = moduleRequest.WheelForceFeedforwardX;
        m_jni.moduleApplyParams.wheelForceFeedforwardY = moduleRequest.WheelForceFeedforwardY;
        m_jni.moduleApplyParams.driveRequest = moduleRequest.DriveRequest.value;
        m_jni.moduleApplyParams.steerRequest = moduleRequest.SteerRequest.value;
        m_jni.moduleApplyParams.updatePeriod = moduleRequest.UpdatePeriod;
        m_jni.moduleApplyParams.enableFOC = moduleRequest.EnableFOC;

        m_jni.JNI_Module_Apply(m_drivetrainId, m_moduleIdx);
    }

    /**
     * Controls this module using the specified drive and steer control requests.
     *
     * This is intended only to be used for characterization of the robot; do not use this for normal use.
     *
     * @param driveRequest The control request to apply to the drive motor
     * @param steerRequest The control request to apply to the steer motor
     */
    public void apply(ControlRequest driveRequest, ControlRequest steerRequest) {
        m_driveMotor.setControl(driveRequest.withUpdateFreqHz(0));
        m_steerMotor.setControl(steerRequest.withUpdateFreqHz(0));
    }

    /**
     * Gets the state of this module and passes it back as a
     * SwerveModulePosition object with latency compensated values.
     * <p>
     * This function is blocking when it performs a refresh.
     *
     * @param refresh True if the signals should be refreshed
     * @return SwerveModulePosition containing this module's state
     */
    public final SwerveModulePosition getPosition(boolean refresh) {
        try {
            m_stateLock.lock();

            m_jni.JNI_Module_GetPosition(m_drivetrainId, m_moduleIdx, refresh);

            m_currentPosition.distanceMeters = m_jni.modulePosition.distance;
            if (m_currentPosition.angle.getRadians() != m_jni.modulePosition.angle) {
                m_currentPosition.angle = Rotation2d.fromRadians(m_jni.modulePosition.angle);
            }
            return m_currentPosition;
        } finally {
            m_stateLock.unlock();
        }
    }

    /**
     * Gets the last cached swerve module position.
     * This differs from {@link getPosition} in that it will not
     * perform any latency compensation or refresh the signals.
     *
     * @return Last cached SwerveModulePosition
     */
    public final SwerveModulePosition getCachedPosition() {
        try {
            m_stateLock.lock();

            m_jni.JNI_Module_GetCachedPosition(m_drivetrainId, m_moduleIdx);

            m_currentPosition.distanceMeters = m_jni.modulePosition.distance;
            if (m_currentPosition.angle.getRadians() != m_jni.modulePosition.angle) {
                m_currentPosition.angle = Rotation2d.fromRadians(m_jni.modulePosition.angle);
            }
            return m_currentPosition;
        } finally {
            m_stateLock.unlock();
        }
    }

    /**
     * Get the current state of the module.
     * <p>
     * This is typically used for telemetry, as the SwerveModulePosition
     * is used for odometry.
     *
     * @return Current state of the module
     */
    public final SwerveModuleState getCurrentState() {
        try {
            m_stateLock.lock();

            m_jni.JNI_Module_GetCurrentState(m_drivetrainId, m_moduleIdx);

            m_currentState.speedMetersPerSecond = m_jni.moduleState.speed;
            if (m_currentState.angle.getRadians() != m_jni.moduleState.angle) {
                m_currentState.angle = Rotation2d.fromRadians(m_jni.moduleState.angle);
            }
            return m_currentState;
        } finally {
            m_stateLock.unlock();
        }
    }

    /**
     * Get the target state of the module.
     * <p>
     * This is typically used for telemetry.
     *
     * @return Target state of the module
     */
    public final SwerveModuleState getTargetState() {
        try {
            m_stateLock.lock();

            m_jni.JNI_Module_GetTargetState(m_drivetrainId, m_moduleIdx);

            m_targetState.speedMetersPerSecond = m_jni.moduleState.speed;
            if (m_targetState.angle.getRadians() != m_jni.moduleState.angle) {
                m_targetState.angle = Rotation2d.fromRadians(m_jni.moduleState.angle);
            }
            return m_targetState;
        } finally {
            m_stateLock.unlock();
        }
    }

    /**
     * Resets this module's drive motor position to 0 rotations.
     */
    public void resetPosition() {
        SwerveJNI.JNI_Module_ResetPosition(m_drivetrainId, m_moduleIdx);
    }

    /**
     * Gets the closed-loop output type to use for the drive motor.
     *
     * @return Drive motor closed-loop output type
     */
    public final ClosedLoopOutputType getDriveClosedLoopOutputType() {
        return m_driveClosedLoopOutput;
    }

    /**
     * Gets the closed-loop output type to use for the steer motor.
     *
     * @return Steer motor closed-loop output type
     */
    public final ClosedLoopOutputType getSteerClosedLoopOutputType() {
        return m_steerClosedLoopOutput;
    }

    /**
     * Gets this module's Drive Motor reference.
     * <p>
     * This should be used only to access signals and change configurations that the
     * swerve drivetrain does not configure itself.
     *
     * @return This module's Drive Motor reference
     */
    public final DriveMotorT getDriveMotor() {
        return m_driveMotor;
    }

    /**
     * Gets this module's Steer Motor reference.
     * <p>
     * This should be used only to access signals and change configurations that the
     * swerve drivetrain does not configure itself.
     *
     * @return This module's Steer Motor reference
     */
    public final SteerMotorT getSteerMotor() {
        return m_steerMotor;
    }

    /**
     * Gets this module's azimuth encoder reference.
     * <p>
     * This should be used only to access signals and change configurations that the
     * swerve drivetrain does not configure itself.
     *
     * @return This module's azimuth encoder reference
     */
    public final EncoderT getEncoder() {
        return m_encoder;
    }
}