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

/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-2020 Analog Devices Inc. All Rights Reserved.           */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*                                                                            */
/* Juan Chong - frcsupport@analog.com                                         */
/*----------------------------------------------------------------------------*/

#include <frc/ADIS16470_IMU.h>
#include <frc/DigitalInput.h>
#include <frc/DigitalSource.h>
#include <frc/DriverStation.h>
#include <frc/Timer.h>

#include <cmath>
#include <iostream>
#include <string>

#include <hal/HAL.h>
#include <networktables/NTSendableBuilder.h>
#include <wpi/numbers>
#include <wpi/sendable/SendableRegistry.h>

#include "frc/Errors.h"

/* Helpful conversion functions */
static inline int32_t ToInt(const uint32_t* buf) {
  return static_cast<int32_t>((buf[0] << 24) | (buf[1] << 16) | (buf[2] << 8) |
                              buf[3]);
}

static inline int16_t BuffToShort(const uint32_t* buf) {
  return (static_cast<int16_t>(buf[0]) << 8) | buf[1];
}

static inline uint16_t ToUShort(const uint8_t* buf) {
  return (static_cast<uint16_t>(buf[0]) << 8) | buf[1];
}

using namespace frc;

namespace {
template <typename S, typename... Args>
inline void ADISReportError(int32_t status, const char* file, int line,
                            const char* function, const S& format,
                            Args&&... args) {
  frc::ReportErrorV(status, file, line, function, format,
                    fmt::make_args_checked<Args...>(format, args...));
}
}  // namespace

#define REPORT_WARNING(msg) \
  ADISReportError(warn::Warning, __FILE__, __LINE__, __FUNCTION__, "{}", msg);
#define REPORT_ERROR(msg) \
  ADISReportError(err::Error, __FILE__, __LINE__, __FUNCTION__, "{}", msg)

/**
 * Constructor.
 */
ADIS16470_IMU::ADIS16470_IMU()
    : ADIS16470_IMU(kZ, SPI::Port::kOnboardCS0, CalibrationTime::_4s) {}

ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, SPI::Port port,
                             CalibrationTime cal_time)
    : m_yaw_axis(yaw_axis),
      m_spi_port(port),
      m_calibration_time(static_cast<uint16_t>(cal_time)),
      m_simDevice("Gyro:ADIS16470", port) {
  if (m_simDevice) {
    m_simGyroAngleX =
        m_simDevice.CreateDouble("gyro_angle_x", hal::SimDevice::kInput, 0.0);
    m_simGyroAngleY =
        m_simDevice.CreateDouble("gyro_angle_y", hal::SimDevice::kInput, 0.0);
    m_simGyroAngleZ =
        m_simDevice.CreateDouble("gyro_angle_z", hal::SimDevice::kInput, 0.0);
    m_simGyroRateX =
        m_simDevice.CreateDouble("gyro_rate_x", hal::SimDevice::kInput, 0.0);
    m_simGyroRateY =
        m_simDevice.CreateDouble("gyro_rate_y", hal::SimDevice::kInput, 0.0);
    m_simGyroRateZ =
        m_simDevice.CreateDouble("gyro_rate_z", hal::SimDevice::kInput, 0.0);
    m_simAccelX =
        m_simDevice.CreateDouble("accel_x", hal::SimDevice::kInput, 0.0);
    m_simAccelY =
        m_simDevice.CreateDouble("accel_y", hal::SimDevice::kInput, 0.0);
    m_simAccelZ =
        m_simDevice.CreateDouble("accel_z", hal::SimDevice::kInput, 0.0);
  }

  if (!m_simDevice) {
    // Force the IMU reset pin to toggle on startup (doesn't require DS enable)
    // Relies on the RIO hardware by default configuring an output as low
    // and configuring an input as high Z. The 10k pull-up resistor internal to
    // the IMU then forces the reset line high for normal operation.
    DigitalOutput* m_reset_out =
        new DigitalOutput(27);  // Drive SPI CS2 (IMU RST) low
    Wait(10_ms);                // Wait 10ms
    delete m_reset_out;
    new DigitalInput(27);  // Set SPI CS2 (IMU RST) high
    Wait(500_ms);          // Wait 500ms for reset to complete

    // Configure standard SPI
    if (!SwitchToStandardSPI()) {
      return;
    }

    // Set IMU internal decimation to 4 (output data rate of 2000 SPS / (4 + 1)
    // = 400Hz)
    WriteRegister(DEC_RATE, 0x0004);
    // Set data ready polarity (HIGH = Good Data), Disable gSense Compensation
    // and PoP
    WriteRegister(MSC_CTRL, 0x0001);
    // Configure IMU internal Bartlett filter
    WriteRegister(FILT_CTRL, 0x0000);
    // Configure continuous bias calibration time based on user setting
    WriteRegister(NULL_CNFG, m_calibration_time | 0x700);

    // Notify DS that IMU calibration delay is active
    REPORT_WARNING(
        "ADIS16470 IMU Detected. Starting initial calibration delay.");

    // Wait for samples to accumulate internal to the IMU (110% of user-defined
    // time)
    Wait(units::second_t{pow(2, m_calibration_time) / 2000 * 64 * 1.1});

    // Write offset calibration command to IMU
    WriteRegister(GLOB_CMD, 0x0001);

    // Configure and enable auto SPI
    if (!SwitchToAutoSPI()) {
      return;
    }

    // Let the user know the IMU was initiallized successfully
    REPORT_WARNING("ADIS16470 IMU Successfully Initialized!");

    // Drive SPI CS3 (IMU ready LED) low (active low)
    new DigitalOutput(28);
  }

  // Report usage and post data to DS
  HAL_Report(HALUsageReporting::kResourceType_ADIS16470, 0);

  wpi::SendableRegistry::AddLW(this, "ADIS16470", port);
}

