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

import java.util.Optional;
import java.util.OptionalInt;
import java.util.concurrent.atomic.AtomicBoolean;
import java.util.function.BooleanSupplier;

import edu.wpi.first.units.measure.*;
import edu.wpi.first.wpilibj.DriverStation;

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

import com.ctre.phoenix6.BaseStatusSignal;
import com.ctre.phoenix6.CANBus;
import com.ctre.phoenix6.StatusCode;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.DifferentialSensorsConfigs;
import com.ctre.phoenix6.configs.MotorOutputConfigs;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.configs.TalonFXSConfiguration;
import com.ctre.phoenix6.controls.DifferentialFollower;
import com.ctre.phoenix6.controls.NeutralOut;
import com.ctre.phoenix6.controls.CoastOut;
import com.ctre.phoenix6.controls.StaticBrake;
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.hardware.CANdi;
import com.ctre.phoenix6.hardware.Pigeon2;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.hardware.TalonFXS;
import com.ctre.phoenix6.hardware.traits.CommonTalon;
import com.ctre.phoenix6.mechanisms.DifferentialMotorConstants.*;
import com.ctre.phoenix6.signals.DifferentialSensorSourceValue;
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.MotorAlignmentValue;
import com.ctre.phoenix6.signals.NeutralModeValue;

import com.ctre.phoenix6.controls.DifferentialDutyCycle;
import com.ctre.phoenix6.controls.DifferentialMotionMagicDutyCycle;
import com.ctre.phoenix6.controls.DifferentialMotionMagicExpoDutyCycle;
import com.ctre.phoenix6.controls.DifferentialMotionMagicExpoVoltage;
import com.ctre.phoenix6.controls.DifferentialMotionMagicVelocityDutyCycle;
import com.ctre.phoenix6.controls.DifferentialMotionMagicVelocityVoltage;
import com.ctre.phoenix6.controls.DifferentialMotionMagicVoltage;
import com.ctre.phoenix6.controls.DifferentialPositionDutyCycle;
import com.ctre.phoenix6.controls.DifferentialPositionVoltage;
import com.ctre.phoenix6.controls.DifferentialVelocityDutyCycle;
import com.ctre.phoenix6.controls.DifferentialVelocityVoltage;
import com.ctre.phoenix6.controls.DifferentialVoltage;

/**
 * Manages control of a simple two-axis differential mechanism.
 * <p>
 * This mechanism provides limited differential functionality. Pro users
 * on a CAN FD bus can use the {@link DifferentialMechanism} instead
 * for full functionality.
 * <p>
 * A differential mechanism has two axes of motion, where the position
 * along each axis is determined by two motors in separate gearboxes:
 * <ul>
 *   <li>Driving both motors in a common direction causes the mechanism
 *       to move forward/reverse, up/down, etc.
 *   <ul>
 *     <li>This is the Average axis: position is determined by the
 *         average of the two motors' positions.
 *   </ul>
 *   <li>Driving the motors in opposing directions causes the mechanism
 *       to twist or rotate left/right.
 *   <ul>
 *     <li>This is the Difference axis: rotation is determined by half
 *         the difference of the two motors' positions.
 *   </ul>
 * </ul>
 */
public class SimpleDifferentialMechanism<MotorT extends CommonTalon> {
    /**
     * Functional interface for device constructors.
     */
    @FunctionalInterface
    public static interface DeviceConstructor<DeviceT> {
        DeviceT create(int id, CANBus canbus);
    }

    /**
     * Possible reasons for the mechanism to disable.
     */
    public enum DisabledReasonValue {
        /**
         * No reason given.
         */
        None,
        /**
         * A remote sensor is not present on CAN Bus.
         */
        MissingRemoteSensor,
        /**
         * The remote Talon FX used for differential
         * control is not present on CAN Bus.
         */
        MissingDifferentialFX,
        /**
         * A remote sensor position has overflowed. Because of the nature
         * of remote sensors, it is possible for a remote sensor position
         * to overflow beyond what is supported by the status signal frame.
         * However, this is rare and cannot occur over the course of an FRC
         * match under normal use.
         */
        RemoteSensorPosOverflow,
        /**
         * A device or remote sensor has reset.
         */
        DeviceHasReset,
    }

    /**
     * Possible reasons for the mechanism to require
     * user action to resume control.
     */
    public enum RequiresUserReasonValue {
        /**
         * No reason given.
         */
        None,
        /**
         * A remote sensor position has overflowed. Because of the nature
         * of remote sensors, it is possible for a remote sensor position
         * to overflow beyond what is supported by the status signal frame.
         * However, this is rare and cannot occur over the course of an FRC
         * match under normal use.
         */
        RemoteSensorPosOverflow,
        /**
         * A device or remote sensor has reset.
         */
        DeviceHasReset,
    }

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

    private final MotorT _diffLeaderFX;
    private final MotorT _diffFollowerFX;

    private final double _sensorToDiffRatio;

    private final BooleanSupplier _diffLeaderFXResetChecker;
    private final BooleanSupplier _diffFollowerFXResetChecker;
    private final Optional<BooleanSupplier> _diffSensorResetChecker;

    private final DifferentialFollower _diffFollow;

    private final NeutralOut _neutral = new NeutralOut();
    private final CoastOut _coast = new CoastOut();
    private final StaticBrake _brake = new StaticBrake();

    private final AtomicBoolean _mechanismDisabled = new AtomicBoolean(false);
    private final AtomicBoolean _requiresUserAction = new AtomicBoolean(false);

    private DisabledReasonValue _disabledReason = DisabledReasonValue.None;
    private RequiresUserReasonValue _requiresUserReason = RequiresUserReasonValue.None;

    /**
     * Creates a new simple differential mechanism using two {@link CommonTalon} devices.
     * The mechanism will use the average of the two Talon FX sensors on the primary axis,
     * and half of the difference between the two Talon FX sensors on the differential axis.
     * <p>
     * This mechanism provides limited differential functionality. Pro users on a CAN FD
     * bus can use the {@link DifferentialMechanism} class instead for full functionality.
     *
     * @param motorConstructor Constructor for the motors, such as {@code TalonFX::new}
     * @param constants Constants used to construct the mechanism
     */
    public SimpleDifferentialMechanism(DeviceConstructor<MotorT> motorConstructor, DifferentialMotorConstants<?> constants) {
        final var canbus = new CANBus(constants.CANBusName);
        _diffLeaderFX = motorConstructor.create(constants.LeaderId, canbus);
        _diffFollowerFX = motorConstructor.create(constants.FollowerId, canbus);
        _sensorToDiffRatio = constants.SensorToDifferentialRatio;
        _diffLeaderFXResetChecker = _diffLeaderFX.getResetOccurredChecker();
        _diffFollowerFXResetChecker = _diffFollowerFX.getResetOccurredChecker();
        _diffFollow = new DifferentialFollower(_diffLeaderFX.getDeviceID(), constants.Alignment);
        _diffSensorResetChecker = Optional.empty();

        applyConfigs(constants, DifferentialSensorSourceValue.RemoteTalonFX_HalfDiff, OptionalInt.empty());
        /* Set update frequency of relevant signals */
        BaseStatusSignal.setUpdateFrequencyForAll(
            constants.ClosedLoopRate,
            _diffLeaderFX.getDifferentialOutput(false),
            _diffFollowerFX.getPosition(false),
            _diffFollowerFX.getVelocity(false)
        );
    }

