/*
 * Copyright (c) 2021 REV Robotics
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. Neither the name of REV Robotics nor the names of its
 *    contributors may be used to endorse or promote products derived from
 *    this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

package com.revrobotics;

import com.revrobotics.jni.CANSparkMaxJNI;

/** Get an instance of this class by using {@link CANSparkMax#getPIDController()}. */
public class SparkMaxPIDController implements CANPIDController {
  private final CANSparkMax sparkMax;

  public enum AccelStrategy {
    kTrapezoidal(0),
    kSCurve(1);

    @SuppressWarnings("MemberName")
    public final int value;

    AccelStrategy(int value) {
      this.value = value;
    }

    public static AccelStrategy fromInt(int value) {
      switch (value) {
        case 0:
          return kTrapezoidal;
        case 1:
          return kSCurve;
        default:
          return kTrapezoidal;
      }
    }
  }

  public enum ArbFFUnits {
    kVoltage(0),
    kPercentOut(1);

    @SuppressWarnings("MemberName")
    public final int value;

    ArbFFUnits(int value) {
      this.value = value;
    }
  }

  // package-private (can only be used by other classes in this package)
  SparkMaxPIDController(CANSparkMax device) {
    sparkMax = device;
  }

  /**
   * Set the controller reference value based on the selected control mode.
   *
   * @param value The value to set depending on the control mode. For basic duty cycle control this
   *     should be a value between -1 and 1 Otherwise: Voltage Control: Voltage (volts) Velocity
   *     Control: Velocity (RPM) Position Control: Position (Rotations) Current Control: Current
   *     (Amps). Native units can be changed using the setPositionConversionFactor() or
   *     setVelocityConversionFactor() methods of the CANEncoder class
   * @param ctrl the control type
   * @return {@link REVLibError#kOk} if successful
   */
  public REVLibError setReference(double value, CANSparkMax.ControlType ctrl) {
    sparkMax.throwIfClosed();
    return setReference(value, ctrl, 0);
  }

  /**
   * Set the controller reference value based on the selected control mode.
   *
   * @param value The value to set depending on the control mode. For basic duty cycle control this
   *     should be a value between -1 and 1 Otherwise: Voltage Control: Voltage (volts) Velocity
   *     Control: Velocity (RPM) Position Control: Position (Rotations) Current Control: Current
   *     (Amps). Native units can be changed using the setPositionConversionFactor() or
   *     setVelocityConversionFactor() methods of the CANEncoder class
   * @param ctrl the control type
   * @return {@link REVLibError#kOk} if successful
   * @deprecated Use {@link #setReference(double, CANSparkMax.ControlType)} instead.
   */
  @Deprecated(forRemoval = true)
  @SuppressWarnings("removal")
  public REVLibError setReference(double value, ControlType ctrl) {
    sparkMax.throwIfClosed();
    return setReference(value, ctrl, 0);
  }

  /**
   * Set the controller reference value based on the selected control mode. This will override the
   * pre-programmed control mode but not change what is programmed to the controller.
   *
   * @param value The value to set depending on the control mode. For basic duty cycle control this
   *     should be a value between -1 and 1 Otherwise: Voltage Control: Voltage (volts) Velocity
   *     Control: Velocity (RPM) Position Control: Position (Rotations) Current Control: Current
   *     (Amps). Native units can be changed using the setPositionConversionFactor() or
   *     setVelocityConversionFactor() methods of the CANEncoder class
   * @param ctrl Is the control type to override with
   * @param pidSlot for this command
   * @return {@link REVLibError#kOk} if successful
   */
  public REVLibError setReference(double value, CANSparkMax.ControlType ctrl, int pidSlot) {
    sparkMax.throwIfClosed();
    return setReference(value, ctrl, pidSlot, 0);
  }

  /**
   * Set the controller reference value based on the selected control mode. This will override the
   * pre-programmed control mode but not change what is programmed to the controller.
   *
   * @param value The value to set depending on the control mode. For basic duty cycle control this
   *     should be a value between -1 and 1 Otherwise: Voltage Control: Voltage (volts) Velocity
   *     Control: Velocity (RPM) Position Control: Position (Rotations) Current Control: Current
   *     (Amps). Native units can be changed using the setPositionConversionFactor() or
   *     setVelocityConversionFactor() methods of the CANEncoder class
   * @param ctrl Is the control type to override with
   * @param pidSlot for this command
   * @return {@link REVLibError#kOk} if successful
   * @deprecated Use {@link #setReference(double, CANSparkMax.ControlType, int)} instead.
   */
  @Deprecated(forRemoval = true)
  @SuppressWarnings("removal")
  public REVLibError setReference(double value, ControlType ctrl, int pidSlot) {
    sparkMax.throwIfClosed();
    return setReference(value, ctrl, pidSlot, 0);
  }

  /**
   * Set the controller reference value based on the selected control mode. This will override the
   * pre-programmed control mode but not change what is programmed to the controller.
   *
   * @param value The value to set depending on the control mode. For basic duty cycle control this
   *     should be a value between -1 and 1 Otherwise: Voltage Control: Voltage (volts) Velocity
   *     Control: Velocity (RPM) Position Control: Position (Rotations) Current Control: Current
   *     (Amps). Native units can be changed using the setPositionConversionFactor() or
   *     setVelocityConversionFactor() methods of the CANEncoder class
   * @param ctrl Is the control type to override with
   * @param pidSlot for this command
   * @param arbFeedforward A value from which is represented in voltage applied to the motor after
   *     the result of the specified control mode. The units for the parameter is Volts. This value
   *     is set after the control mode, but before any current limits or ramp rates.
   * @return {@link REVLibError#kOk} if successful
   */
  public REVLibError setReference(
      double value, CANSparkMax.ControlType ctrl, int pidSlot, double arbFeedforward) {
    sparkMax.throwIfClosed();
    return sparkMax.setpointCommand(value, ctrl, pidSlot, arbFeedforward);
  }