/**
 * @brief Switches to standard SPI operation. Primarily used when exiting auto
 *SPI mode.
 *
 * @return A boolean indicating the success or failure of setting up the SPI
 *peripheral in standard SPI mode.
 *
 * This function switches the active SPI port to standard SPI and is used
 *primarily when exiting auto SPI. Exiting auto SPI is required to read or write
 *using SPI since the auto SPI configuration, once active, locks the SPI message
 *being transacted. This function also verifies that the SPI port is operating
 *in standard SPI mode by reading back the IMU product ID.
 **/
bool ADIS16470_IMU::SwitchToStandardSPI() {
  // Check to see whether the acquire thread is active. If so, wait for it to
  // stop producing data.
  if (m_thread_active) {
    m_thread_active = false;
    while (!m_thread_idle) {
      Wait(10_ms);
    }
    std::cout << "Paused the IMU processing thread successfully!" << std::endl;
    // Maybe we're in auto SPI mode? If so, kill auto SPI, and then SPI.
    if (m_spi != nullptr && m_auto_configured) {
      m_spi->StopAuto();
      // We need to get rid of all the garbage left in the auto SPI buffer after
      // stopping it. Sometimes data magically reappears, so we have to check
      // the buffer size a couple of times
      //  to be sure we got it all. Yuck.
      uint32_t trashBuffer[200];
      Wait(100_ms);
      int data_count = m_spi->ReadAutoReceivedData(trashBuffer, 0, 0_s);
      while (data_count > 0) {
        /* Receive data, max of 200 words at a time (prevent potential segfault)
         */
        m_spi->ReadAutoReceivedData(trashBuffer, std::min(data_count, 200),
                                    0_s);
        /*Get the reamining data count */
        data_count = m_spi->ReadAutoReceivedData(trashBuffer, 0, 0_s);
      }
      std::cout << "Paused the auto SPI successfully!" << std::endl;
    }
  }
  // There doesn't seem to be a SPI port active. Let's try to set one up
  if (m_spi == nullptr) {
    std::cout << "Setting up a new SPI port." << std::endl;
    m_spi = new SPI(m_spi_port);
    m_spi->SetClockRate(2000000);
    m_spi->SetMSBFirst();
    m_spi->SetSampleDataOnTrailingEdge();
    m_spi->SetClockActiveLow();
    m_spi->SetChipSelectActiveLow();
    ReadRegister(PROD_ID);  // Dummy read

    // Validate the product ID
    uint16_t prod_id = ReadRegister(PROD_ID);
    if (prod_id != 16982 && prod_id != 16470) {
      REPORT_ERROR("Could not find ADIS16470!");
      Close();
      return false;
    }
    return true;
  } else {
    // Maybe the SPI port is active, but not in auto SPI mode? Try to read the
    // product ID.
    ReadRegister(PROD_ID);  // Dummy read
    uint16_t prod_id = ReadRegister(PROD_ID);
    if (prod_id != 16982 && prod_id != 16470) {
      REPORT_ERROR("Could not find ADIS16470!");
      Close();
      return false;
    } else {
      return true;
    }
  }
}