    /**
     * Creates a new simple differential mechanism using two {@link CommonTalon} devices and
     * a {@link Pigeon2}. The mechanism will use the average of the two Talon FX sensors on the
     * primary axis, and the selected Pigeon 2 sensor source on the differential axis.
     * <p>
     * This mechanism provides limited differential functionality. Pro users on a CAN FD
     * bus can use the {@link DifferentialMechanism} class instead for full functionality.
     *
     * @param motorConstructor Constructor for the motors, such as {@code TalonFX::new}
     * @param constants Constants used to construct the mechanism
     * @param pigeon2 The Pigeon 2 to use for the differential axis
     * @param pigeonSource The sensor source to use for the Pigeon 2 (Yaw, Pitch, or Roll)
     */
    public SimpleDifferentialMechanism(DeviceConstructor<MotorT> motorConstructor, DifferentialMotorConstants<?> constants, Pigeon2 pigeon2, DifferentialPigeon2Source pigeonSource) {
        final var canbus = new CANBus(constants.CANBusName);
        _diffLeaderFX = motorConstructor.create(constants.LeaderId, canbus);
        _diffFollowerFX = motorConstructor.create(constants.FollowerId, canbus);
        _sensorToDiffRatio = constants.SensorToDifferentialRatio;
        _diffLeaderFXResetChecker = _diffLeaderFX.getResetOccurredChecker();
        _diffFollowerFXResetChecker = _diffFollowerFX.getResetOccurredChecker();
        _diffSensorResetChecker = Optional.of(pigeon2.getResetOccurredChecker());
        _diffFollow = new DifferentialFollower(_diffLeaderFX.getDeviceID(), constants.Alignment);

        DifferentialSensorSourceValue diffSensorSource;
        switch (pigeonSource) {
            case Yaw:
            default:
                diffSensorSource = DifferentialSensorSourceValue.RemotePigeon2Yaw;
                break;
            case Pitch:
                diffSensorSource = DifferentialSensorSourceValue.RemotePigeon2Pitch;
                break;
            case Roll:
                diffSensorSource = DifferentialSensorSourceValue.RemotePigeon2Roll;
                break;
        }
        applyConfigs(constants, diffSensorSource, OptionalInt.of(pigeon2.getDeviceID()));

        /* Set update frequency of relevant signals */
        BaseStatusSignal.setUpdateFrequencyForAll(
            constants.ClosedLoopRate,
            _diffLeaderFX.getDifferentialOutput(false),
            _diffFollowerFX.getPosition(false),
            _diffFollowerFX.getVelocity(false)
        );
        switch (pigeonSource) {
            case Yaw:
                pigeon2.getYaw(false).setUpdateFrequency(constants.ClosedLoopRate);
                break;
            case Pitch:
                pigeon2.getPitch(false).setUpdateFrequency(constants.ClosedLoopRate);
                break;
            case Roll:
                pigeon2.getRoll(false).setUpdateFrequency(constants.ClosedLoopRate);
                break;
        }
    }

    /**
     * Creates a new simple differential mechanism using two {@link CommonTalon} devices and
     * a {@link CANcoder}. The mechanism will use the average of the two Talon FX sensors on the
     * primary axis, and the CANcoder position/velocity on the differential axis.
     * <p>
     * This mechanism provides limited differential functionality. Pro users on a CAN FD
     * bus can use the {@link DifferentialMechanism} class instead for full functionality.
     *
     * @param motorConstructor Constructor for the motors, such as {@code TalonFX::new}
     * @param constants Constants used to construct the mechanism
     * @param cancoder The CANcoder to use for the differential axis
     */
    public SimpleDifferentialMechanism(DeviceConstructor<MotorT> motorConstructor, DifferentialMotorConstants<?> constants, CANcoder cancoder) {
        final var canbus = new CANBus(constants.CANBusName);
        _diffLeaderFX = motorConstructor.create(constants.LeaderId, canbus);
        _diffFollowerFX = motorConstructor.create(constants.FollowerId, canbus);
        _sensorToDiffRatio = constants.SensorToDifferentialRatio;
        _diffLeaderFXResetChecker = _diffLeaderFX.getResetOccurredChecker();
        _diffFollowerFXResetChecker = _diffFollowerFX.getResetOccurredChecker();
        _diffSensorResetChecker = Optional.of(cancoder.getResetOccurredChecker());
        _diffFollow = new DifferentialFollower(_diffLeaderFX.getDeviceID(), constants.Alignment);

        applyConfigs(constants, DifferentialSensorSourceValue.RemoteCANcoder, OptionalInt.of(cancoder.getDeviceID()));
        /* Set update frequency of relevant signals */
        BaseStatusSignal.setUpdateFrequencyForAll(
            constants.ClosedLoopRate,
            _diffLeaderFX.getDifferentialOutput(false),
            _diffFollowerFX.getPosition(false),
            _diffFollowerFX.getVelocity(false),
            cancoder.getPosition(false),
            cancoder.getVelocity(false)
        );
    }

    /**
     * Creates a new simple differential mechanism using two hardware#traits#CommonTalon devices and a
     * {@link CANdi}. The mechanism will use the average of the two Talon FX sensors on the primary
     * axis, and the selected CTR Electronics' CANdi™ branded sensor source on the differential axis.
     * <p>
     * This mechanism provides limited differential functionality. Pro users on a CAN FD
     * bus can use the {@link DifferentialMechanism} class instead for full functionality.
     *
     * @param motorConstructor Constructor for the motors, such as {@code TalonFX::new}
     * @param constants Constants used to construct the mechanism
     * @param candi The CTR Electronics' CANdi™ branded device to use for the differential axis
     * @param candiSource The sensor source to use for the CTR Electronics' CANdi™ branded device
     */
    public SimpleDifferentialMechanism(DeviceConstructor<MotorT> motorConstructor, DifferentialMotorConstants<?> constants, CANdi candi, DifferentialCANdiSource candiSource) {
        final var canbus = new CANBus(constants.CANBusName);
        _diffLeaderFX = motorConstructor.create(constants.LeaderId, canbus);
        _diffFollowerFX = motorConstructor.create(constants.FollowerId, canbus);
        _sensorToDiffRatio = constants.SensorToDifferentialRatio;
        _diffLeaderFXResetChecker = _diffLeaderFX.getResetOccurredChecker();
        _diffFollowerFXResetChecker = _diffFollowerFX.getResetOccurredChecker();
        _diffSensorResetChecker = Optional.of(candi.getResetOccurredChecker());
        _diffFollow = new DifferentialFollower(_diffLeaderFX.getDeviceID(), constants.Alignment);

        DifferentialSensorSourceValue diffSensorSource;
        switch (candiSource) {
            case PWM1:
            default:
                diffSensorSource = DifferentialSensorSourceValue.RemoteCANdiPWM1;
                break;
            case PWM2:
                diffSensorSource = DifferentialSensorSourceValue.RemoteCANdiPWM2;
                break;
            case Quadrature:
                diffSensorSource = DifferentialSensorSourceValue.RemoteCANdiQuadrature;
                break;
        }
        applyConfigs(constants, diffSensorSource, OptionalInt.of(candi.getDeviceID()));

        /* Set update frequency of relevant signals */
        BaseStatusSignal.setUpdateFrequencyForAll(
            constants.ClosedLoopRate,
            _diffLeaderFX.getDifferentialOutput(false),
            _diffFollowerFX.getPosition(false),
            _diffFollowerFX.getVelocity(false)
        );
        switch (candiSource) {
            case PWM1:
                BaseStatusSignal.setUpdateFrequencyForAll(
                    constants.ClosedLoopRate,
                    candi.getPWM1Position(false),
                    candi.getPWM1Velocity(false)
                );
                break;
            case PWM2:
                BaseStatusSignal.setUpdateFrequencyForAll(
                    constants.ClosedLoopRate,
                    candi.getPWM2Position(false),
                    candi.getPWM2Velocity(false)
                );
                break;
            case Quadrature:
                BaseStatusSignal.setUpdateFrequencyForAll(
                    constants.ClosedLoopRate,
                    candi.getQuadraturePosition(false),
                    candi.getQuadratureVelocity(false)
                );
                break;
        }
    }