  /**
   * Set the controller reference value based on the selected control mode. This will override the
   * pre-programmed control mode but not change what is programmed to the controller.
   *
   * @param value The value to set depending on the control mode. For basic duty cycle control this
   *     should be a value between -1 and 1 Otherwise: Voltage Control: Voltage (volts) Velocity
   *     Control: Velocity (RPM) Position Control: Position (Rotations) Current Control: Current
   *     (Amps). Native units can be changed using the setPositionConversionFactor() or
   *     setVelocityConversionFactor() methods of the CANEncoder class
   * @param ctrl Is the control type to override with
   * @param pidSlot for this command
   * @param arbFeedforward A value from which is represented in voltage applied to the motor after
   *     the result of the specified control mode. The units for the parameter is Volts. This value
   *     is set after the control mode, but before any current limits or ramp rates.
   * @return {@link REVLibError#kOk} if successful
   * @deprecated Use {@link #setReference(double, CANSparkMax.ControlType, int, double)} instead.
   */
  @Deprecated(forRemoval = true)
  @SuppressWarnings("removal")
  public REVLibError setReference(
      double value, ControlType ctrl, int pidSlot, double arbFeedforward) {
    sparkMax.throwIfClosed();
    return sparkMax.setpointCommand(value, ctrl, pidSlot, arbFeedforward);
  }

  /**
   * Set the controller reference value based on the selected control mode. This will override the
   * pre-programmed control mode but not change what is programmed to the controller.
   *
   * @param value The value to set depending on the control mode. For basic duty cycle control this
   *     should be a value between -1 and 1 Otherwise: Voltage Control: Voltage (volts) Velocity
   *     Control: Velocity (RPM) Position Control: Position (Rotations) Current Control: Current
   *     (Amps). Native units can be changed using the setPositionConversionFactor() or
   *     setVelocityConversionFactor() methods of the CANEncoder class
   * @param ctrl Is the control type to override with
   * @param pidSlot for this command
   * @param arbFeedforward A value from which is represented in voltage applied to the motor after
   *     the result of the specified control mode. The units for the parameter is Volts. This value
   *     is set after the control mode, but before any current limits or ramp rates.
   * @param arbFFUnits The units the arbitrary feed forward term is in
   * @return {@link REVLibError#kOk} if successful
   */
  public REVLibError setReference(
      double value,
      CANSparkMax.ControlType ctrl,
      int pidSlot,
      double arbFeedforward,
      ArbFFUnits arbFFUnits) {
    sparkMax.throwIfClosed();
    return sparkMax.setpointCommand(value, ctrl, pidSlot, arbFeedforward, arbFFUnits.value);
  }

  /**
   * Set the controller reference value based on the selected control mode. This will override the
   * pre-programmed control mode but not change what is programmed to the controller.
   *
   * @param value The value to set depending on the control mode. For basic duty cycle control this
   *     should be a value between -1 and 1 Otherwise: Voltage Control: Voltage (volts) Velocity
   *     Control: Velocity (RPM) Position Control: Position (Rotations) Current Control: Current
   *     (Amps). Native units can be changed using the setPositionConversionFactor() or
   *     setVelocityConversionFactor() methods of the CANEncoder class
   * @param ctrl Is the control type to override with
   * @param pidSlot for this command
   * @param arbFeedforward A value from which is represented in voltage applied to the motor after
   *     the result of the specified control mode. The units for the parameter is Volts. This value
   *     is set after the control mode, but before any current limits or ramp rates.
   * @param arbFFUnits The units the arbitrary feed forward term is in
   * @return {@link REVLibError#kOk} if successful
   * @deprecated Use {@link #setReference(double, CANSparkMax.ControlType, int, double,
   *     SparkMaxPIDController.ArbFFUnits)} instead.
   */
  @SuppressWarnings("removal")
  @Deprecated(forRemoval = true)
  public REVLibError setReference(
      double value,
      ControlType ctrl,
      int pidSlot,
      double arbFeedforward,
      CANPIDController.ArbFFUnits arbFFUnits) {
    sparkMax.throwIfClosed();
    return sparkMax.setpointCommand(value, ctrl, pidSlot, arbFeedforward, arbFFUnits.value);
  }

  /**
   * Set the Proportional Gain constant of the PIDF controller on the SPARK MAX. This uses the Set
   * Parameter API and should be used infrequently. The parameter does not presist unless
   * burnFlash() is called. The recommended method to configure this parameter is use to SPARK MAX
   * GUI to tune and save parameters.
   *
   * @param gain The proportional gain value, must be positive
   * @return {@link REVLibError#kOk} if successful
   */
  public REVLibError setP(double gain) {
    sparkMax.throwIfClosed();
    return setP(gain, 0);
  }