/**
 * @brief Switches to auto SPI operation. Primarily used when exiting standard
 *SPI mode.
 *
 * @return A boolean indicating the success or failure of setting up the SPI
 *peripheral in auto SPI mode.
 *
 * This function switches the active SPI port to auto SPI and is used primarily
 *when exiting standard SPI. Auto SPI is required to asynchronously read data
 *over SPI as it utilizes special FPGA hardware to react to an external data
 *ready (GPIO) input. Data captured using auto SPI is buffered in the FPGA and
 *can be read by the CPU asynchronously. Standard SPI transactions are
 * impossible on the selected SPI port once auto SPI is enabled. The stall
 *settings, GPIO interrupt pin, and data packet settings used in this function
 *are hard-coded to work only with the ADIS16470 IMU.
 **/
bool ADIS16470_IMU::SwitchToAutoSPI() {
  // No SPI port has been set up. Go set one up first.
  if (m_spi == nullptr) {
    if (!SwitchToStandardSPI()) {
      REPORT_ERROR("Failed to start/restart auto SPI");
      return false;
    }
  }
  // Only set up the interrupt if needed.
  if (m_auto_interrupt == nullptr) {
    m_auto_interrupt = new DigitalInput(26);
  }
  // The auto SPI controller gets angry if you try to set up two instances on
  // one bus.
  if (!m_auto_configured) {
    m_spi->InitAuto(8200);
    m_auto_configured = true;
  }
  // Do we need to change auto SPI settings?
  switch (m_yaw_axis) {
    case kX:
      m_spi->SetAutoTransmitData(m_autospi_x_packet, 2);
      break;
    case kY:
      m_spi->SetAutoTransmitData(m_autospi_y_packet, 2);
      break;
    default:
      m_spi->SetAutoTransmitData(m_autospi_z_packet, 2);
      break;
  }
  // Configure auto stall time
  m_spi->ConfigureAutoStall(HAL_SPI_kOnboardCS0, 5, 1000, 1);
  // Kick off DMA SPI (Note: Device configration impossible after SPI DMA is
  // activated) DR High = Data good (data capture should be triggered on the
  // rising edge)
  m_spi->StartAutoTrigger(*m_auto_interrupt, true, false);
  // Check to see if the acquire thread is running. If not, kick one off.
  if (!m_thread_idle) {
    m_first_run = true;
    m_thread_active = true;
    m_acquire_task = std::thread(&ADIS16470_IMU::Acquire, this);
    std::cout << "New IMU Processing thread activated!" << std::endl;
  } else {
    m_first_run = true;
    m_thread_active = true;
    std::cout << "Old IMU Processing thread re-activated!" << std::endl;
  }
  // Looks like the thread didn't start for some reason. Abort.
  /*
  if(!m_thread_idle) {
    REPORT_ERROR("Failed to start/restart the acquire() thread.");
    Close();
    return false;
  }
  */
  return true;
}