    private void applyConfigs(DifferentialMotorConstants<?> constants, DifferentialSensorSourceValue diffSensorSource, OptionalInt diffSensorID) {
        /*
         * The onboard differential controller adds the differential output to its own output
         * and subtracts the differential output for differential followers, so use _diffLeaderFX
         * as the primary controller.
         */

        InvertedValue leaderInvert;

        /* set up differential control on _diffLeaderFX */
        {
            var diff_cfg = new DifferentialSensorsConfigs();

            diff_cfg.DifferentialTalonFXSensorID = _diffFollowerFX.getDeviceID();
            diff_cfg.DifferentialSensorSource = diffSensorSource;
            diffSensorID.ifPresent(id -> diff_cfg.DifferentialRemoteSensorID = id);
            diff_cfg.SensorToDifferentialRatio = constants.SensorToDifferentialRatio;

            StatusCode response = StatusCode.OK;

            if (
                _diffLeaderFX instanceof TalonFX diffLeaderFX &&
                (constants.LeaderInitialConfigs == null || constants.LeaderInitialConfigs instanceof TalonFXConfiguration)
            ) {
                var leaderConfigs = (TalonFXConfiguration)constants.LeaderInitialConfigs;
                if (leaderConfigs == null) {
                    leaderConfigs = new TalonFXConfiguration();
                }
                leaderInvert = leaderConfigs.MotorOutput.Inverted;

                leaderConfigs.DifferentialSensors = diff_cfg;
                for (int i = 0; i < kNumConfigAttempts; ++i) {
                    response = diffLeaderFX.getConfigurator().apply(leaderConfigs);
                    if (response.isOK()) break;
                }
            } else if (
                _diffLeaderFX instanceof TalonFXS diffLeaderFXS &&
                (constants.LeaderInitialConfigs == null || constants.LeaderInitialConfigs instanceof TalonFXSConfiguration)
            ) {
                var leaderConfigs = (TalonFXSConfiguration)constants.LeaderInitialConfigs;
                if (leaderConfigs == null) {
                    leaderConfigs = new TalonFXSConfiguration();
                }
                leaderInvert = leaderConfigs.MotorOutput.Inverted;

                leaderConfigs.DifferentialSensors = diff_cfg;
                for (int i = 0; i < kNumConfigAttempts; ++i) {
                    response = diffLeaderFXS.getConfigurator().apply(leaderConfigs);
                    if (response.isOK()) break;
                }
            } else {
                DriverStation.reportError(
                    "[phoenix] DifferentialMechanism motor type does not match the DifferentialMotorConstants initial configs type. Ensure that the DifferentialMechanism and DifferentialMotorConstants instances are both constructed with the same motor type (TalonFX/TalonFXConfiguration, TalonFXS/TalonFXSConfiguration, etc.).",
                    true
                );
                return;
            }

            if (!response.isOK()) {
                System.out.println(
                    "Talon ID " + _diffLeaderFX.getDeviceID() + " failed config with error " + response.toString()
                );
            }
        }

        /* disable differential control on _diffFollowerFX */
        {
            StatusCode response = StatusCode.OK;

            if (_diffFollowerFX instanceof TalonFX diffFollowerFX) {
                var followerConfigs = (TalonFXConfiguration)constants.FollowerInitialConfigs;
                if (followerConfigs == null) {
                    followerConfigs = new TalonFXConfiguration();
                }
                followerConfigs.DifferentialSensors = new DifferentialSensorsConfigs();
                followerConfigs.MotorOutput.Inverted = constants.Alignment == MotorAlignmentValue.Aligned
                    ? leaderInvert
                    : leaderInvert == InvertedValue.CounterClockwise_Positive
                        ? InvertedValue.Clockwise_Positive
                        : InvertedValue.CounterClockwise_Positive;

                if (constants.FollowerUsesCommonLeaderConfigs) {
                    /* copy over common config groups from the leader */
                    var leaderConfigs = (TalonFXConfiguration)constants.LeaderInitialConfigs;
                    if (leaderConfigs == null) {
                        leaderConfigs = new TalonFXConfiguration();
                    }

                    followerConfigs.Audio = leaderConfigs.Audio;
                    followerConfigs.CurrentLimits = leaderConfigs.CurrentLimits;
                    followerConfigs.MotorOutput = leaderConfigs.MotorOutput.clone()
                        .withInverted(followerConfigs.MotorOutput.Inverted);
                    followerConfigs.Voltage = leaderConfigs.Voltage;
                    followerConfigs.TorqueCurrent = leaderConfigs.TorqueCurrent;
                }

                for (int i = 0; i < kNumConfigAttempts; ++i) {
                    response = diffFollowerFX.getConfigurator().apply(followerConfigs);
                    if (response.isOK()) break;
                }
            } else if (_diffFollowerFX instanceof TalonFXS diffFollowerFXS) {
                var followerConfigs = (TalonFXSConfiguration)constants.FollowerInitialConfigs;
                if (followerConfigs == null) {
                    followerConfigs = new TalonFXSConfiguration();
                }
                followerConfigs.DifferentialSensors = new DifferentialSensorsConfigs();
                followerConfigs.MotorOutput.Inverted = constants.Alignment == MotorAlignmentValue.Aligned
                    ? leaderInvert
                    : leaderInvert == InvertedValue.CounterClockwise_Positive
                        ? InvertedValue.Clockwise_Positive
                        : InvertedValue.CounterClockwise_Positive;

                if (constants.FollowerUsesCommonLeaderConfigs) {
                    /* copy over common config groups from the leader */
                    var leaderConfigs = (TalonFXSConfiguration)constants.LeaderInitialConfigs;
                    if (leaderConfigs == null) {
                        leaderConfigs = new TalonFXSConfiguration();
                    }

                    followerConfigs.Audio = leaderConfigs.Audio;
                    followerConfigs.CurrentLimits = leaderConfigs.CurrentLimits;
                    followerConfigs.MotorOutput = leaderConfigs.MotorOutput.clone()
                        .withInverted(followerConfigs.MotorOutput.Inverted);
                    followerConfigs.Voltage = leaderConfigs.Voltage;
                }

                for (int i = 0; i < kNumConfigAttempts; ++i) {
                    response = diffFollowerFXS.getConfigurator().apply(followerConfigs);
                    if (response.isOK()) break;
                }
            } else {
                DriverStation.reportError(
                    "[phoenix] DifferentialMechanism motor type does not match the DifferentialMotorConstants initial configs type. Ensure that the DifferentialMechanism and DifferentialMotorConstants instances are both constructed with the same motor type (TalonFX/TalonFXConfiguration, TalonFXS/TalonFXSConfiguration, etc.).",
                    true
                );
                return;
            }

            if (!response.isOK()) {
                System.out.println(
                    "Talon ID " + _diffFollowerFX.getDeviceID() + " failed config with error " + response.toString()
                );
            }
        }
    }