  /**
   * Set the Proportional Gain constant of the PIDF controller on the SPARK MAX. This uses the Set
   * Parameter API and should be used infrequently. The parameter does not presist unless
   * burnFlash() is called. The recommended method to configure this parameter is use to SPARK MAX
   * GUI to tune and save parameters.
   *
   * @param gain The proportional gain value, must be positive
   * @param slotID Is the gain schedule slot, the value is a number between 0 and 3. Each slot has
   *     its own set of gain values and can be changed in each control frame using SetReference().
   * @return {@link REVLibError#kOk} if successful
   */
  public REVLibError setP(double gain, int slotID) {
    sparkMax.throwIfClosed();
    return REVLibError.fromInt(
        CANSparkMaxJNI.c_SparkMax_SetP(sparkMax.sparkMaxHandle, slotID, (float) gain));
  }

  /**
   * Set the Integral Gain constant of the PIDF controller on the SPARK MAX. This uses the Set
   * Parameter API and should be used infrequently. The parameter does not presist unless
   * burnFlash() is called. The recommended method to configure this parameter is use to SPARK MAX
   * GUI to tune and save parameters.
   *
   * @param gain The integral gain value, must be positive
   * @return {@link REVLibError#kOk} if successful
   */
  public REVLibError setI(double gain) {
    sparkMax.throwIfClosed();
    return setI(gain, 0);
  }

  /**
   * Set the Integral Gain constant of the PIDF controller on the SPARK MAX. This uses the Set
   * Parameter API and should be used infrequently. The parameter does not presist unless
   * burnFlash() is called. The recommended method to configure this parameter is use to SPARK MAX
   * GUI to tune and save parameters.
   *
   * @param gain The integral gain value, must be positive
   * @param slotID Is the gain schedule slot, the value is a number between 0 and 3. Each slot has
   *     its own set of gain values and can be changed in each control frame using SetReference().
   * @return {@link REVLibError#kOk} if successful
   */
  public REVLibError setI(double gain, int slotID) {
    sparkMax.throwIfClosed();
    return REVLibError.fromInt(
        CANSparkMaxJNI.c_SparkMax_SetI(sparkMax.sparkMaxHandle, slotID, (float) gain));
  }

  /**
   * Set the Derivative Gain constant of the PIDF controller on the SPARK MAX. This uses the Set
   * Parameter API and should be used infrequently. The parameter does not presist unless
   * burnFlash() is called. The recommended method to configure this parameter is use to SPARK MAX
   * GUI to tune and save parameters.
   *
   * @param gain The derivative gain value, must be positive
   * @return {@link REVLibError#kOk} if successful
   */
  public REVLibError setD(double gain) {
    sparkMax.throwIfClosed();
    return setD(gain, 0);
  }

  /**
   * Set the Derivative Gain constant of the PIDF controller on the SPARK MAX. This uses the Set
   * Parameter API and should be used infrequently. The parameter does not presist unless
   * burnFlash() is called. The recommended method to configure this parameter is use to SPARK MAX
   * GUI to tune and save parameters.
   *
   * @param gain The derivative gain value, must be positive
   * @param slotID Is the gain schedule slot, the value is a number between 0 and 3. Each slot has
   *     its own set of gain values and can be changed in each control frame using SetReference().
   * @return {@link REVLibError#kOk} if successful
   */
  public REVLibError setD(double gain, int slotID) {
    sparkMax.throwIfClosed();
    return REVLibError.fromInt(
        CANSparkMaxJNI.c_SparkMax_SetD(sparkMax.sparkMaxHandle, slotID, (float) gain));
  }

  /**
   * Set the Derivative Filter constant of the PIDF controller on the SPARK MAX. This uses the Set
   * Parameter API and should be used infrequently. The parameter does not presist unless
   * burnFlash() is called.
   *
   * @param gain The derivative filter value, must be a positive number between 0 and 1
   * @return {@link REVLibError#kOk} if successful
   */
  public REVLibError setDFilter(double gain) {
    sparkMax.throwIfClosed();
    return setDFilter(gain, 0);
  }

  /**
   * Set the Derivative Filter constant of the PIDF controller on the SPARK MAX. This uses the Set
   * Parameter API and should be used infrequently. The parameter does not presist unless
   * burnFlash() is called.
   *
   * @param gain The derivative filter value, must be a positive number between 0 and 1
   * @param slotID Is the gain schedule slot, the value is a number between 0 and 3. Each slot has
   *     its own set of gain values and can be changed in each control frame using SetReference().
   * @return {@link REVLibError#kOk} if successful
   */
  public REVLibError setDFilter(double gain, int slotID) {
    sparkMax.throwIfClosed();
    return REVLibError.fromInt(
        CANSparkMaxJNI.c_SparkMax_SetDFilter(sparkMax.sparkMaxHandle, slotID, (float) gain));
  }

  /**
   * Set the Feed-froward Gain constant of the PIDF controller on the SPARK MAX. This uses the Set
   * Parameter API and should be used infrequently. The parameter does not presist unless
   * burnFlash() is called. The recommended method to configure this parameter is use to SPARK MAX
   * GUI to tune and save parameters.
   *
   * @param gain The feed-forward gain value
   * @return {@link REVLibError#kOk} if successful
   */
  public REVLibError setFF(double gain) {
    sparkMax.throwIfClosed();
    return setFF(gain, 0);
  }

