/*
 * 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.CANSparkMaxLowLevel.MotorType;
import com.revrobotics.jni.CANSparkMaxJNI;

/**
 * Get an instance of this class by using {@link CANSparkMax#getEncoder()} or {@link
 * CANSparkMax#getEncoder(Type, int)}.
 */
public class SparkMaxRelativeEncoder implements RelativeEncoder {
  public enum Type {
    kNoSensor(0),
    kHallSensor(1),
    kQuadrature(2);

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

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

    public static Type fromId(int id) {
      switch (id) {
        case 1:
          return kHallSensor;
        case 2:
          return kQuadrature;
        default:
          return kNoSensor;
      }
    }
  }

  // package-private to the revrobotics package
  final Type type;
  final int countsPerRev;
  final CANSparkMax sparkMax;

  // package-private to the revrobotics package
  SparkMaxRelativeEncoder(CANSparkMax sparkMax, Type type, int countsPerRev) {
    this.sparkMax = sparkMax;
    this.type = type;
    this.countsPerRev = countsPerRev;

    if (type == null) {
      throw new IllegalArgumentException("encoderType must not be null");
    }

    boolean setCountsPerRevParameter = true;

    if (type == Type.kHallSensor) {
      // The CPR for a hall sensor is hardcoded in the SPARK MAX firmware as 42
      setCountsPerRevParameter = false;

      if (sparkMax.motorType == MotorType.kBrushed) {
        throw new IllegalStateException("A hall sensor cannot be used with a brushed motor");
      }

      // Make sure that the correct CPR value was provided
      if (countsPerRev != 42 && countsPerRev != 0) {
        throw new IllegalArgumentException("countsPerRev must be 42 when using the hall sensor");
      }
    } else { // non-hall sensor
      if (sparkMax.motorType == MotorType.kBrushless) {
        throw new IllegalStateException(
            "The encoder type must be kHallSensor when the SPARK MAX is configured in brushless mode.\n"
                + "To use an external quadrature encoder with a brushless motor, you must wire it as an Alternate Encoder, and then call getAlternateEncoder() on the CANSparkMax object.");
      }

      if (countsPerRev < 1) {
        throw new IllegalArgumentException("countsPerRev must be a positive number");
      }
    }

    CANSparkMaxJNI.c_SparkMax_SetSensorType(sparkMax.sparkMaxHandle, type.value);
    if (setCountsPerRevParameter) {
      CANSparkMaxJNI.c_SparkMax_SetCountsPerRevolution(sparkMax.sparkMaxHandle, countsPerRev);
    }
  }

  @Override
  public double getPosition() {
    sparkMax.throwIfClosed();
    return CANSparkMaxJNI.c_SparkMax_GetEncoderPosition(sparkMax.sparkMaxHandle);
  }

  @Override
  public double getVelocity() {
    sparkMax.throwIfClosed();
    return CANSparkMaxJNI.c_SparkMax_GetEncoderVelocity(sparkMax.sparkMaxHandle);
  }

  @Override
  public REVLibError setPosition(double position) {
    sparkMax.throwIfClosed();
    return REVLibError.fromInt(
        CANSparkMaxJNI.c_SparkMax_SetEncoderPosition(sparkMax.sparkMaxHandle, (float) position));
  }

  @Override
  public REVLibError setPositionConversionFactor(double factor) {
    sparkMax.throwIfClosed();
    return REVLibError.fromInt(
        CANSparkMaxJNI.c_SparkMax_SetPositionConversionFactor(
            sparkMax.sparkMaxHandle, (float) factor));
  }

  @Override
  public REVLibError setVelocityConversionFactor(double factor) {
    sparkMax.throwIfClosed();
    return REVLibError.fromInt(
        CANSparkMaxJNI.c_SparkMax_SetVelocityConversionFactor(
            sparkMax.sparkMaxHandle, (float) factor));
  }

  @Override
  public double getPositionConversionFactor() {
    sparkMax.throwIfClosed();
    return CANSparkMaxJNI.c_SparkMax_GetPositionConversionFactor(sparkMax.sparkMaxHandle);
  }

  @Override
  public double getVelocityConversionFactor() {
    sparkMax.throwIfClosed();
    return CANSparkMaxJNI.c_SparkMax_GetVelocityConversionFactor(sparkMax.sparkMaxHandle);
  }

  @Override
  public REVLibError setAverageDepth(int depth) {
    sparkMax.throwIfClosed();
    return REVLibError.fromInt(
        CANSparkMaxJNI.c_SparkMax_SetAverageDepth(sparkMax.sparkMaxHandle, depth));
  }

  @Override
  public int getAverageDepth() {
    sparkMax.throwIfClosed();
    return CANSparkMaxJNI.c_SparkMax_GetAverageDepth(sparkMax.sparkMaxHandle);
  }

  @Override
  public REVLibError setMeasurementPeriod(int period_us) {
    sparkMax.throwIfClosed();
    return REVLibError.fromInt(
        CANSparkMaxJNI.c_SparkMax_SetMeasurementPeriod(sparkMax.sparkMaxHandle, period_us));
  }

  @Override
  public int getMeasurementPeriod() {
    sparkMax.throwIfClosed();
    return CANSparkMaxJNI.c_SparkMax_GetMeasurementPeriod(sparkMax.sparkMaxHandle);
  }

  @Override
  public int getCountsPerRevolution() {
    sparkMax.throwIfClosed();
    return CANSparkMaxJNI.c_SparkMax_GetCountsPerRevolution(sparkMax.sparkMaxHandle);
  }

  @Override
  public REVLibError setInverted(boolean inverted) {
    sparkMax.throwIfClosed();
    if (sparkMax.getMotorType() == MotorType.kBrushless) {
      throw new IllegalArgumentException(
          "The encoder cannot be inverted separately from the motor in Brushless Mode");
    }
    return REVLibError.fromInt(
        CANSparkMaxJNI.c_SparkMax_SetEncoderInverted(sparkMax.sparkMaxHandle, inverted));
  }

  @Override
  public boolean getInverted() {
    sparkMax.throwIfClosed();
    return CANSparkMaxJNI.c_SparkMax_GetEncoderInverted(sparkMax.sparkMaxHandle);
  }

  // package-private to the revrobotics package
  int getSparkMaxFeedbackDeviceId() {
    return (sparkMax.getMotorType() == MotorType.kBrushless)
        ? CANSparkMaxLowLevel.FeedbackSensorType.kHallSensor.value
        : CANSparkMaxLowLevel.FeedbackSensorType.kQuadrature.value;
  }
}