/*!   @file Adafruit_ICM20948.cpp
 */
#include "Arduino.h"
#include <Wire.h>

#include "Adafruit_ICM20948.h"
#include "Adafruit_ICM20X.h"

/*!
 *    @brief  Instantiates a new ICM20948 class!
 */

Adafruit_ICM20948::Adafruit_ICM20948(void) {}
/*!
 *    @brief  Sets up the hardware and initializes I2C
 *    @param  i2c_address
 *            The I2C address to be used.
 *    @param  wire
 *            The Wire object to be used for I2C connections.
 *    @param  sensor_id
 *            An optional parameter to set the sensor ids to differentiate
 * similar sensors The passed value is assigned to the accelerometer and the
 * gyro get +1 and the temperature sensor +2.
 *    @return True if initialization was successful, otherwise false.
 */
bool Adafruit_ICM20948::begin_I2C(uint8_t i2c_address, TwoWire *wire,
                                  int32_t sensor_id) {

  if (i2c_dev) {
    delete i2c_dev; // remove old interface
  }

  i2c_dev = new Adafruit_I2CDevice(i2c_address, wire);

  if (!i2c_dev->begin()) {
    Serial.println("I2C begin Failed");
    return false;
  }
  bool init_success = _init(sensor_id);
  if (!setupMag()) {
    Serial.println("failed to setup mag");
    return false;
  }

  return init_success;
}

// A million thanks to the SparkFun folks for their library that I pillaged to
// write this method! See their Arduino library here:
// https://github.com/sparkfun/SparkFun_ICM-20948_ArduinoLibrary
bool Adafruit_ICM20948::auxI2CBusSetupFailed(void) {
  // check aux I2C bus connection by reading the magnetometer chip ID
  bool aux_i2c_setup_failed = true;
  for (int i = 0; i < I2C_MASTER_RESETS_BEFORE_FAIL; i++) {
    if (getMagId() != ICM20948_MAG_ID) {
      resetI2CMaster();
    } else {
      aux_i2c_setup_failed = false;
      break;
    }
  }
  return aux_i2c_setup_failed;
}

uint8_t Adafruit_ICM20948::getMagId(void) {
  // verify the magnetometer id
  return readExternalRegister(0x8C, 0x01);
}

bool Adafruit_ICM20948::setupMag(void) {
  uint8_t buffer[2];

  setI2CBypass(false);

  configureI2CMaster();

  enableI2CMaster(true);

  if (auxI2CBusSetupFailed()) {
    return false;
  }

  // set mag data rate
  if (!setMagDataRate(AK09916_MAG_DATARATE_100_HZ)) {
    Serial.println("Error setting magnetometer data rate on external bus");
    return false;
  }

  // TODO: extract method
  // Set up Slave0 to proxy Mag readings
  _setBank(3);
  // set up slave0 to proxy reads to mag
  buffer[0] = ICM20X_B3_I2C_SLV0_ADDR;
  buffer[1] = 0x8C;
  if (!i2c_dev->write(buffer, 2)) {
    return false;
  }

  buffer[0] = ICM20X_B3_I2C_SLV0_REG;
  buffer[1] = 0x10;
  if (!i2c_dev->write(buffer, 2)) {
    return false;
  }

  buffer[0] = ICM20X_B3_I2C_SLV0_CTRL;
  buffer[1] = 0x89; // enable, read 9 bytes
  if (!i2c_dev->write(buffer, 2)) {
    return false;
  }

  return true;
}

/**
 * @brief
 *
 * @param slv_addr
 * @param mag_reg_addr
 * @param num_finished_checks
 * @return uint8_t
 */
uint8_t Adafruit_ICM20948::readMagRegister(uint8_t mag_reg_addr) {
  return readExternalRegister(0x8C, mag_reg_addr);
}

bool Adafruit_ICM20948::writeMagRegister(uint8_t mag_reg_addr, uint8_t value) {
  return writeExternalRegister(0x0C, mag_reg_addr, value);
}