  /**
   * Set the Feed-froward Gain constant of the PIDF controller on the SPARK MAX. This uses the Set
   * Parameter API and should be used infrequently. The parameter does not presist unless
   * burnFlash() is called. The recommended method to configure this parameter is use to SPARK MAX
   * GUI to tune and save parameters.
   *
   * @param gain The feed-forward gain value
   * @param slotID Is the gain schedule slot, the value is a number between 0 and 3. Each slot has
   *     its own set of gain values and can be changed in each control frame using SetReference().
   * @return {@link REVLibError#kOk} if successful
   */
  public REVLibError setFF(double gain, int slotID) {
    sparkMax.throwIfClosed();
    return REVLibError.fromInt(
        CANSparkMaxJNI.c_SparkMax_SetFF(sparkMax.sparkMaxHandle, slotID, (float) gain));
  }

  /**
   * Set the IZone range of the PIDF controller on the SPARK MAX. This value specifies the range the
   * |error| must be within for the integral constant to take effect.
   *
   * <p>This uses the Set Parameter API and should be used infrequently. The parameter does not
   * presist unless burnFlash() is called. The recommended method to configure this parameter is to
   * use the SPARK MAX GUI to tune and save parameters.
   *
   * @param IZone The IZone value, must be positive. Set to 0 to disable
   * @return {@link REVLibError#kOk} if successful
   */
  public REVLibError setIZone(double IZone) {
    sparkMax.throwIfClosed();
    return setIZone(IZone, 0);
  }

  /**
   * Set the IZone range of the PIDF controller on the SPARK MAX. This value specifies the range the
   * |error| must be within for the integral constant to take effect.
   *
   * <p>This uses the Set Parameter API and should be used infrequently. The parameter does not
   * presist unless burnFlash() is called. The recommended method to configure this parameter is to
   * use the SPARK MAX GUI to tune and save parameters.
   *
   * @param IZone The IZone value, must be positive. Set to 0 to disable
   * @param slotID Is the gain schedule slot, the value is a number between 0 and 3. Each slot has
   *     its own set of gain values and can be changed in each control frame using SetReference().
   * @return {@link REVLibError#kOk} if successful
   */
  public REVLibError setIZone(double IZone, int slotID) {
    sparkMax.throwIfClosed();
    return REVLibError.fromInt(
        CANSparkMaxJNI.c_SparkMax_SetIZone(sparkMax.sparkMaxHandle, slotID, (float) IZone));
  }

  /**
   * Set the min amd max output for the closed loop mode.
   *
   * <p>This uses the Set Parameter API and should be used infrequently. The parameter does not
   * presist unless burnFlash() is called. The recommended method to configure this parameter is to
   * use the SPARK MAX GUI to tune and save parameters.
   *
   * @param min Reverse power minimum to allow the controller to output
   * @param max Forward power maximum to allow the controller to output
   * @return {@link REVLibError#kOk} if successful
   */
  public REVLibError setOutputRange(double min, double max) {
    sparkMax.throwIfClosed();
    return setOutputRange(min, max, 0);
  }

  /**
   * Set the min amd max output for the closed loop mode.
   *
   * <p>This uses the Set Parameter API and should be used infrequently. The parameter does not
   * presist unless burnFlash() is called. The recommended method to configure this parameter is to
   * use the SPARK MAX GUI to tune and save parameters.
   *
   * @param min Reverse power minimum to allow the controller to output
   * @param max Forward power maximum to allow the controller to output
   * @param slotID Is the gain schedule slot, the value is a number between 0 and 3. Each slot has
   *     its own set of gain values and can be changed in each control frame using SetReference().
   * @return {@link REVLibError#kOk} if successful
   */
  public REVLibError setOutputRange(double min, double max, int slotID) {
    sparkMax.throwIfClosed();
    return REVLibError.fromInt(
        CANSparkMaxJNI.c_SparkMax_SetOutputRange(
            sparkMax.sparkMaxHandle, slotID, (float) min, (float) max));
  }

  /**
   * Get the Proportional Gain constant of the PIDF controller on the SPARK MAX.
   *
   * <p>This uses the Get Parameter API and should be used infrequently. This function uses a
   * non-blocking call and will return a cached value if the parameter is not returned by the
   * timeout. The timeout can be changed by calling SetCANTimeout(int milliseconds)
   *
   * @return double P Gain value
   */
  public double getP() {
    sparkMax.throwIfClosed();
    return getP(0);
  }

  /**
   * Get the Proportional Gain constant of the PIDF controller on the SPARK MAX.
   *
   * <p>This uses the Get Parameter API and should be used infrequently. This function uses a
   * non-blocking call and will return a cached value if the parameter is not returned by the
   * timeout. The timeout can be changed by calling SetCANTimeout(int milliseconds)
   *
   * @param slotID Is the gain schedule slot, the value is a number between 0 and 3. Each slot has
   *     its own set of gain values and can be changed in each control frame using SetReference().
   * @return double P Gain value
   */
  public double getP(int slotID) {
    sparkMax.throwIfClosed();
    return CANSparkMaxJNI.c_SparkMax_GetP(sparkMax.sparkMaxHandle, slotID);
  }

  /**
   * Get the Integral Gain constant of the PIDF controller on the SPARK MAX.
   *
   * <p>This uses the Get Parameter API and should be used infrequently. This function uses a
   * non-blocking call and will return a cached value if the parameter is not returned by the
   * timeout. The timeout can be changed by calling SetCANTimeout(int milliseconds)
   *
   * @return double I Gain value
   */
  public double getI() {
    sparkMax.throwIfClosed();
    return getI(0);
  }