/**
 * @brief Switches the active SPI port to standard SPI mode, writes a new value
 *to the NULL_CNFG register in the IMU, and re-enables auto SPI.
 *
 * @param new_cal_time Calibration time to be set.
 *
 * @return An int indicating the success or failure of writing the new NULL_CNFG
 *setting and returning to auto SPI mode. 0 = Success, 1 = No Change, 2 =
 *Failure
 *
 * This function enters standard SPI mode, writes a new NULL_CNFG setting to the
 *IMU, and re-enters auto SPI mode. This function does not include a blocking
 *sleep, so the user must keep track of the elapsed offset calibration time
 * themselves. After waiting the configured calibration time, the Calibrate()
 *function should be called to activate the new offset calibration.
 **/
int ADIS16470_IMU::ConfigCalTime(CalibrationTime new_cal_time) {
  if (m_calibration_time == static_cast<uint16_t>(new_cal_time)) {
    return 1;
  }
  if (!SwitchToStandardSPI()) {
    REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
    return 2;
  }
  m_calibration_time = static_cast<uint16_t>(new_cal_time);
  WriteRegister(NULL_CNFG, m_calibration_time | 0x700);
  if (!SwitchToAutoSPI()) {
    REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
    return 2;
  }
  return 0;
}

/**
 * @brief Switches the active SPI port to standard SPI mode, writes a new value
 *to the DECIMATE register in the IMU, and re-enables auto SPI.
 *
 * @param reg Decimation value to be set.
 *
 * @return An int indicating the success or failure of writing the new DECIMATE
 *setting and returning to auto SPI mode. 0 = Success, 1 = No Change, 2 =
 *Failure
 *
 * This function enters standard SPI mode, writes a new DECIMATE setting to the
 *IMU, adjusts the sample scale factor, and re-enters auto SPI mode.
 **/
int ADIS16470_IMU::ConfigDecRate(uint16_t reg) {
  uint16_t m_reg = reg;
  if (!SwitchToStandardSPI()) {
    REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
    return 2;
  }
  if (m_reg > 1999) {
    REPORT_ERROR("Attempted to write an invalid decimation value.");
    m_reg = 1999;
  }
  m_scaled_sample_rate = (((m_reg + 1.0) / 2000.0) * 1000000.0);
  WriteRegister(DEC_RATE, m_reg);
  if (!SwitchToAutoSPI()) {
    REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
    return 2;
  }
  return 0;
}

/**
 * @brief Switches the active SPI port to standard SPI mode, writes the command
 *to activate the new null configuration, and re-enables auto SPI.
 *
 * This function enters standard SPI mode, writes 0x0001 to the GLOB_CMD
 *register (thus making the new offset active in the IMU), and re-enters auto
 *SPI mode. This function does not include a blocking sleep, so the user must
 *keep track of the elapsed offset calibration time themselves.
 **/
void ADIS16470_IMU::Calibrate() {
  if (!SwitchToStandardSPI()) {
    REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
  }
  WriteRegister(GLOB_CMD, 0x0001);
  if (!SwitchToAutoSPI()) {
    REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
  }
}

int ADIS16470_IMU::SetYawAxis(IMUAxis yaw_axis) {
  if (m_yaw_axis == yaw_axis) {
    return 1;
  }
  if (!SwitchToStandardSPI()) {
    REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
    return 2;
  }
  m_yaw_axis = yaw_axis;
  if (!SwitchToAutoSPI()) {
    REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
    return 2;
  }
  return 0;
}

/**
 * @brief Reads the contents of a specified register location over SPI.
 *
 * @param reg An unsigned, 8-bit register location.
 *
 * @return An unsigned, 16-bit number representing the contents of the requested
 *register location.
 *
 * This function reads the contents of an 8-bit register location by
 *transmitting the register location byte along with a null (0x00) byte using
 *the standard WPILib API. The response (two bytes) is read back using the
 *WPILib API and joined using a helper function. This function assumes the
 *controller is set to standard SPI mode.
 **/