    private StatusCode configNeutralModeImpl(MotorT motor, NeutralModeValue neutralMode, double timeoutSeconds) {
        StatusCode status = StatusCode.OK;

        var motorOutCfg = new MotorOutputConfigs();
        if (motor instanceof TalonFX motorFX) {
            /* First read the configs so they're up-to-date */
            status = motorFX.getConfigurator().refresh(motorOutCfg, timeoutSeconds);
            if (status.isOK()) {
                /* Then set the neutral mode config to the appropriate value */
                motorOutCfg.NeutralMode = neutralMode;
                status = motorFX.getConfigurator().apply(motorOutCfg, timeoutSeconds);
            }
        } else if (motor instanceof TalonFXS motorFXS) {
            /* First read the configs so they're up-to-date */
            status = motorFXS.getConfigurator().refresh(motorOutCfg, timeoutSeconds);
            if (status.isOK()) {
                /* Then set the neutral mode config to the appropriate value */
                motorOutCfg.NeutralMode = neutralMode;
                status = motorFXS.getConfigurator().apply(motorOutCfg, timeoutSeconds);
            }
        }

        if (!status.isOK()) {
            System.out.println(
                "Talon ID " + motor.getDeviceID() + " failed config with error " + status.toString()
            );
        }
        return status;
    }

    /**
     * Configures the neutral mode to use for both motors in the mechanism.
     * <p>
     * This will wait up to 0.100 seconds (100ms) by default.
     *
     * @param neutralMode The state of the motor controller bridge when output is neutral or disabled
     * @return Status code of the first failed config call, or OK if all succeeded
     */
    public final StatusCode configNeutralMode(NeutralModeValue neutralMode) {
        return configNeutralMode(neutralMode, 0.100);
    }

    /**
     * Configures the neutral mode to use for both motors in the mechanism.
     *
     * @param neutralMode The state of the motor controller bridge when output is neutral or disabled
     * @param timeoutSeconds Maximum amount of time to wait when performing each configuration
     * @return Status code of the first failed config call, or OK if all succeeded
     */
    public final StatusCode configNeutralMode(NeutralModeValue neutralMode, double timeoutSeconds) {
        StatusCode retval = StatusCode.OK;
        {
            final var status = configNeutralModeImpl(_diffLeaderFX, neutralMode, timeoutSeconds);
            if (retval.isOK()) retval = status;
        }
        {
            final var status = configNeutralModeImpl(_diffFollowerFX, neutralMode, timeoutSeconds);
            if (retval.isOK()) retval = status;
        }
        return retval;
    }

    /**
     * Call this method periodically to automatically protect against dangerous
     * fault conditions and keep {@link #getMechanismState()} updated.
     */
    public final void periodic() {
        StatusCode retval = StatusCode.OK;

        /* handle remote sensor position overflow fault */
        if (_diffLeaderFX.getFault_RemoteSensorPosOverflow().getValue()) {
            /* fault the mechanism until the user clears it manually */
            _requiresUserReason = RequiresUserReasonValue.RemoteSensorPosOverflow;
            _requiresUserAction.setRelease(true);

            _disabledReason = DisabledReasonValue.RemoteSensorPosOverflow;
            retval = StatusCode.MechanismFaulted;
        }

        /* handle missing remote sensor fault */
        if (_diffLeaderFX.getFault_RemoteSensorDataInvalid().getValue() ||
            _diffFollowerFX.getFault_RemoteSensorDataInvalid().getValue()
        ) {
            /* temporarily fault the mechanism while the fault is active */
            _disabledReason = DisabledReasonValue.MissingRemoteSensor;
            retval = StatusCode.MechanismFaulted;
        }
        /* handle missing differential Talon FX fault */
        if (_diffLeaderFX.getFault_MissingDifferentialFX().getValue()) {
            /* temporarily fault the mechanism while the fault is active */
            _disabledReason = DisabledReasonValue.MissingDifferentialFX;
            retval = StatusCode.MechanismFaulted;
        }

        /* handle if any of the devices have power cycled */
        final boolean diffLeaderFX_hasReset = _diffLeaderFXResetChecker.getAsBoolean();
        final boolean diffFollowerFX_hasReset = _diffFollowerFXResetChecker.getAsBoolean();
        final boolean diffSensor_hasReset = _diffSensorResetChecker.isPresent() && _diffSensorResetChecker.get().getAsBoolean();
        final boolean diffLeaderFX_remsens_hasReset = _diffLeaderFX.getStickyFault_RemoteSensorReset().getValue();
        final boolean diffFollowerFX_remsens_hasReset = _diffFollowerFX.getStickyFault_RemoteSensorReset().getValue();

        if (diffLeaderFX_hasReset || diffFollowerFX_hasReset || diffSensor_hasReset ||
            diffLeaderFX_remsens_hasReset || diffFollowerFX_remsens_hasReset
        ) {
            /* fault the mechanism until the user clears it manually */
            _requiresUserReason = RequiresUserReasonValue.DeviceHasReset;
            _requiresUserAction.setRelease(true);

            _disabledReason = DisabledReasonValue.DeviceHasReset;
            retval = StatusCode.MechanismFaulted;
        }

        if (retval.isOK() && _requiresUserAction.getOpaque()) {
            /* keep the mechanism faulted until user clears the fault */
            retval = StatusCode.MechanismFaulted;
        }

        if (!retval.isOK()) {
            /* disable the mechanism */
            _mechanismDisabled.setRelease(true);
        } else {
            /* re-enable the mechanism */
            _disabledReason = DisabledReasonValue.None;
            _mechanismDisabled.setRelease(false);
        }
    }

    /**
     * Get whether the mechanism is currently disabled due to an issue.
     *
     * @return true if the mechanism is temporarily disabled
     */
    public final boolean isDisabled() {
        return _mechanismDisabled.getAcquire();
    }