  /**
   * Get the Integral Gain constant of the PIDF controller on the SPARK MAX.
   *
   * <p>This uses the Get Parameter API and should be used infrequently. This function uses a
   * non-blocking call and will return a cached value if the parameter is not returned by the
   * timeout. The timeout can be changed by calling SetCANTimeout(int milliseconds)
   *
   * @param slotID Is the gain schedule slot, the value is a number between 0 and 3. Each slot has
   *     its own set of gain values and can be changed in each control frame using SetReference().
   * @return double I Gain value
   */
  public double getI(int slotID) {
    sparkMax.throwIfClosed();
    return CANSparkMaxJNI.c_SparkMax_GetI(sparkMax.sparkMaxHandle, slotID);
  }

  /**
   * Get the Derivative Gain constant of the PIDF controller on the SPARK MAX.
   *
   * <p>This uses the Get Parameter API and should be used infrequently. This function uses a
   * non-blocking call and will return a cached value if the parameter is not returned by the
   * timeout. The timeout can be changed by calling SetCANTimeout(int milliseconds)
   *
   * @return double D Gain value
   */
  public double getD() {
    sparkMax.throwIfClosed();
    return getD(0);
  }

  /**
   * Get the Derivative Gain constant of the PIDF controller on the SPARK MAX.
   *
   * <p>This uses the Get Parameter API and should be used infrequently. This function uses a
   * non-blocking call and will return a cached value if the parameter is not returned by the
   * timeout. The timeout can be changed by calling SetCANTimeout(int milliseconds)
   *
   * @param slotID Is the gain schedule slot, the value is a number between 0 and 3. Each slot has
   *     its own set of gain values and can be changed in each control frame using SetReference().
   * @return double D Gain value
   */
  public double getD(int slotID) {
    sparkMax.throwIfClosed();
    return CANSparkMaxJNI.c_SparkMax_GetD(sparkMax.sparkMaxHandle, slotID);
  }

  /**
   * Get the Derivative Filter constant of the PIDF controller on the SPARK MAX.
   *
   * <p>This uses the Get Parameter API and should be used infrequently. This function uses a
   * non-blocking call and will return a cached value if the parameter is not returned by the
   * timeout. The timeout can be changed by calling SetCANTimeout(int milliseconds)
   *
   * @param slotID Is the gain schedule slot, the value is a number between 0 and 3. Each slot has
   *     its own set of gain values and can be changed in each control frame using SetReference().
   * @return double D Filter value
   */
  public double getDFilter(int slotID) {
    sparkMax.throwIfClosed();
    return CANSparkMaxJNI.c_SparkMax_GetDFilter(sparkMax.sparkMaxHandle, slotID);
  }

  /**
   * Get the Feed-forward Gain constant of the PIDF controller on the SPARK MAX.
   *
   * <p>This uses the Get Parameter API and should be used infrequently. This function uses a
   * non-blocking call and will return a cached value if the parameter is not returned by the
   * timeout. The timeout can be changed by calling SetCANTimeout(int milliseconds)
   *
   * @return double F Gain value
   */
  public double getFF() {
    sparkMax.throwIfClosed();
    return getFF(0);
  }

  /**
   * Get the Feed-forward Gain constant of the PIDF controller on the SPARK MAX.
   *
   * <p>This uses the Get Parameter API and should be used infrequently. This function uses a
   * non-blocking call and will return a cached value if the parameter is not returned by the
   * timeout. The timeout can be changed by calling SetCANTimeout(int milliseconds)
   *
   * @param slotID Is the gain schedule slot, the value is a number between 0 and 3. Each slot has
   *     its own set of gain values and can be changed in each control frame using SetReference().
   * @return double F Gain value
   */
  public double getFF(int slotID) {
    sparkMax.throwIfClosed();
    return CANSparkMaxJNI.c_SparkMax_GetFF(sparkMax.sparkMaxHandle, slotID);
  }

  /**
   * Get the IZone constant of the PIDF controller on the SPARK MAX.
   *
   * <p>This uses the Get Parameter API and should be used infrequently. This function uses a
   * non-blocking call and will return a cached value if the parameter is not returned by the
   * timeout. The timeout can be changed by calling SetCANTimeout(int milliseconds)
   *
   * @return double IZone value
   */
  public double getIZone() {
    sparkMax.throwIfClosed();
    return getIZone(0);
  }

  /**
   * Get the IZone constant of the PIDF controller on the SPARK MAX.
   *
   * <p>This uses the Get Parameter API and should be used infrequently. This function uses a
   * non-blocking call and will return a cached value if the parameter is not returned by the
   * timeout. The timeout can be changed by calling SetCANTimeout(int milliseconds)
   *
   * @param slotID Is the gain schedule slot, the value is a number between 0 and 3. Each slot has
   *     its own set of gain values and can be changed in each control frame using SetReference().
   * @return double IZone value
   */
  public double getIZone(int slotID) {
    sparkMax.throwIfClosed();
    return CANSparkMaxJNI.c_SparkMax_GetIZone(sparkMax.sparkMaxHandle, slotID);
  }

  /**
   * Get the derivative filter constant of the PIDF controller on the SPARK MAX.
   *
   * <p>This uses the Get Parameter API and should be used infrequently. This function uses a
   * non-blocking call and will return a cached value if the parameter is not returned by the
   * timeout. The timeout can be changed by calling SetCANTimeout(int milliseconds)
   *
   * @param slotID Is the gain schedule slot, the value is a number between 0 and 3. Each slot has
   *     its own set of gain values and can be changed in each control frame using SetReference().
   * @return double D Filter
   */
  // public double getDFilter(int slotID = 0);