uint16_t ADIS16470_IMU::ReadRegister(uint8_t reg) {
  uint8_t buf[2];
  buf[0] = reg & 0x7f;
  buf[1] = 0;

  m_spi->Write(buf, 2);
  m_spi->Read(false, buf, 2);

  return ToUShort(buf);
}

/**
 * @brief Writes an unsigned, 16-bit value to two adjacent, 8-bit register
 *locations over SPI.
 *
 * @param reg An unsigned, 8-bit register location.
 *
 * @param val An unsigned, 16-bit value to be written to the specified register
 *location.
 *
 * This function writes an unsigned, 16-bit value into adjacent 8-bit addresses
 *via SPI. The upper and lower bytes that make up the 16-bit value are split
 *into two unsined, 8-bit values and written to the upper and lower addresses of
 *the specified register value. Only the lower (base) address must be specified.
 *This function assumes the controller is set to standard SPI mode.
 **/
void ADIS16470_IMU::WriteRegister(uint8_t reg, uint16_t val) {
  uint8_t buf[2];
  buf[0] = 0x80 | reg;
  buf[1] = val & 0xff;
  m_spi->Write(buf, 2);
  buf[0] = 0x81 | reg;
  buf[1] = val >> 8;
  m_spi->Write(buf, 2);
}

/**
 * @brief Resets (zeros) the xgyro, ygyro, and zgyro angle integrations.
 *
 * This function resets (zeros) the accumulated (integrated) angle estimates for
 *the xgyro, ygyro, and zgyro outputs.
 **/
void ADIS16470_IMU::Reset() {
  std::scoped_lock sync(m_mutex);
  m_integ_angle = 0.0;
}

void ADIS16470_IMU::Close() {
  if (m_thread_active) {
    m_thread_active = false;
    if (m_acquire_task.joinable()) {
      m_acquire_task.join();
    }
  }
  if (m_spi != nullptr) {
    if (m_auto_configured) {
      m_spi->StopAuto();
    }
    delete m_spi;
    m_auto_configured = false;
    if (m_auto_interrupt != nullptr) {
      delete m_auto_interrupt;
      m_auto_interrupt = nullptr;
    }
    m_spi = nullptr;
  }
  std::cout << "Finished cleaning up after the IMU driver." << std::endl;
}

ADIS16470_IMU::~ADIS16470_IMU() {
  Close();
}

/**
 * @brief Main acquisition loop. Typically called asynchronously and free-wheels
 *while the robot code is active.
 *
 * This is the main acquisiton loop for the IMU. During each iteration, data
 *read using auto SPI is extracted from the FPGA FIFO, split, scaled, and
 *integrated. Each X, Y, and Z value is 32-bits split across 4 indices (bytes)
 *in the buffer. Auto SPI puts one byte in each index location. Each index is
 *32-bits wide because the timestamp is an unsigned 32-bit int. The timestamp is
 *always located at the beginning of the frame. Two indices (request_1 and
 *request_2 below) are always invalid (garbage) and can be disregarded.
 *
 * Data order: [timestamp, request_1, request_2, x_1, x_2, x_3, x_4, y_1, y_2,
 *y_3, y_4, z_1, z_2, z_3, z_4] x = delta x (32-bit x_1 = highest bit) y = delta
 *y (32-bit y_1 = highest bit) z = delta z (32-bit z_1 = highest bit)
 *
 * Complementary filter code was borrowed from
 *https://github.com/tcleg/Six_Axis_Complementary_Filter
 **/