    /**
     * Get whether the mechanism is currently disabled and requires
     * user action to re-enable mechanism control.
     *
     * @return true if the mechanism is disabled and the user must manually
     *         perform an action
     */
    public final boolean requiresUserAction() {
        return _requiresUserAction.getAcquire();
    }

    /**
     * Gets the state of the mechanism.
     *
     * @return MechanismState representing the state of the mechanism
     */
    public final MechanismState getMechanismState() {
        if (requiresUserAction()) {
            return MechanismState.RequiresUserAction;
        } else if (isDisabled()) {
            return MechanismState.Disabled;
        } else {
            return MechanismState.OK;
        }
    }

    /**
     * Indicate to the mechanism that the user has performed the required
     * action to resume mechanism control.
     */
    public final void clearUserRequirement() {
        if (_diffLeaderFX.getStickyFault_RemoteSensorReset().getValue()) {
            _diffLeaderFX.clearStickyFault_RemoteSensorReset();
        }
        if (_diffFollowerFX.getStickyFault_RemoteSensorReset().getValue()) {
            _diffFollowerFX.clearStickyFault_RemoteSensorReset();
        }
        _requiresUserReason = RequiresUserReasonValue.None;
        _requiresUserAction.setRelease(false);
    }

    /**
     * @return The reason for the mechanism being disabled
     */
    public final DisabledReasonValue getDisabledReason() {
        return _disabledReason;
    }
    /**
     * @return The reason for the mechanism requiring user
     *         action to resume control
     */
    public final RequiresUserReasonValue getRequiresUserReason() {
        return _requiresUserReason;
    }

    /**
     * Average component of the mechanism position.
     * 
     * <ul>
     *   <li> <b>Minimum Value:</b> -16384.0
     *   <li> <b>Maximum Value:</b> 16383.999755859375
     *   <li> <b>Default Value:</b> 0
     *   <li> <b>Units:</b> rotations
     * </ul>
     * <p>
     * This refreshes and returns a cached StatusSignal object.
     * 
     * @return DifferentialAveragePosition Status Signal Object
     */
    public final StatusSignal<Angle> getAveragePosition() {
        return getAveragePosition(true);
    }
    /**
     * Average component of the mechanism position.
     * 
     * <ul>
     *   <li> <b>Minimum Value:</b> -16384.0
     *   <li> <b>Maximum Value:</b> 16383.999755859375
     *   <li> <b>Default Value:</b> 0
     *   <li> <b>Units:</b> rotations
     * </ul>
     * <p>
     * This refreshes and returns a cached StatusSignal object.
     * 
     * @param refresh Whether to refresh the StatusSignal before returning it; defaults to true
     * @return DifferentialAveragePosition Status Signal Object
     */
    public final StatusSignal<Angle> getAveragePosition(boolean refresh) {
        return _diffLeaderFX.getDifferentialAveragePosition(refresh);
    }

    /**
     * Average component of the mechanism velocity.
     * 
     * <ul>
     *   <li> <b>Minimum Value:</b> -512.0
     *   <li> <b>Maximum Value:</b> 511.998046875
     *   <li> <b>Default Value:</b> 0
     *   <li> <b>Units:</b> rotations per second
     * </ul>
     * <p>
     * This refreshes and returns a cached StatusSignal object.
     * 
     * @return DifferentialAverageVelocity Status Signal Object
     */
    public final StatusSignal<AngularVelocity> getAverageVelocity() {
        return getAverageVelocity(true);
    }
    /**
     * Average component of the mechanism velocity.
     * 
     * <ul>
     *   <li> <b>Minimum Value:</b> -512.0
     *   <li> <b>Maximum Value:</b> 511.998046875
     *   <li> <b>Default Value:</b> 0
     *   <li> <b>Units:</b> rotations per second
     * </ul>
     * <p>
     * This refreshes and returns a cached StatusSignal object.
     * 
     * @param refresh Whether to refresh the StatusSignal before returning it; defaults to true
     * @return DifferentialAverageVelocity Status Signal Object
     */
    public final StatusSignal<AngularVelocity> getAverageVelocity(boolean refresh) {
        return _diffLeaderFX.getDifferentialAverageVelocity(refresh);
    }

    /**
     * Differential component of the mechanism position.
     * 
     * <ul>
     *   <li> <b>Minimum Value:</b> -16384.0
     *   <li> <b>Maximum Value:</b> 16383.999755859375
     *   <li> <b>Default Value:</b> 0
     *   <li> <b>Units:</b> rotations
     * </ul>
     * <p>
     * This refreshes and returns a cached StatusSignal object.
     * 
     * @return DifferentialDifferencePosition Status Signal Object
     */
    public final StatusSignal<Angle> getDifferentialPosition() {
        return getDifferentialPosition(true);
    }
    /**
     * Differential component of the mechanism position.
     * 
     * <ul>
     *   <li> <b>Minimum Value:</b> -16384.0
     *   <li> <b>Maximum Value:</b> 16383.999755859375
     *   <li> <b>Default Value:</b> 0
     *   <li> <b>Units:</b> rotations
     * </ul>
     * <p>
     * This refreshes and returns a cached StatusSignal object.
     * 
     * @param refresh Whether to refresh the StatusSignal before returning it; defaults to true
     * @return DifferentialDifferencePosition Status Signal Object
     */
    public final StatusSignal<Angle> getDifferentialPosition(boolean refresh) {
        return _diffLeaderFX.getDifferentialDifferencePosition(refresh);
    }

    /**
     * Differential component of the mechanism velocity.
     * 
     * <ul>
     *   <li> <b>Minimum Value:</b> -512.0
     *   <li> <b>Maximum Value:</b> 511.998046875
     *   <li> <b>Default Value:</b> 0
     *   <li> <b>Units:</b> rotations per second
     * </ul>
     * <p>
     * This refreshes and returns a cached StatusSignal object.
     * 
     * @return DifferentialDifferenceVelocity Status Signal Object
     */
    public final StatusSignal<AngularVelocity> getDifferentialVelocity() {
        return getDifferentialVelocity(true);
    }
    /**
     * Differential component of the mechanism velocity.
     * 
     * <ul>
     *   <li> <b>Minimum Value:</b> -512.0
     *   <li> <b>Maximum Value:</b> 511.998046875
     *   <li> <b>Default Value:</b> 0
     *   <li> <b>Units:</b> rotations per second
     * </ul>
     * <p>
     * This refreshes and returns a cached StatusSignal object.
     * 
     * @param refresh Whether to refresh the StatusSignal before returning it; defaults to true
     * @return DifferentialDifferenceVelocity Status Signal Object
     */
    public final StatusSignal<AngularVelocity> getDifferentialVelocity(boolean refresh) {
        return _diffLeaderFX.getDifferentialDifferenceVelocity(refresh);
    }