  /**
   * Get the min output of the PIDF controller on the SPARK MAX.
   *
   * <p>This uses the Get Parameter API and should be used infrequently. This function uses a
   * non-blocking call and will return a cached value if the parameter is not returned by the
   * timeout. The timeout can be changed by calling SetCANTimeout(int milliseconds)
   *
   * @return double min value
   */
  public double getOutputMin() {
    sparkMax.throwIfClosed();
    return getOutputMin(0);
  }

  /**
   * Get the min output of the PIDF controller on the SPARK MAX.
   *
   * <p>This uses the Get Parameter API and should be used infrequently. This function uses a
   * non-blocking call and will return a cached value if the parameter is not returned by the
   * timeout. The timeout can be changed by calling SetCANTimeout(int milliseconds)
   *
   * @param slotID Is the gain schedule slot, the value is a number between 0 and 3. Each slot has
   *     its own set of gain values and can be changed in each control frame using SetReference().
   * @return double min value
   */
  public double getOutputMin(int slotID) {
    sparkMax.throwIfClosed();
    return CANSparkMaxJNI.c_SparkMax_GetOutputMin(sparkMax.sparkMaxHandle, slotID);
  }

  /**
   * Get the max output of the PIDF controller on the SPARK MAX.
   *
   * <p>This uses the Get Parameter API and should be used infrequently. This function uses a
   * non-blocking call and will return a cached value if the parameter is not returned by the
   * timeout. The timeout can be changed by calling SetCANTimeout(int milliseconds)
   *
   * @return double max value
   */
  public double getOutputMax() {
    sparkMax.throwIfClosed();
    return getOutputMax(0);
  }

  /**
   * Get the max output of the PIDF controller on the SPARK MAX.
   *
   * <p>This uses the Get Parameter API and should be used infrequently. This function uses a
   * non-blocking call and will return a cached value if the parameter is not returned by the
   * timeout. The timeout can be changed by calling SetCANTimeout(int milliseconds)
   *
   * @param slotID Is the gain schedule slot, the value is a number between 0 and 3. Each slot has
   *     its own set of gain values and can be changed in each control frame using SetReference().
   * @return double max value
   */
  public double getOutputMax(int slotID) {
    sparkMax.throwIfClosed();
    return CANSparkMaxJNI.c_SparkMax_GetOutputMax(sparkMax.sparkMaxHandle, slotID);
  }

  /**
   * Configure the maximum velocity of the SmartMotion mode. This is the velocity that is reached in
   * the middle of the profile and is what the motor should spend most of its time at
   *
   * @param maxVel The maxmimum cruise velocity for the motion profile in RPM
   * @param slotID Is the gain schedule slot, the value is a number between 0 and 3. Each slot has
   *     its own set of gain values and can be changed in each control frame using SetReference().
   * @return {@link REVLibError#kOk} if successful
   */
  public REVLibError setSmartMotionMaxVelocity(double maxVel, int slotID) {
    sparkMax.throwIfClosed();
    return REVLibError.fromInt(
        CANSparkMaxJNI.c_SparkMax_SetSmartMotionMaxVelocity(
            sparkMax.sparkMaxHandle, slotID, (float) maxVel));
  }

  /**
   * Configure the maximum acceleration of the SmartMotion mode. This is the accleration that the
   * motor velocity will increase at until the max velocity is reached
   *
   * @param maxAccel The maxmimum acceleration for the motion profile in RPM per second
   * @param slotID Is the gain schedule slot, the value is a number between 0 and 3. Each slot has
   *     its own set of gain values and can be changed in each control frame using SetReference().
   * @return {@link REVLibError#kOk} if successful
   */
  public REVLibError setSmartMotionMaxAccel(double maxAccel, int slotID) {
    sparkMax.throwIfClosed();
    return REVLibError.fromInt(
        CANSparkMaxJNI.c_SparkMax_SetSmartMotionMaxAccel(
            sparkMax.sparkMaxHandle, slotID, (float) maxAccel));
  }

  /**
   * Configure the mimimum velocity of the SmartMotion mode. Any requested velocities below this
   * value will be set to 0.
   *
   * @param minVel The minimum velocity for the motion profile in RPM
   * @param slotID Is the gain schedule slot, the value is a number between 0 and 3. Each slot has
   *     its own set of gain values and can be changed in each control frame using SetReference().
   * @return {@link REVLibError#kOk} if successful
   */
  public REVLibError setSmartMotionMinOutputVelocity(double minVel, int slotID) {
    sparkMax.throwIfClosed();
    return REVLibError.fromInt(
        CANSparkMaxJNI.c_SparkMax_SetSmartMotionMinOutputVelocity(
            sparkMax.sparkMaxHandle, slotID, (float) minVel));
  }

  /**
   * Configure the allowed closed loop error of SmartMotion mode. This value is how much deviation
   * from your setpoint is tolerated and is useful in preventing oscillation around your setpoint.
   *
   * @param allowedErr The allowed deviation for your setpoint vs actual position in rotations
   * @param slotID Is the gain schedule slot, the value is a number between 0 and 3. Each slot has
   *     its own set of gain values and can be changed in each control frame using SetReference().
   * @return {@link REVLibError#kOk} if successful
   */
  public REVLibError setSmartMotionAllowedClosedLoopError(double allowedErr, int slotID) {
    sparkMax.throwIfClosed();
    return REVLibError.fromInt(
        CANSparkMaxJNI.c_SparkMax_SetSmartMotionAllowedClosedLoopError(
            sparkMax.sparkMaxHandle, slotID, (float) allowedErr));
  }