void ADIS16470_IMU::Acquire() {
  // Set data packet length
  const int dataset_len = 19;  // 18 data points + timestamp

  /* Fixed buffer size */
  const int BUFFER_SIZE = 4000;

  // This buffer can contain many datasets
  uint32_t buffer[BUFFER_SIZE];
  int data_count = 0;
  int data_remainder = 0;
  int data_to_read = 0;
  uint32_t previous_timestamp = 0;
  double delta_angle = 0.0;
  double gyro_x = 0.0;
  double gyro_y = 0.0;
  double gyro_z = 0.0;
  double accel_x = 0.0;
  double accel_y = 0.0;
  double accel_z = 0.0;
  double gyro_x_si = 0.0;
  double gyro_y_si = 0.0;
  // double gyro_z_si = 0.0;
  double accel_x_si = 0.0;
  double accel_y_si = 0.0;
  double accel_z_si = 0.0;
  double compAngleX = 0.0;
  double compAngleY = 0.0;
  double accelAngleX = 0.0;
  double accelAngleY = 0.0;

  while (true) {
    // Sleep loop for 10ms (wait for data)
    Wait(10_ms);

    if (m_thread_active) {
      m_thread_idle = false;

      data_count = m_spi->ReadAutoReceivedData(
          buffer, 0,
          0_s);  // Read number of bytes currently stored in the buffer
      data_remainder =
          data_count % dataset_len;  // Check if frame is incomplete. Add 1
                                     // because of timestamp
      data_to_read = data_count -
                     data_remainder;  // Remove incomplete data from read count
      /* Want to cap the data to read in a single read at the buffer size */
      if (data_to_read > BUFFER_SIZE) {
        REPORT_WARNING(
            "ADIS16470 data processing thread overrun has occurred!");
        data_to_read = BUFFER_SIZE - (BUFFER_SIZE % dataset_len);
      }
      m_spi->ReadAutoReceivedData(
          buffer, data_to_read,
          0_s);  // Read data from DMA buffer (only complete sets)

      /*
      // DEBUG: Print buffer size and contents to terminal
      std::cout << "Start - " << data_count << "," << data_remainder << "," <<
      data_to_read << std::endl; for (int m = 0; m < data_to_read - 1; m++ )
      {
        std::cout << buffer[m] << ",";
      }
      std::cout << " " << std::endl;
      std::cout << "End" << std::endl;
      std::cout << "Reading " << data_count << " bytes." << std::endl;
      */

      // Could be multiple data sets in the buffer. Handle each one.
      for (int i = 0; i < data_to_read; i += dataset_len) {
        // Timestamp is at buffer[i]
        m_dt = (buffer[i] - previous_timestamp) / 1000000.0;
        /* Get delta angle value for selected yaw axis and scale by the elapsed
         * time (based on timestamp) */
        delta_angle = (ToInt(&buffer[i + 3]) * delta_angle_sf) /
                      (m_scaled_sample_rate / (buffer[i] - previous_timestamp));
        gyro_x = (BuffToShort(&buffer[i + 7]) / 10.0);
        gyro_y = (BuffToShort(&buffer[i + 9]) / 10.0);
        gyro_z = (BuffToShort(&buffer[i + 11]) / 10.0);
        accel_x = (BuffToShort(&buffer[i + 13]) / 800.0);
        accel_y = (BuffToShort(&buffer[i + 15]) / 800.0);
        accel_z = (BuffToShort(&buffer[i + 17]) / 800.0);

        // Convert scaled sensor data to SI units
        gyro_x_si = gyro_x * deg_to_rad;
        gyro_y_si = gyro_y * deg_to_rad;
        // gyro_z_si = gyro_z * deg_to_rad;
        accel_x_si = accel_x * grav;
        accel_y_si = accel_y * grav;
        accel_z_si = accel_z * grav;

        // Store timestamp for next iteration
        previous_timestamp = buffer[i];

        /*
        // DEBUG: Print timestamp and delta values
        std::cout << previous_timestamp << "," << delta_x << "," << delta_y <<
        "," << delta_z << std::endl;
        */

        m_alpha = m_tau / (m_tau + m_dt);

        if (m_first_run) {
          accelAngleX = atan2f(accel_x_si, sqrtf((accel_y_si * accel_y_si) +
                                                 (accel_z_si * accel_z_si)));
          accelAngleY = atan2f(accel_y_si, sqrtf((accel_x_si * accel_x_si) +
                                                 (accel_z_si * accel_z_si)));
          compAngleX = accelAngleX;
          compAngleY = accelAngleY;
        } else {
          // Process X angle
          accelAngleX = atan2f(accel_x_si, sqrtf((accel_y_si * accel_y_si) +
                                                 (accel_z_si * accel_z_si)));
          accelAngleY = atan2f(accel_y_si, sqrtf((accel_x_si * accel_x_si) +
                                                 (accel_z_si * accel_z_si)));
          accelAngleX = FormatAccelRange(accelAngleX, accel_z_si);
          accelAngleY = FormatAccelRange(accelAngleY, accel_z_si);
          compAngleX = CompFilterProcess(compAngleX, accelAngleX, -gyro_y_si);
          compAngleY = CompFilterProcess(compAngleY, accelAngleY, gyro_x_si);
        }

        // DEBUG: Print accumulated values
        // std::cout << m_compAngleX << "," << m_compAngleY << std::endl;

        {
          std::scoped_lock sync(m_mutex);
          /* Push data to global variables */
          if (m_first_run) {
            /* Don't accumulate first run. previous_timestamp will be "very" old
             * and the integration will end up way off */
            m_integ_angle = 0.0;
          } else {
            m_integ_angle += delta_angle;
          }
          m_gyro_x = gyro_x;
          m_gyro_y = gyro_y;
          m_gyro_z = gyro_z;
          m_accel_x = accel_x;
          m_accel_y = accel_y;
          m_accel_z = accel_z;
          m_compAngleX = compAngleX * rad_to_deg;
          m_compAngleY = compAngleY * rad_to_deg;
          m_accelAngleX = accelAngleX * rad_to_deg;
          m_accelAngleY = accelAngleY * rad_to_deg;
        }
        m_first_run = false;
      }
    } else {
      m_thread_idle = true;
      data_count = 0;
      data_remainder = 0;
      data_to_read = 0;
      previous_timestamp = 0.0;
      delta_angle = 0.0;
      gyro_x = 0.0;
      gyro_y = 0.0;
      gyro_z = 0.0;
      accel_x = 0.0;
      accel_y = 0.0;
      accel_z = 0.0;
      gyro_x_si = 0.0;
      gyro_y_si = 0.0;
      // gyro_z_si = 0.0;
      accel_x_si = 0.0;
      accel_y_si = 0.0;
      accel_z_si = 0.0;
      compAngleX = 0.0;
      compAngleY = 0.0;
      accelAngleX = 0.0;
      accelAngleY = 0.0;
    }
  }
}

