// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc4388.utility;

import com.ctre.phoenix.sensors.PigeonIMU;
import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode;
import com.kauailabs.navx.frc.AHRS;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.util.sendable.SendableBuilder;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.PIDSourceType;
import edu.wpi.first.wpilibj.interfaces.Gyro;

/**
 * Gyro class that allows for interchangeable use between a pigeon and a navX
 */
public class RobotGyro implements Gyro, PIDSource, Sendable {
  private RobotTime m_robotTime = RobotTime.getInstance();

  private PigeonIMU m_pigeon = null;
  private AHRS m_navX = null;
  public boolean m_isGyroAPigeon; // true if pigeon, false if navX

  private double m_lastPigeonAngle;
  private double m_deltaPigeonAngle;

  /**
   * Creates a Gyro based on a pigeon
   * 
   * @param gyro the gyroscope to use for Gyro
   */
  public RobotGyro(PigeonIMU gyro) {
    m_pigeon = gyro;
    m_isGyroAPigeon = true;
  }

  /**
   * Creates a Gyro based on a navX
   * 
   * @param gyro the gyroscope to use for Gyro
   */
  public RobotGyro(AHRS gyro) {
    m_navX = gyro;
    m_isGyroAPigeon = false;
  }

  /**
   * Run in periodic if you are using a pigeon. Updates a delta angle so that it
   * can calculate getRate(). Note
   * that the getRate() method for a navX will likely be much more accurate than
   * for a pigeon.
   */
  public void updatePigeonDeltas() {
    double currentPigeonAngle = getAngle();
    m_deltaPigeonAngle = currentPigeonAngle - m_lastPigeonAngle;
    m_lastPigeonAngle = currentPigeonAngle;
  }

  /**
   * <p>
   * NavX:
   * <p>
   * Calibrate the gyro by running for a number of samples and computing the
   * center value. Then use
   * the center value as the Accumulator center value for subsequent measurements.
   * It's important to
   * make sure that the robot is not moving while the centering calculations are
   * in progress, this
   * is typically done when the robot is first turned on while it's sitting at
   * rest before the
   * competition starts.
   * 
   * <p>
   * Pigeon:
   * <p>
   * Calibrate the gyro by collecting data at a range of tempuratures. Allow
   * pigeon to cool, then boot
   * into calibration mode. For faster calibration, use a heat lamp to heat up the
   * pigeon. Once the pigeon
   * has seen a reasonable range of tempuratures, it will exit calibration mode.
   * It's important to
   * make sure that the robot is not moving while the tempurature calculations are
   * in progress, this
   * is typically done when the robot is first turned on while it's sitting at
   * rest before the
   * competition starts.
   */
  @Override
  public void calibrate() {
    if (m_isGyroAPigeon)
      m_pigeon.enterCalibrationMode(CalibrationMode.Temperature);
    else
      m_navX.calibrate();
  }

  @Override
  public void reset() {
    if (m_isGyroAPigeon)
      m_pigeon.setYaw(0);
    else
      m_navX.reset();
  }

  /**
   * Get Yaw, Pitch, and Roll data.
   *
   * @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data.
   *         Yaw is within [-368,640, +368,640] degrees.
   *         Pitch is within [-90,+90] degrees.
   *         Roll is within [-90,+90] degrees.
   */
  private double[] getPigeonAngles() {
    double[] angles = new double[3];
    m_pigeon.getYawPitchRoll(angles);
    return angles;
  }

  @Override
  public double getAngle() {
    if (m_isGyroAPigeon) {
      return getPigeonAngles()[0];
    } else {
      return m_navX.getAngle();
    }
  }

  /**
   * Gets an absolute heading of the robot
   * 
   * @return heading from -180 to 180 degrees
   */
  public double getHeading() {
    return getHeading(getAngle());
  }

  /**
   * Gets an absolute heading of the robot
   * 
   * @return heading from -180 to 180 degrees
   */
  public double getHeading(double angle) {
    return Math.IEEEremainder(angle, 360);
  }

  /**
   * Returns the current pitch value (in degrees, from -90 to 90)
   * reported by the sensor. Pitch is a measure of rotation around
   * the Y Axis.
   * 
   * @return The current pitch value in degrees (-90 to 90).
   */
  public double getPitch() {
    if (m_isGyroAPigeon) {
      return MathUtil.clamp(getPigeonAngles()[1], -90, 90);
    } else {
      return MathUtil.clamp(m_navX.getPitch(), -90, 90);
    }
  }

  /**
   * Returns the current roll value (in degrees, from -90 to 90)
   * reported by the sensor. Roll is a measure of rotation around
   * the X Axis.
   * 
   * @return The current roll value in degrees (-90 to 90).
   */
  public double getRoll() {
    if (m_isGyroAPigeon) {
      return MathUtil.clamp(getPigeonAngles()[2], -90, 90);
    } else {
      return MathUtil.clamp(m_navX.getRoll(), -90, 90);
    }
  }

  @Override
  public double getRate() {
    if (m_isGyroAPigeon) {
      return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000;
    } else {
      return m_navX.getRate();
    }
  }

  public PigeonIMU getPigeon() {
    return m_pigeon;
  }

  public AHRS getNavX() {
    return m_navX;
  }

  @Override
  public void close() throws Exception {

  }

  // Begin old GyroBase class
  private PIDSourceType m_pidSource = PIDSourceType.kDisplacement;

  /**
   * Set which parameter of the gyro you are using as a process control variable.
   * The Gyro class
   * supports the rate and displacement parameters
   *
   * @param pidSource An enum to select the parameter.
   */
  @Override
  public void setPIDSourceType(PIDSourceType pidSource) {
    m_pidSource = pidSource;
  }

  @Override
  public PIDSourceType getPIDSourceType() {
    return m_pidSource;
  }

  /**
   * Get the output of the gyro for use with PIDControllers. May be the angle or
   * rate depending on
   * the set PIDSourceType
   *
   * @return the output according to the gyro
   */
  @Override
  public double pidGet() {
    switch (m_pidSource) {
      case kRate:
        return getRate();
      case kDisplacement:
        return getAngle();
      default:
        return 0.0;
    }
  }

  @Override
  public void initSendable(SendableBuilder builder) {
    builder.setSmartDashboardType("Gyro");
    builder.addDoubleProperty("Value", this::getAngle, null);
  }
}