#include "mgos.h"
#include "tests_autogen.h"
#include "mgos_imu.h"

uint32_t test_imu_period_ms = 100; // 10Hz
bool     test_imu_enabled   = true;

static struct mgos_imu *s_imu = NULL;

#define BOARD    2

#if BOARD == 1
// LSM303D_L3GD20_COMBO -- sold as GY-89
  #define ACC_I2CADDR     0x1d
  #define ACC_TYPE        ACC_LSM303D
  #define GYRO_I2CADDR    0x6b
  #define GYRO_TYPE       GYRO_L3GD20
  #define MAG_I2CADDR     0x1d
  #define MAG_TYPE        MAG_LSM303D
#elif BOARD == 2
// ITG3205_ADXL345_HMC5883L_COMBO -- sold as HW-579
  #define ACC_I2CADDR     0x53
  #define ACC_TYPE        ACC_ADXL345
  #define GYRO_I2CADDR    0x68
  #define GYRO_TYPE       GYRO_ITG3205
  #define MAG_I2CADDR     0x1d
  #define MAG_TYPE        MAG_HMC5883L
#else
// Sold as M5 Stack (ESP32 + MPU9250)
  #define ACC_I2CADDR     0x68
  #define ACC_TYPE        ACC_MPU9250
  #define GYRO_I2CADDR    0x68
  #define GYRO_TYPE       GYRO_MPU9250
  #define MAG_I2CADDR     0x0c
  #define MAG_TYPE        MAG_AK8963
#endif

bool test_imu_create(void) {
  if (!(s_imu = mgos_imu_create())) {
    LOG(LL_ERROR, ("Cannot create IMU"));
    return false;
  }

  if (!mgos_imu_accelerometer_create_i2c(s_imu, mgos_i2c_get_global(), ACC_I2CADDR, ACC_TYPE)) {
    LOG(LL_ERROR, ("Cannot create accelerometer on IMU"));
  }
  if (!mgos_imu_gyroscope_create_i2c(s_imu, mgos_i2c_get_global(), GYRO_I2CADDR, GYRO_TYPE)) {
    LOG(LL_ERROR, ("Cannot create gyroscope on IMU"));
  }
  if (!mgos_imu_magnetometer_create_i2c(s_imu, mgos_i2c_get_global(), MAG_I2CADDR, MAG_TYPE)) {
    LOG(LL_ERROR, ("Cannot create magnetometer on IMU"));
  }

  return true;
}

bool test_imu_run(void) {
  float ax, ay, az;
  float gx, gy, gz;
  float mx, my, mz;

  if (mgos_imu_accelerometer_get(s_imu, &ax, &ay, &az)) {
    LOG(LL_INFO, ("type=%-10s Accel X=%+.2f Y=%+.2f Z=%+.2f (m/s/s)",
                  mgos_imu_accelerometer_get_name(s_imu), ax, ay, az));
  }
  if (mgos_imu_gyroscope_get(s_imu, &gx, &gy, &gz)) {
    LOG(LL_INFO, ("type=%-10s Gyro  X=%+.2f Y=%+.2f Z=%+.2f (Rad/sec)",
                  mgos_imu_gyroscope_get_name(s_imu), gx, gy, gz));
  }
  if (mgos_imu_magnetometer_get(s_imu, &mx, &my, &mz)) {
    LOG(LL_INFO, ("type=%-10s Mag   X=%+.2f Y=%+.2f Z=%+.2f (uTesla)",
                  mgos_imu_magnetometer_get_name(s_imu), mx, my, mz));
  }
  return true;
}

bool test_imu_destroy(void) {
  mgos_imu_destroy(&s_imu);
  return true;
}