/* Complementary filter functions */
double ADIS16470_IMU::FormatFastConverge(double compAngle, double accAngle) {
  if (compAngle > accAngle + wpi::numbers::pi) {
    compAngle = compAngle - 2.0 * wpi::numbers::pi;
  } else if (accAngle > compAngle + wpi::numbers::pi) {
    compAngle = compAngle + 2.0 * wpi::numbers::pi;
  }
  return compAngle;
}

double ADIS16470_IMU::FormatRange0to2PI(double compAngle) {
  while (compAngle >= 2 * wpi::numbers::pi) {
    compAngle = compAngle - 2.0 * wpi::numbers::pi;
  }
  while (compAngle < 0.0) {
    compAngle = compAngle + 2.0 * wpi::numbers::pi;
  }
  return compAngle;
}

double ADIS16470_IMU::FormatAccelRange(double accelAngle, double accelZ) {
  if (accelZ < 0.0) {
    accelAngle = wpi::numbers::pi - accelAngle;
  } else if (accelZ > 0.0 && accelAngle < 0.0) {
    accelAngle = 2.0 * wpi::numbers::pi + accelAngle;
  }
  return accelAngle;
}

double ADIS16470_IMU::CompFilterProcess(double compAngle, double accelAngle,
                                        double omega) {
  compAngle = FormatFastConverge(compAngle, accelAngle);
  compAngle =
      m_alpha * (compAngle + omega * m_dt) + (1.0 - m_alpha) * accelAngle;
  compAngle = FormatRange0to2PI(compAngle);
  if (compAngle > wpi::numbers::pi) {
    compAngle = compAngle - 2.0 * wpi::numbers::pi;
  }
  return compAngle;
}