void Adafruit_ICM20948::scaleValues(void) {

  icm20948_gyro_range_t gyro_range = (icm20948_gyro_range_t)current_gyro_range;
  icm20948_accel_range_t accel_range =
      (icm20948_accel_range_t)current_accel_range;

  float accel_scale = 1.0;
  float gyro_scale = 1.0;

  if (gyro_range == ICM20948_GYRO_RANGE_250_DPS)
    gyro_scale = 131.0;
  if (gyro_range == ICM20948_GYRO_RANGE_500_DPS)
    gyro_scale = 65.5;
  if (gyro_range == ICM20948_GYRO_RANGE_1000_DPS)
    gyro_scale = 32.8;
  if (gyro_range == ICM20948_GYRO_RANGE_2000_DPS)
    gyro_scale = 16.4;

  if (accel_range == ICM20948_ACCEL_RANGE_2_G)
    accel_scale = 16384.0;
  if (accel_range == ICM20948_ACCEL_RANGE_4_G)
    accel_scale = 8192.0;
  if (accel_range == ICM20948_ACCEL_RANGE_8_G)
    accel_scale = 4096.0;
  if (accel_range == ICM20948_ACCEL_RANGE_16_G)
    accel_scale = 2048.0;

  gyroX = rawGyroX / gyro_scale;
  gyroY = rawGyroY / gyro_scale;
  gyroZ = rawGyroZ / gyro_scale;

  accX = rawAccX / accel_scale;
  accY = rawAccY / accel_scale;
  accZ = rawAccZ / accel_scale;

  magX = rawMagX * ICM20948_UT_PER_LSB;
  magY = rawMagY * ICM20948_UT_PER_LSB;
  magZ = rawMagZ * ICM20948_UT_PER_LSB;
}

/**************************************************************************/
/*!
    @brief Get the accelerometer's measurement range.
    @returns The accelerometer's measurement range (`icm20948_accel_range_t`).
*/
icm20948_accel_range_t Adafruit_ICM20948::getAccelRange(void) {
  return (icm20948_accel_range_t)readAccelRange();
}

/**************************************************************************/
/*!

    @brief Sets the accelerometer's measurement range.
    @param  new_accel_range
            Measurement range to be set. Must be an
            `icm20948_accel_range_t`.
*/
void Adafruit_ICM20948::setAccelRange(icm20948_accel_range_t new_accel_range) {
  writeAccelRange((uint8_t)new_accel_range);
}

/**************************************************************************/
/*!
    @brief Get the gyro's measurement range.
    @returns The gyro's measurement range (`icm20948_gyro_range_t`).
*/
icm20948_gyro_range_t Adafruit_ICM20948::getGyroRange(void) {
  return (icm20948_gyro_range_t)readGyroRange();
}

/**************************************************************************/
/*!

    @brief Sets the gyro's measurement range.
    @param  new_gyro_range
            Measurement range to be set. Must be an
            `icm20948_gyro_range_t`.
*/
void Adafruit_ICM20948::setGyroRange(icm20948_gyro_range_t new_gyro_range) {
  writeGyroRange((uint8_t)new_gyro_range);
}

/**
 * @brief Get the current magnetometer measurement rate
 *
 * @return ak09916_data_rate_t the current rate
 */
ak09916_data_rate_t Adafruit_ICM20948::getMagDataRate(void) {

  uint8_t raw_mag_rate = readMagRegister(AK09916_CNTL2);
  return (ak09916_data_rate_t)(raw_mag_rate);
}
/**
 * @brief Set the magnetometer measurement rate
 *
 * @param rate The rate to set.
 *
 * @return true: success false: failure
 */
bool Adafruit_ICM20948::setMagDataRate(ak09916_data_rate_t rate) {
  /*
   * Following the datasheet, the sensor will be set to
   * AK09916_MAG_DATARATE_SHUTDOWN followed by a 100ms delay, followed by
   * setting the new data rate.
   *
   * See page 9 of https://www.y-ic.es/datasheet/78/SMDSW.020-2OZ.pdf
   */

  // don't need to read/mask because there's nothing else in the register and
  // it's right justified
  bool success = writeMagRegister(AK09916_CNTL2, AK09916_MAG_DATARATE_SHUTDOWN);
  delay(1);
  return writeMagRegister(AK09916_CNTL2, rate) && success;
}