  /**
   * NOTE: As of the 2022 FRC season, the firmware only supports the trapezoidal motion profiling
   * acceleration strategy.
   *
   * <p>Configure the acceleration strategy used to control acceleration on the motor.
   *
   * @param accelStrategy The acceleration strategy to use for the automatically generated motion
   *     profile
   * @param slotID Is the gain schedule slot, the value is a number between 0 and 3. Each slot has
   *     its own set of gain values and can be changed in each control frame using SetReference().
   * @return {@link REVLibError#kOk} if successful
   */
  public REVLibError setSmartMotionAccelStrategy(AccelStrategy accelStrategy, int slotID) {
    sparkMax.throwIfClosed();
    return REVLibError.fromInt(
        CANSparkMaxJNI.c_SparkMax_SetSmartMotionAccelStrategy(
            sparkMax.sparkMaxHandle, slotID, accelStrategy.value));
  }

  /**
   * NOTE: As of the 2022 FRC season, the firmware only supports the trapezoidal motion profiling
   * acceleration strategy.
   *
   * <p>Configure the acceleration strategy used to control acceleration on the motor. The current
   * strategy is trapezoidal motion profiling.
   *
   * @param accelStrategy The acceleration strategy to use for the automatically generated motion
   *     profile
   * @param slotID Is the gain schedule slot, the value is a number between 0 and 3. Each slot has
   *     its own set of gain values and can be changed in each control frame using SetReference().
   * @return {@link REVLibError#kOk} if successful
   * @deprecated Use {@link #setSmartMotionAccelStrategy(AccelStrategy, int)} instead.
   */
  @SuppressWarnings("removal")
  @Deprecated(forRemoval = true)
  public REVLibError setSmartMotionAccelStrategy(
      CANPIDController.AccelStrategy accelStrategy, int slotID) {
    sparkMax.throwIfClosed();
    return REVLibError.fromInt(
        CANSparkMaxJNI.c_SparkMax_SetSmartMotionAccelStrategy(
            sparkMax.sparkMaxHandle, slotID, accelStrategy.value));
  }

  /**
   * Get the maximum velocity of the SmartMotion mode. This is the velocity that is reached in the
   * middle of the profile and is what the motor should spend most of its time at
   *
   * @param slotID Is the gain schedule slot, the value is a number between 0 and 3. Each slot has
   *     its own set of gain values and can be changed in each control frame using SetReference().
   * @return The maxmimum cruise velocity for the motion profile in RPM
   */
  public double getSmartMotionMaxVelocity(int slotID) {
    sparkMax.throwIfClosed();
    return CANSparkMaxJNI.c_SparkMax_GetSmartMotionMaxVelocity(sparkMax.sparkMaxHandle, slotID);
  }

  /**
   * Get the maximum acceleration of the SmartMotion mode. This is the accleration that the motor
   * velocity will increase at until the max velocity is reached
   *
   * @param slotID Is the gain schedule slot, the value is a number between 0 and 3. Each slot has
   *     its own set of gain values and can be changed in each control frame using SetReference().
   * @return The maxmimum acceleration for the motion profile in RPM per second
   */
  public double getSmartMotionMaxAccel(int slotID) {
    sparkMax.throwIfClosed();
    return CANSparkMaxJNI.c_SparkMax_GetSmartMotionMaxAccel(sparkMax.sparkMaxHandle, slotID);
  }

  /**
   * Get the mimimum velocity of the SmartMotion mode. Any requested velocities below this value
   * will be set to 0.
   *
   * @param slotID Is the gain schedule slot, the value is a number between 0 and 3. Each slot has
   *     its own set of gain values and can be changed in each control frame using SetReference().
   * @return The minimum velocity for the motion profile in RPM
   */
  public double getSmartMotionMinOutputVelocity(int slotID) {
    sparkMax.throwIfClosed();
    return CANSparkMaxJNI.c_SparkMax_GetSmartMotionMinOutputVelocity(
        sparkMax.sparkMaxHandle, slotID);
  }

  /**
   * Get the allowed closed loop error of SmartMotion mode. This value is how much deviation from
   * your setpoint is tolerated and is useful in preventing oscillation around your setpoint.
   *
   * @param slotID Is the gain schedule slot, the value is a number between 0 and 3. Each slot has
   *     its own set of gain values and can be changed in each control frame using SetReference().
   * @return The allowed deviation for your setpoint vs actual position in rotations
   */
  public double getSmartMotionAllowedClosedLoopError(int slotID) {
    sparkMax.throwIfClosed();
    return CANSparkMaxJNI.c_SparkMax_GetSmartMotionAllowedClosedLoopError(
        sparkMax.sparkMaxHandle, slotID);
  }

  /**
   * Get the acceleration strategy used to control acceleration on the motor. As of the 2022 FRC
   * season, the strategy is always trapezoidal motion profiling, regardless of what the device may
   * report.
   *
   * @param slotID Is the gain schedule slot, the value is a number between 0 and 3. Each slot has
   *     its own set of gain values and can be changed in each control frame using SetReference().
   * @return The acceleration strategy to use for the automatically generated motion profile.
   */
  public AccelStrategy getSmartMotionAccelStrategy(int slotID) {
    sparkMax.throwIfClosed();
    return AccelStrategy.fromInt(
        CANSparkMaxJNI.c_SparkMax_GetSmartMotionAccelStrategy(sparkMax.sparkMaxHandle, slotID));
  }