    /**
     * Value that the average closed loop is targeting.
     * <p>
     * This is the value that the closed loop PID controller targets.
     * <p>
     * This refreshes and returns a cached StatusSignal object.
     * 
     * @return ClosedLoopReference Status Signal object
     */
    public final StatusSignal<Double> getAverageClosedLoopReference() {
        return getAverageClosedLoopReference(true);
    }
    /**
     * Value that the average closed loop is targeting.
     * <p>
     * This is the value that the closed loop PID controller targets.
     * <p>
     * This refreshes and returns a cached StatusSignal object.
     * 
     * @param refresh Whether to refresh the StatusSignal before returning it; defaults to true
     * @return ClosedLoopReference Status Signal object
     */
    public final StatusSignal<Double> getAverageClosedLoopReference(boolean refresh) {
        return _diffLeaderFX.getClosedLoopReference(refresh);
    }

    /**
     * Derivative of the target that the average closed loop is targeting.
     * <p>
     * This is the change in the closed loop reference. This may be used
     * in the feed-forward calculation, the derivative-error, or in
     * application of the signage for kS. Typically, this represents the
     * target velocity during Motion Magic®.
     * <p>
     * This refreshes and returns a cached StatusSignal object.
     * 
     * @return ClosedLoopReferenceSlope Status Signal object
     */
    public final StatusSignal<Double> getAverageClosedLoopReferenceSlope() {
        return getAverageClosedLoopReferenceSlope(true);
    }
    /**
     * Derivative of the target that the average closed loop is targeting.
     * <p>
     * This is the change in the closed loop reference. This may be used
     * in the feed-forward calculation, the derivative-error, or in
     * application of the signage for kS. Typically, this represents the
     * target velocity during Motion Magic®.
     * <p>
     * This refreshes and returns a cached StatusSignal object.
     * 
     * @param refresh Whether to refresh the StatusSignal before returning it; defaults to true
     * @return ClosedLoopReferenceSlope Status Signal object
     */
    public final StatusSignal<Double> getAverageClosedLoopReferenceSlope(boolean refresh) {
        return _diffLeaderFX.getClosedLoopReferenceSlope(refresh);
    }

    /**
     * The difference between target average reference and current measurement.
     * <p>
     * This is the value that is treated as the error in the PID loop.
     * <p>
     * This refreshes and returns a cached StatusSignal object.
     * 
     * @return ClosedLoopError Status Signal object
     */
    public final StatusSignal<Double> getAverageClosedLoopError() {
        return getAverageClosedLoopError(true);
    }
    /**
     * The difference between target average reference and current measurement.
     * <p>
     * This is the value that is treated as the error in the PID loop.
     * <p>
     * This refreshes and returns a cached StatusSignal object.
     * 
     * @param refresh Whether to refresh the StatusSignal before returning it; defaults to true
     * @return ClosedLoopError Status Signal object
     */
    public final StatusSignal<Double> getAverageClosedLoopError(boolean refresh) {
        return _diffLeaderFX.getClosedLoopError(refresh);
    }

    /**
     * Value that the differential closed loop is targeting.
     * <p>
     * This is the value that the differential closed loop PID controller
     * targets (on the difference axis).
     * <p>
     * This refreshes and returns a cached StatusSignal object.
     * 
     * @return DifferentialClosedLoopReference Status Signal object
     */
    public final StatusSignal<Double> getDifferentialClosedLoopReference() {
        return getDifferentialClosedLoopReference(true);
    }
    /**
     * Value that the differential closed loop is targeting.
     * <p>
     * This is the value that the differential closed loop PID controller
     * targets (on the difference axis).
     * <p>
     * This refreshes and returns a cached StatusSignal object.
     * 
     * @param refresh Whether to refresh the StatusSignal before returning it; defaults to true
     * @return DifferentialClosedLoopReference Status Signal object
     */
    public final StatusSignal<Double> getDifferentialClosedLoopReference(boolean refresh) {
        return _diffLeaderFX.getDifferentialClosedLoopReference(refresh);
    }

    /**
     * Derivative of the target that the differential closed loop is
     * targeting.
     * <p>
     * This is the change in the closed loop reference (on the difference
     * axis). This may be used in the feed-forward calculation, the
     * derivative-error, or in application of the signage for kS.
     * Typically, this represents the target velocity during Motion
     * Magic®.
     * <p>
     * This refreshes and returns a cached StatusSignal object.
     * 
     * @return DifferentialClosedLoopReferenceSlope Status Signal object
     */
    public final StatusSignal<Double> getDifferentialClosedLoopReferenceSlope() {
        return getDifferentialClosedLoopReferenceSlope(true);
    }
    /**
     * Derivative of the target that the differential closed loop is
     * targeting.
     * <p>
     * This is the change in the closed loop reference (on the difference
     * axis). This may be used in the feed-forward calculation, the
     * derivative-error, or in application of the signage for kS.
     * Typically, this represents the target velocity during Motion
     * Magic®.
     * <p>
     * This refreshes and returns a cached StatusSignal object.
     * 
     * @param refresh Whether to refresh the StatusSignal before returning it; defaults to true
     * @return DifferentialClosedLoopReferenceSlope Status Signal object
     */
    public final StatusSignal<Double> getDifferentialClosedLoopReferenceSlope(boolean refresh) {
        return _diffLeaderFX.getDifferentialClosedLoopReferenceSlope(refresh);
    }

    /**
     * The difference between target differential reference and current
     * measurement.
     * <p>
     * This is the value that is treated as the error in the differential
     * PID loop (on the difference axis).
     * <p>
     * This refreshes and returns a cached StatusSignal object.
     * 
     * @return DifferentialClosedLoopError Status Signal object
     */
    public final StatusSignal<Double> getDifferentialClosedLoopError() {
        return getDifferentialClosedLoopError(true);
    }
    /**
     * The difference between target differential reference and current
     * measurement.
     * <p>
     * This is the value that is treated as the error in the differential
     * PID loop (on the difference axis).
     * <p>
     * This refreshes and returns a cached StatusSignal object.
     * 
     * @param refresh Whether to refresh the StatusSignal before returning it; defaults to true
     * @return DifferentialClosedLoopError Status Signal object
     */
    public final StatusSignal<Double> getDifferentialClosedLoopError(boolean refresh) {
        return _diffLeaderFX.getDifferentialClosedLoopError(refresh);
    }

    /**
     * Sets the position of the mechanism in rotations, assuming a differential
     * position of 0 rotations.
     * <p>
     * This will wait up to 0.100 seconds (100ms) by default.
     *
     * @param avgPosition The average position of the mechanism, in rotations
     * @return StatusCode of the set command
     */
    public final StatusCode setPosition(double avgPosition) {
        return setPosition(avgPosition, 0.0, 0.100);
    }
    /**
     * Sets the position of the mechanism in rotations.
     * <p>
     * This will wait up to 0.100 seconds (100ms) by default.
     *
     * @param avgPosition The average position of the mechanism, in rotations
     * @param diffPosition The differential position of the mechanism, in rotations
     * @return StatusCode of the set command
     */
    public final StatusCode setPosition(double avgPosition, double diffPosition) {
        return setPosition(avgPosition, diffPosition, 0.100);
    }
    /**
     * Sets the position of the mechanism in rotations.
     *
     * @param avgPosition The average position of the mechanism, in rotations
     * @param diffPosition The differential position of the mechanism, in rotations
     * @param timeoutSeconds Maximum time to wait up to in seconds
     * @return StatusCode of the set command
     */
    public final StatusCode setPosition(double avgPosition, double diffPosition, double timeoutSeconds) {
        var retval = StatusCode.OK;

        var response = _diffLeaderFX.setPosition(avgPosition + diffPosition * _sensorToDiffRatio, timeoutSeconds);
        if (retval.isOK()) retval = response;
        response = _diffFollowerFX.setPosition(avgPosition - diffPosition * _sensorToDiffRatio, timeoutSeconds);
        if (retval.isOK()) retval = response;

        return retval;
    }

