/*
 * Copyright (c) 2018-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;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.motorcontrol.MotorController;
import java.nio.ByteBuffer;
import java.util.concurrent.atomic.AtomicBoolean;

public abstract class CANSparkMaxLowLevel
    implements SpeedController, MotorController, AutoCloseable {
  // TODO(Noah): Deprecate and introduce correctly named constants in a more appropriate spot
  public static final int kAPIMajorVersion = CANSparkMaxJNI.c_SparkMax_GetAPIMajorRevision();
  public static final int kAPIMinorVersion = CANSparkMaxJNI.c_SparkMax_GetAPIMinorRevision();
  public static final int kAPIBuildVersion = CANSparkMaxJNI.c_SparkMax_GetAPIBuildRevision();
  public static final int kAPIVersion = CANSparkMaxJNI.c_SparkMax_GetAPIVersion();

  public enum MotorType {
    kBrushed(0),
    kBrushless(1);

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

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

    public static MotorType fromId(int id) {
      for (MotorType type : values()) {
        if (type.value == id) {
          return type;
        }
      }
      return null;
    }
  }

  public enum PeriodicFrame {
    kStatus0(0),
    kStatus1(1),
    kStatus2(2),
    kStatus3(3);

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

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

    public static PeriodicFrame fromId(int id) {
      for (PeriodicFrame type : values()) {
        if (type.value == id) {
          return type;
        }
      }
      return null;
    }
  }

  public class PeriodicStatus0 {
    public double appliedOutput;
    public short faults;
    public short stickyFaults;
    public byte lock;
    public MotorType motorType;
    public boolean isFollower;
    public boolean isInverted;
    public boolean roboRIO;
  }

  public class PeriodicStatus1 {
    public double sensorVelocity;
    public byte motorTemperature;
    public double busVoltage;
    public double outputCurrent;
  }

  public class PeriodicStatus2 {
    public double sensorPosition;
    public double iAccum;
  }

  protected final long sparkMaxHandle;
  protected final AtomicBoolean isClosed = new AtomicBoolean(false);
  private final int deviceId;
  private String firmwareString;
  protected final MotorType motorType;

  /**
   * Create a new SPARK MAX Controller
   *
   * <p>Package-private, so that only other classes in our package can create
   *
   * @param deviceId The device ID
   * @param type The motor type connected to the controller. Brushless motors must be connected to
   *     their matching color and the hall sensor plugged in. Brushed motors must be connected to
   *     the Red and Black terminals only.
   */
  CANSparkMaxLowLevel(int deviceId, MotorType type) {
    if (type == null) {
      throw new IllegalArgumentException("type must not be null");
    }
    this.deviceId = deviceId;
    firmwareString = "";
    motorType = type;
    if (CANSparkMaxJNI.c_SparkMax_RegisterId(deviceId) == REVLibError.kDuplicateCANId.value) {
      throw new IllegalStateException(
          "A CANSparkMax instance has already been created with this device ID: " + deviceId);
    }
    sparkMaxHandle = CANSparkMaxJNI.c_SparkMax_Create(deviceId, type.value);
  }

  /** Closes the SPARK MAX Controller */
  @Override
  public void close() {
    boolean alreadyClosed = isClosed.getAndSet(true);
    if (alreadyClosed) {
      return;
    }
    CANSparkMaxJNI.c_SparkMax_Destroy(sparkMaxHandle);
  }

  /**
   * Get the firmware version of the SPARK MAX.
   *
   * @return uint32_t Firmware version integer. Value is represented as 4 bytes, Major.Minor.Build
   *     H.Build L
   */
  // TODO(Noah): Add a function that returns the different version fields as an object
  public int getFirmwareVersion() {
    throwIfClosed();
    return CANSparkMaxJNI.c_SparkMax_GetFirmwareVersion(sparkMaxHandle);
  }

  /**
   * Set the control frame send period for the native CAN Send thread.
   *
   * @param periodMs The send period in milliseconds between 1ms and 100ms or set to 0 to disable
   *     periodic sends. Note this is not updated until the next call to Set() or SetReference().
   */
  public void setControlFramePeriodMs(int periodMs) {
    throwIfClosed();
    CANSparkMaxJNI.c_SparkMax_SetControlFramePeriod(sparkMaxHandle, periodMs);
  }

  /**
   * Get the firmware version of the SPARK MAX as a string.
   *
   * @return std::string Human readable firmware version string
   */
  public String getFirmwareString() {
    throwIfClosed();
    if (firmwareString == "") {
      int version = getFirmwareVersion();
      ByteBuffer b = ByteBuffer.allocate(4);
      b.putInt(version);

      byte[] verBytes = b.array();

      StringBuilder firmwareString = new StringBuilder();
      firmwareString
          .append("v")
          .append((int) verBytes[0])
          .append(".")
          .append((int) verBytes[1])
          .append(".")
          .append((int) verBytes[2] << 8 | (int) verBytes[3]);

      this.firmwareString = firmwareString.toString();
    }
    return firmwareString;
  }

  /**
   * Get the unique serial number of the SPARK MAX. Not currently available.
   *
   * @return byte[] Vector of bytes representig the unique serial number
   */
  public byte[] getSerialNumber() {
    throwIfClosed();
    return new byte[0];
  }

  /**
   * Get the configured Device ID of the SPARK MAX.
   *
   * @return int device ID
   */
  public int getDeviceId() {
    throwIfClosed();
    return deviceId;
  }

  /**
   * Get the motor type setting for the SPARK MAX
   *
   * @return MotorType Motor type setting
   * @deprecated Use {@link #getMotorType()} instead
   */
  @Deprecated(forRemoval = true)
  public MotorType getInitialMotorType() {
    throwIfClosed();
    return motorType;
  }

  /**
   * Get the motor type setting for the SPARK MAX
   *
   * @return MotorType Motor type setting
   */
  public MotorType getMotorType() {
    throwIfClosed();
    return motorType;
  }

  /**
   * Set the rate of transmission for periodic frames from the SPARK MAX
   *
   * <p>Each motor controller sends back three status frames with different data at set rates. Use
   * this function to change the default rates.
   *
   * <p>Defaults: Status0 - 10ms Status1 - 20ms Status2 - 50ms
   *
   * <p>This value is not stored in the FLASH after calling burnFlash() and is reset on powerup.
   *
   * <p>Refer to the SPARK MAX reference manual on details for how and when to configure this
   * parameter.
   *
   * @param frameID The frame ID can be one of PeriodicFrame type
   * @param periodMs The rate the controller sends the frame to the controller.
   * @return {@link REVLibError#kOk} if successful
   */
  public REVLibError setPeriodicFramePeriod(PeriodicFrame frameID, int periodMs) {
    throwIfClosed();
    return REVLibError.fromInt(
        CANSparkMaxJNI.c_SparkMax_SetPeriodicFramePeriod(sparkMaxHandle, frameID.value, periodMs));
  }

  /**
   * Allow external controllers to recieve control commands over USB. For example, a configuration
   * where the heartbeat (and enable/disable) is sent by the main controller, but control frames are
   * sent by other CAN devices over USB.
   *
   * <p>This is global for all controllers on the same bus.
   *
   * <p>This does not disable sending control frames from this device. To prevent conflicts, do not
   * enable this feature and also send Set() for SetReference() from the controllers you wish to
   * control.
   *
   * @param enable Enable or disable external control
   */
  public static void enableExternalUSBControl(boolean enable) {
    CANSparkMaxJNI.c_SparkMax_EnableExternalControl(enable);
  }

  /**
   * Send enabled or disabled command to controllers. This is global for all controllers on the same
   * bus, and will only work for non-roboRIO targets in non-competiton use. This function will also
   * not work if a roboRIO is present on the CAN bus.
   *
   * <p>This does not disable sending control frames from this device. To prevent conflicts, do not
   * enable this feature and also send Set() for SetReference() from the controllers you wish to
   * control.
   *
   * @param enable Enable or disable external control
   */
  static void setEnable(boolean enable) {
    CANSparkMaxJNI.c_SparkMax_SetEnable(enable);
  }

  REVLibError setFollow(FollowConfig follower) {
    throwIfClosed();
    return REVLibError.fromInt(
        CANSparkMaxJNI.c_SparkMax_SetFollow(
            sparkMaxHandle, follower.leaderArbId, follower.config.getRaw()));
  }

  REVLibError setpointCommand(double value) {
    throwIfClosed();
    return setpointCommand(value, CANSparkMax.ControlType.kDutyCycle);
  }

  REVLibError setpointCommand(double value, CANSparkMax.ControlType ctrl) {
    throwIfClosed();
    return setpointCommand(value, ctrl, 0);
  }

  /** @deprecated Use {@link #setpointCommand(double, CANSparkMax.ControlType)} instead. */
  @Deprecated(forRemoval = true)
  @SuppressWarnings("removal")
  REVLibError setpointCommand(double value, ControlType ctrl) {
    throwIfClosed();
    return setpointCommand(value, ctrl, 0);
  }

  REVLibError setpointCommand(double value, CANSparkMax.ControlType ctrl, int pidSlot) {
    throwIfClosed();
    return setpointCommand(value, ctrl, pidSlot, 0);
  }

  /** @deprecated Use {@link #setpointCommand(double, CANSparkMax.ControlType, int)} instead. */
  @Deprecated(forRemoval = true)
  @SuppressWarnings("removal")
  REVLibError setpointCommand(double value, ControlType ctrl, int pidSlot) {
    throwIfClosed();
    return setpointCommand(value, ctrl, pidSlot, 0);
  }

  REVLibError setpointCommand(
      double value, CANSparkMax.ControlType ctrl, int pidSlot, double arbFeedforward) {
    throwIfClosed();
    return setpointCommand(value, ctrl, pidSlot, arbFeedforward, 0);
  }

  /**
   * @deprecated Use {@link #setpointCommand(double, CANSparkMax.ControlType, int, double)} instead.
   */
  @Deprecated(forRemoval = true)
  @SuppressWarnings("removal")
  REVLibError setpointCommand(double value, ControlType ctrl, int pidSlot, double arbFeedforward) {
    throwIfClosed();
    return setpointCommand(value, ctrl, pidSlot, arbFeedforward, 0);
  }

  // TODO(Noah): Deprecate all setpointCommand() methods in favor of the corresponding methods in
  // SparkMaxPIDController.
  //             Next year, we can set them all to be protected.
  REVLibError setpointCommand(
      double value,
      CANSparkMax.ControlType ctrl,
      int pidSlot,
      double arbFeedforward,
      int arbFFUnits) {
    throwIfClosed();
    return REVLibError.fromInt(
        CANSparkMaxJNI.c_SparkMax_SetpointCommand(
            sparkMaxHandle,
            (float) value,
            ctrl.value,
            pidSlot,
            (float) arbFeedforward,
            arbFFUnits));
  }

  /**
   * @deprecated Use {@link #setpointCommand(double, CANSparkMax.ControlType, int, double, int)}
   *     instead.
   */
  @Deprecated(forRemoval = true)
  @SuppressWarnings("removal")
  REVLibError setpointCommand(
      double value, ControlType ctrl, int pidSlot, double arbFeedforward, int arbFFUnits) {
    throwIfClosed();

    return REVLibError.fromInt(
        CANSparkMaxJNI.c_SparkMax_SetpointCommand(
            sparkMaxHandle,
            (float) value,
            ctrl.value,
            pidSlot,
            (float) arbFeedforward,
            arbFFUnits));
  }

  public float getSafeFloat(float f) {
    throwIfClosed();
    if (Float.isNaN(f) || Float.isInfinite(f)) return 0;

    return f;
  }

  /**
   * Restore motor controller parameters to factory default until the next controller reboot
   *
   * @return {@link REVLibError#kOk} if successful
   */
  public REVLibError restoreFactoryDefaults() {
    throwIfClosed();
    return restoreFactoryDefaults(false);
  }

  /**
   * Restore motor controller parameters to factory default
   *
   * @param persist If true, burn the flash with the factory default parameters
   * @return {@link REVLibError#kOk} if successful
   */
  public REVLibError restoreFactoryDefaults(boolean persist) {
    throwIfClosed();
    return REVLibError.fromInt(
        CANSparkMaxJNI.c_SparkMax_RestoreFactoryDefaults(sparkMaxHandle, persist));
  }

  protected void throwIfClosed() {
    if (isClosed.get()) {
      throw new IllegalStateException("This SPARK MAX object has previously been closed.");
    }
  }

  protected static class FollowConfig {
    int leaderArbId;
    Config config;

    public static class Config {
      public int rsvd1;
      public int invert;
      public int rsvd2;
      public int predefined;

      public int getRaw() {
        return (rsvd1 & 0x3FFFF)
            | (invert & 0x1) << 18
            | (rsvd2 & 0x1F) << 19
            | (predefined & 0xFF) << 24;
      }
    }

    FollowConfig() {
      config = new Config();
    }
  }

  // package-private to the revrobotics package
  enum FeedbackSensorType {
    kNoSensor(0),
    kHallSensor(1),
    kQuadrature(2),
    kSensorless(3),
    kAnalog(4),
    kAlternateQuadrature(5);

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

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