/**
 * @brief Returns the current integrated angle for the axis specified.
 *
 * @param m_yaw_axis An enum indicating the axis chosen to act as the yaw axis.
 *
 * @return The current integrated angle in degrees.
 *
 * This function returns the most recent integrated angle for the axis chosen by
 *m_yaw_axis. This function is most useful in situations where the yaw axis may
 *not coincide with the IMU Z axis.
 **/
double ADIS16470_IMU::GetAngle() const {
  switch (m_yaw_axis) {
    case kX:
      if (m_simGyroAngleX) {
        return m_simGyroAngleX.Get();
      }
      break;
    case kY:
      if (m_simGyroAngleY) {
        return m_simGyroAngleY.Get();
      }
      break;
    case kZ:
      if (m_simGyroAngleZ) {
        return m_simGyroAngleZ.Get();
      }
      break;
  }
  std::scoped_lock sync(m_mutex);
  return m_integ_angle;
}

double ADIS16470_IMU::GetRate() const {
  std::scoped_lock sync(m_mutex);
  switch (m_yaw_axis) {
    case kX:
      return GetGyroRateX();
    case kY:
      return GetGyroRateY();
    case kZ:
      return GetGyroRateZ();
    default:
      return 0.0;
  }
}

ADIS16470_IMU::IMUAxis ADIS16470_IMU::GetYawAxis() const {
  return m_yaw_axis;
}

double ADIS16470_IMU::GetGyroRateX() const {
  if (m_simGyroRateX) {
    return m_simGyroRateX.Get();
  }
  std::scoped_lock sync(m_mutex);
  return m_gyro_x;
}

double ADIS16470_IMU::GetGyroRateY() const {
  if (m_simGyroRateY) {
    return m_simGyroRateY.Get();
  }
  std::scoped_lock sync(m_mutex);
  return m_gyro_y;
}

double ADIS16470_IMU::GetGyroRateZ() const {
  if (m_simGyroRateZ) {
    return m_simGyroRateZ.Get();
  }
  std::scoped_lock sync(m_mutex);
  return m_gyro_z;
}

double ADIS16470_IMU::GetAccelX() const {
  if (m_simAccelX) {
    return m_simAccelX.Get();
  }
  std::scoped_lock sync(m_mutex);
  return m_accel_x;
}

double ADIS16470_IMU::GetAccelY() const {
  if (m_simAccelY) {
    return m_simAccelY.Get();
  }
  std::scoped_lock sync(m_mutex);
  return m_accel_y;
}

double ADIS16470_IMU::GetAccelZ() const {
  if (m_simAccelZ) {
    return m_simAccelZ.Get();
  }
  std::scoped_lock sync(m_mutex);
  return m_accel_z;
}

double ADIS16470_IMU::GetXComplementaryAngle() const {
  std::scoped_lock sync(m_mutex);
  return m_compAngleX;
}

double ADIS16470_IMU::GetYComplementaryAngle() const {
  std::scoped_lock sync(m_mutex);
  return m_compAngleY;
}

double ADIS16470_IMU::GetXFilteredAccelAngle() const {
  std::scoped_lock sync(m_mutex);
  return m_accelAngleX;
}

double ADIS16470_IMU::GetYFilteredAccelAngle() const {
  std::scoped_lock sync(m_mutex);
  return m_accelAngleY;
}

int ADIS16470_IMU::GetPort() const {
  return m_spi_port;
}

/**
 * @brief Builds a Sendable object to push IMU data to the driver station.
 *
 * This function pushes the most recent angle estimates for all axes to the
 *driver station.
 **/
void ADIS16470_IMU::InitSendable(nt::NTSendableBuilder& builder) {
  builder.SetSmartDashboardType("ADIS16470 IMU");
  auto yaw_angle = builder.GetEntry("Yaw Angle").GetHandle();
  builder.SetUpdateTable(
      [=]() { nt::NetworkTableEntry(yaw_angle).SetDouble(GetAngle()); });
}