    /**
     * Sets the position of the mechanism, assuming a differential
     * position of 0 rotations.
     * <p>
     * This will wait up to 0.100 seconds (100ms) by default.
     *
     * @param avgPosition The average position of the mechanism
     * @return StatusCode of the set command
     */
    public final StatusCode setPosition(Angle avgPosition) {
        return setPosition(avgPosition.in(Rotations));
    }
    /**
     * Sets the position of the mechanism.
     * <p>
     * This will wait up to 0.100 seconds (100ms) by default.
     *
     * @param avgPosition The average position of the mechanism
     * @param diffPosition The differential position of the mechanism
     * @return StatusCode of the set command
     */
    public final StatusCode setPosition(Angle avgPosition, Angle diffPosition) {
        return setPosition(avgPosition.in(Rotations), diffPosition.in(Rotations));
    }
    /**
     * Sets the position of the mechanism.
     *
     * @param avgPosition The average position of the mechanism
     * @param diffPosition The differential position of the mechanism
     * @param timeoutSeconds Maximum time to wait up to in seconds
     * @return StatusCode of the set command
     */
    public final StatusCode setPosition(Angle avgPosition, Angle diffPosition, double timeoutSeconds) {
        return setPosition(avgPosition.in(Rotations), diffPosition.in(Rotations), timeoutSeconds);
    }

    /**
     * Get the Talon FX that is differential leader. The differential
     * leader calculates the output for the differential follower. The
     * differential leader is also useful for fault detection, and it
     * reports status signals for the differential controller.
     *
     * @return Differential leader Talon FX
     */
    public final MotorT getLeader() {
        return _diffLeaderFX;
    }

    /**
     * Get the Talon FX that is differential follower. The differential
     * follower's position and velocity are used by the differential leader
     * for the differential controller.
     *
     * @return Differential follower Talon FX
     */
    public final MotorT getFollower() {
        return _diffFollowerFX;
    }

    private StatusCode beforeControl() {
        StatusCode retval = StatusCode.OK;
        if (_mechanismDisabled.getOpaque()) {
            /* disable the mechanism */
            retval = StatusCode.MechanismFaulted;
        }

        if (!retval.isOK()) {
            /* neutral the output */
            setNeutralOut();
        }
        return retval;
    }

