// 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.

#include "wpi/xrp/XRPGyro.hpp"

#include "wpi/units/angle.hpp"
#include "wpi/units/angular_velocity.hpp"

using namespace wpi::xrp;

XRPGyro::XRPGyro() : m_simDevice("Gyro:XRPGyro") {
  if (m_simDevice) {
    m_simDevice.CreateBoolean("init", hal::SimDevice::Direction::OUTPUT, true);
    m_simRateX = m_simDevice.CreateDouble(
        "rate_x", hal::SimDevice::Direction::INPUT, 0.0);
    m_simRateY = m_simDevice.CreateDouble(
        "rate_y", hal::SimDevice::Direction::INPUT, 0.0);
    m_simRateZ = m_simDevice.CreateDouble(
        "rate_z", hal::SimDevice::Direction::INPUT, 0.0);
    m_simAngleX = m_simDevice.CreateDouble(
        "angle_x", hal::SimDevice::Direction::INPUT, 0.0);
    m_simAngleY = m_simDevice.CreateDouble(
        "angle_y", hal::SimDevice::Direction::INPUT, 0.0);
    m_simAngleZ = m_simDevice.CreateDouble(
        "angle_z", hal::SimDevice::Direction::INPUT, 0.0);
  }
}

wpi::units::radian_t XRPGyro::GetAngle() const {
  return GetAngleZ();
}

wpi::math::Rotation2d XRPGyro::GetRotation2d() const {
  return wpi::math::Rotation2d{GetAngle()};
}

wpi::units::radians_per_second_t XRPGyro::GetRate() const {
  return GetRateZ();
}

wpi::units::radians_per_second_t XRPGyro::GetRateX() const {
  if (m_simRateX) {
    return wpi::units::degrees_per_second_t{m_simRateX.Get()};
  }

  return 0_rad_per_s;
}

wpi::units::radians_per_second_t XRPGyro::GetRateY() const {
  if (m_simRateY) {
    return wpi::units::degrees_per_second_t{m_simRateY.Get()};
  }

  return 0_rad_per_s;
}

wpi::units::radians_per_second_t XRPGyro::GetRateZ() const {
  if (m_simRateZ) {
    return wpi::units::degrees_per_second_t{m_simRateZ.Get()};
  }

  return 0_rad_per_s;
}

wpi::units::radian_t XRPGyro::GetAngleX() const {
  if (m_simAngleX) {
    return wpi::units::degree_t{m_simAngleX.Get()} - m_angleXOffset;
  }

  return 0_rad;
}

wpi::units::radian_t XRPGyro::GetAngleY() const {
  if (m_simAngleY) {
    return wpi::units::degree_t{m_simAngleY.Get()} - m_angleYOffset;
  }

  return 0_rad;
}

wpi::units::radian_t XRPGyro::GetAngleZ() const {
  if (m_simAngleZ) {
    return wpi::units::degree_t{m_simAngleZ.Get()} - m_angleZOffset;
  }

  return 0_rad;
}

void XRPGyro::Reset() {
  if (m_simAngleX) {
    m_angleXOffset = wpi::units::degree_t{m_simAngleX.Get()};
    m_angleYOffset = wpi::units::degree_t{m_simAngleY.Get()};
    m_angleZOffset = wpi::units::degree_t{m_simAngleZ.Get()};
  }
}