  /**
   * Configure the maximum I accumulator of the PID controller. This value is used to constrain the
   * I accumulator to help manage integral wind-up
   *
   * @param iMaxAccum The max value to contrain the I accumulator to
   * @param slotID Is the gain schedule slot, the value is a number between 0 and 3. Each slot has
   *     its own set of gain values and can be changed in each control frame using SetReference().
   * @return {@link REVLibError#kOk} if successful
   */
  public REVLibError setIMaxAccum(double iMaxAccum, int slotID) {
    sparkMax.throwIfClosed();
    return REVLibError.fromInt(
        CANSparkMaxJNI.c_SparkMax_SetIMaxAccum(sparkMax.sparkMaxHandle, slotID, (float) iMaxAccum));
  }

  /**
   * Get the maximum I accumulator of the PID controller. This value is used to constrain the I
   * accumulator to help manage integral wind-up
   *
   * @param slotID Is the gain schedule slot, the value is a number between 0 and 3. Each slot has
   *     its own set of gain values and can be changed in each control frame using SetReference().
   * @return The max value to contrain the I accumulator to
   */
  public double getIMaxAccum(int slotID) {
    sparkMax.throwIfClosed();
    return (double) CANSparkMaxJNI.c_SparkMax_GetIMaxAccum(sparkMax.sparkMaxHandle, slotID);
  }

  /**
   * Set the I accumulator of the PID controller. This is useful when wishing to force a reset on
   * the I accumulator of the PID controller. You can also preset values to see how it will respond
   * to certain I characteristics
   *
   * <p>To use this function, the controller must be in a closed loop control mode by calling
   * setReference()
   *
   * @param iAccum The value to set the I accumulator to
   * @return {@link REVLibError#kOk} if successful
   */
  public REVLibError setIAccum(double iAccum) {
    sparkMax.throwIfClosed();
    return REVLibError.fromInt(
        CANSparkMaxJNI.c_SparkMax_SetIAccum(sparkMax.sparkMaxHandle, (float) iAccum));
  }

  /**
   * Get the I accumulator of the PID controller. This is useful when wishing to see what the I
   * accumulator value is to help with PID tuning
   *
   * @return The value of the I accumulator
   */
  public double getIAccum() {
    sparkMax.throwIfClosed();
    return CANSparkMaxJNI.c_SparkMax_GetIAccum(sparkMax.sparkMaxHandle);
  }

  /**
   * Set the controller's feedback device
   *
   * <p>The default feedback device in brushless mode is assumed to be the integrated encoder and
   * the default feedback device in brushed mode is assumed to be a quadrature encoder. This is used
   * to changed to another feedback device for the controller, such as an analog sensor.
   *
   * <p>If there is a limited range on the feedback sensor that should be observed by the
   * PIDController, it can be set by calling SetFeedbackSensorRange() on the sensor object.
   *
   * @param sensor The sensor to use as a feedback device
   * @return {@link REVLibError#kOk} if successful
   */
  // TODO(Noah): Figure out what's going on with setFeedbackSensorRange()
  public REVLibError setFeedbackDevice(final MotorFeedbackSensor sensor) {
    //noinspection removal
    return setFeedbackDevice((CANSensor) sensor);
  }

  /** @deprecated Use {@link #setFeedbackDevice(MotorFeedbackSensor)} instead */
  @SuppressWarnings("removal")
  @Deprecated(forRemoval = true)
  public REVLibError setFeedbackDevice(final CANSensor sensor) {
    sparkMax.throwIfClosed();

    if (sensor instanceof SparkMaxRelativeEncoder
        || sensor instanceof SparkMaxAlternateEncoder
        || sensor instanceof SparkMaxAnalogSensor) {
      // Handle sensors that are directly connected to the SPARK Max
      CANSparkMax sparkMaxSensorIsConnectedTo;
      int feedbackDeviceId;

      if (sensor instanceof SparkMaxRelativeEncoder) {
        sparkMaxSensorIsConnectedTo = ((SparkMaxRelativeEncoder) sensor).sparkMax;
        feedbackDeviceId = ((SparkMaxRelativeEncoder) sensor).getSparkMaxFeedbackDeviceId();
      } else if (sensor instanceof SparkMaxAlternateEncoder) {
        sparkMaxSensorIsConnectedTo = ((SparkMaxAlternateEncoder) sensor).sparkMax;
        feedbackDeviceId = ((SparkMaxAlternateEncoder) sensor).getSparkMaxFeedbackDeviceId();
      } else {
        sparkMaxSensorIsConnectedTo = ((SparkMaxAnalogSensor) sensor).sparkMax;
        feedbackDeviceId = ((SparkMaxAnalogSensor) sensor).getSparkMaxFeedbackDeviceId();
      }

      if (sparkMaxSensorIsConnectedTo != this.sparkMax) {
        throw new IllegalArgumentException(
            "A sensor attached to one SPARK MAX cannot be used as a feedback device for a different SPARK MAX");
      }

      return REVLibError.fromInt(
          CANSparkMaxJNI.c_SparkMax_SetFeedbackDevice(sparkMax.sparkMaxHandle, feedbackDeviceId));

    } else {
      // Right now, the SPARK MAX does not support sensors that are not directly connected to itself
      throw new IllegalArgumentException(
          sensor.getClass().getSimpleName()
              + " cannot be used as a feedback device for a SPARK MAX at this time");
    }
  }
}