    /**
     * Request neutral output of mechanism. The applied brake type
     * is determined by the NeutralMode configuration of each device.
     * <p>
     * Since the NeutralMode configuration of devices may not align, users
     * may prefer to use the {@link #setCoastOut()} or {@link #setStaticBrake()} method.
     *
     * @return Status Code of the request.
     */
    public final StatusCode setNeutralOut() {
        var retval = StatusCode.OK;
        {
            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_neutral);
            if (retval.isOK()) {
                retval = _diffLeaderFX_retval;
            }
        }
        {
            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_neutral);
            if (retval.isOK()) {
                retval = _diffFollowerFX_retval;
            }
        }
        return retval;
    }

    /**
     * Request coast neutral output of mechanism. The bridge is
     * disabled and the rotor is allowed to coast.
     *
     * @return Status Code of the request.
     */
    public final StatusCode setCoastOut() {
        var retval = StatusCode.OK;
        {
            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_coast);
            if (retval.isOK()) {
                retval = _diffLeaderFX_retval;
            }
        }
        {
            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_coast);
            if (retval.isOK()) {
                retval = _diffFollowerFX_retval;
            }
        }
        return retval;
    }

    /**
     * Applies full neutral-brake on the mechanism by shorting
     * motor leads together.
     *
     * @return Status Code of the request.
     */
    public final StatusCode setStaticBrake() {
        var retval = StatusCode.OK;
        {
            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_brake);
            if (retval.isOK()) {
                retval = _diffLeaderFX_retval;
            }
        }
        {
            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_brake);
            if (retval.isOK()) {
                retval = _diffFollowerFX_retval;
            }
        }
        return retval;
    }

    
    
    /**
     * Sets the control request for this mechanism.
     *
     * @param diffLeaderFXRequest Request a specified motor duty cycle with a
     *                            differential position closed-loop.
     * @return Status Code of the request.
     */
    public final StatusCode setControl(DifferentialDutyCycle diffLeaderFXRequest) {
        var retval = StatusCode.OK;
    
        retval = beforeControl();
        if (!retval.isOK()) {
            return retval;
        }
        
        {
            final var _diffLeaderFX_reqPtr = diffLeaderFXRequest;
            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
            if (retval.isOK()) {
                retval = _diffLeaderFX_retval;
            }
        }
        
        {
            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
            if (retval.isOK()) {
                retval = _diffFollowerFX_retval;
            }
        }
        
        return retval;
    }
    
    /**
     * Sets the control request for this mechanism.
     *
     * @param diffLeaderFXRequest Request a specified voltage with a differential
     *                            position closed-loop.
     * @return Status Code of the request.
     */
    public final StatusCode setControl(DifferentialVoltage diffLeaderFXRequest) {
        var retval = StatusCode.OK;
    
        retval = beforeControl();
        if (!retval.isOK()) {
            return retval;
        }
        
        {
            final var _diffLeaderFX_reqPtr = diffLeaderFXRequest;
            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
            if (retval.isOK()) {
                retval = _diffLeaderFX_retval;
            }
        }
        
        {
            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
            if (retval.isOK()) {
                retval = _diffFollowerFX_retval;
            }
        }
        
        return retval;
    }
    
    /**
     * Sets the control request for this mechanism.
     *
     * @param diffLeaderFXRequest Request PID to target position with a differential
     *                            position setpoint.
     * @return Status Code of the request.
     */
    public final StatusCode setControl(DifferentialPositionDutyCycle diffLeaderFXRequest) {
        var retval = StatusCode.OK;
    
        retval = beforeControl();
        if (!retval.isOK()) {
            return retval;
        }
        
        {
            final var _diffLeaderFX_reqPtr = diffLeaderFXRequest;
            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
            if (retval.isOK()) {
                retval = _diffLeaderFX_retval;
            }
        }
        
        {
            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
            if (retval.isOK()) {
                retval = _diffFollowerFX_retval;
            }
        }
        
        return retval;
    }
    
    /**
     * Sets the control request for this mechanism.
     *
     * @param diffLeaderFXRequest Request PID to target position with a differential
     *                            position setpoint
     * @return Status Code of the request.
     */
    public final StatusCode setControl(DifferentialPositionVoltage diffLeaderFXRequest) {
        var retval = StatusCode.OK;
    
        retval = beforeControl();
        if (!retval.isOK()) {
            return retval;
        }
        
        {
            final var _diffLeaderFX_reqPtr = diffLeaderFXRequest;
            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
            if (retval.isOK()) {
                retval = _diffLeaderFX_retval;
            }
        }
        
        {
            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
            if (retval.isOK()) {
                retval = _diffFollowerFX_retval;
            }
        }
        
        return retval;
    }
    
    /**
     * Sets the control request for this mechanism.
     *
     * @param diffLeaderFXRequest Request PID to target velocity with a differential
     *                            position setpoint.
     * @return Status Code of the request.
     */
    public final StatusCode setControl(DifferentialVelocityDutyCycle diffLeaderFXRequest) {
        var retval = StatusCode.OK;
    
        retval = beforeControl();
        if (!retval.isOK()) {
            return retval;
        }
        
        {
            final var _diffLeaderFX_reqPtr = diffLeaderFXRequest;
            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
            if (retval.isOK()) {
                retval = _diffLeaderFX_retval;
            }
        }
        
        {
            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
            if (retval.isOK()) {
                retval = _diffFollowerFX_retval;
            }
        }
        
        return retval;
    }
    
    /**
     * Sets the control request for this mechanism.
     *
     * @param diffLeaderFXRequest Request PID to target velocity with a differential
     *                            position setpoint.
     * @return Status Code of the request.
     */
    public final StatusCode setControl(DifferentialVelocityVoltage diffLeaderFXRequest) {
        var retval = StatusCode.OK;
    
        retval = beforeControl();
        if (!retval.isOK()) {
            return retval;
        }
        
        {
            final var _diffLeaderFX_reqPtr = diffLeaderFXRequest;
            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
            if (retval.isOK()) {
                retval = _diffLeaderFX_retval;
            }
        }
        
        {
            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
            if (retval.isOK()) {
                retval = _diffFollowerFX_retval;
            }
        }
        
        return retval;
    }
    
    /**
     * Sets the control request for this mechanism.
     *
     * @param diffLeaderFXRequest Requests Motion Magic® to target a final position
     *                            using a motion profile, and PID to a differential
     *                            position setpoint.
     * @return Status Code of the request.
     */
    public final StatusCode setControl(DifferentialMotionMagicDutyCycle diffLeaderFXRequest) {
        var retval = StatusCode.OK;
    
        retval = beforeControl();
        if (!retval.isOK()) {
            return retval;
        }
        
        {
            final var _diffLeaderFX_reqPtr = diffLeaderFXRequest;
            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
            if (retval.isOK()) {
                retval = _diffLeaderFX_retval;
            }
        }
        
        {
            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
            if (retval.isOK()) {
                retval = _diffFollowerFX_retval;
            }
        }
        
        return retval;
    }
    
    /**
     * Sets the control request for this mechanism.
     *
     * @param diffLeaderFXRequest Requests Motion Magic® to target a final position
     *                            using a motion profile, and PID to a differential
     *                            position setpoint.
     * @return Status Code of the request.
     */
    public final StatusCode setControl(DifferentialMotionMagicVoltage diffLeaderFXRequest) {
        var retval = StatusCode.OK;
    
        retval = beforeControl();
        if (!retval.isOK()) {
            return retval;
        }
        
        {
            final var _diffLeaderFX_reqPtr = diffLeaderFXRequest;
            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
            if (retval.isOK()) {
                retval = _diffLeaderFX_retval;
            }
        }
        
        {
            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
            if (retval.isOK()) {
                retval = _diffFollowerFX_retval;
            }
        }
        
        return retval;
    }
    
    /**
     * Sets the control request for this mechanism.
     *
     * @param diffLeaderFXRequest Requests Motion Magic® to target a final position
     *                            using an exponential motion profile, and PID to a
     *                            differential position setpoint.
     * @return Status Code of the request.
     */
    public final StatusCode setControl(DifferentialMotionMagicExpoDutyCycle diffLeaderFXRequest) {
        var retval = StatusCode.OK;
    
        retval = beforeControl();
        if (!retval.isOK()) {
            return retval;
        }
        
        {
            final var _diffLeaderFX_reqPtr = diffLeaderFXRequest;
            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
            if (retval.isOK()) {
                retval = _diffLeaderFX_retval;
            }
        }
        
        {
            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
            if (retval.isOK()) {
                retval = _diffFollowerFX_retval;
            }
        }
        
        return retval;
    }
    
    /**
     * Sets the control request for this mechanism.
     *
     * @param diffLeaderFXRequest Requests Motion Magic® to target a final position
     *                            using an exponential motion profile, and PID to a
     *                            differential position setpoint.
     * @return Status Code of the request.
     */
    public final StatusCode setControl(DifferentialMotionMagicExpoVoltage diffLeaderFXRequest) {
        var retval = StatusCode.OK;
    
        retval = beforeControl();
        if (!retval.isOK()) {
            return retval;
        }
        
        {
            final var _diffLeaderFX_reqPtr = diffLeaderFXRequest;
            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
            if (retval.isOK()) {
                retval = _diffLeaderFX_retval;
            }
        }
        
        {
            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
            if (retval.isOK()) {
                retval = _diffFollowerFX_retval;
            }
        }
        
        return retval;
    }
    
    /**
     * Sets the control request for this mechanism.
     *
     * @param diffLeaderFXRequest Requests Motion Magic® to target a final velocity
     *                            using a motion profile, and PID to a differential
     *                            position setpoint.  This allows smooth transitions
     *                            between velocity set points.
     * @return Status Code of the request.
     */
    public final StatusCode setControl(DifferentialMotionMagicVelocityDutyCycle diffLeaderFXRequest) {
        var retval = StatusCode.OK;
    
        retval = beforeControl();
        if (!retval.isOK()) {
            return retval;
        }
        
        {
            final var _diffLeaderFX_reqPtr = diffLeaderFXRequest;
            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
            if (retval.isOK()) {
                retval = _diffLeaderFX_retval;
            }
        }
        
        {
            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
            if (retval.isOK()) {
                retval = _diffFollowerFX_retval;
            }
        }
        
        return retval;
    }
    
    /**
     * Sets the control request for this mechanism.
     *
     * @param diffLeaderFXRequest Requests Motion Magic® to target a final velocity
     *                            using a motion profile, and PID to a differential
     *                            position setpoint.  This allows smooth transitions
     *                            between velocity set points.
     * @return Status Code of the request.
     */
    public final StatusCode setControl(DifferentialMotionMagicVelocityVoltage diffLeaderFXRequest) {
        var retval = StatusCode.OK;
    
        retval = beforeControl();
        if (!retval.isOK()) {
            return retval;
        }
        
        {
            final var _diffLeaderFX_reqPtr = diffLeaderFXRequest;
            final var _diffLeaderFX_retval = _diffLeaderFX.setControl(_diffLeaderFX_reqPtr);
            if (retval.isOK()) {
                retval = _diffLeaderFX_retval;
            }
        }
        
        {
            final var _diffFollowerFX_retval = _diffFollowerFX.setControl(_diffFollow);
            if (retval.isOK()) {
                retval = _diffFollowerFX_retval;
            }
        }
        
        return retval;
    }
    
}