#include "GY87.h"
#include "board.h"
#include "print.h"
#include "delay.h"
  
#define ADX_SCALE 0.00239420166015625  // 4mg/LSB    16/63356*9.80665
#define GRY_SCALE 0.001065264417860243  //rad/s   4000/65536*(PI/180)
#define HMC5883L_SCALE 0.92  // mG/LSb

#define WRITE_INTERVAL 20

bool GY87::init()
{
    delay_ms(500);
#if IMU_DEBUG_ENABLE
	log("GY87::init");
#endif
    Board::get()->i2c_init();

    if (!mpu6050.testConnection()) {
#if IMU_DEBUG_ENABLE
        log("MPU6050 NOT FOUND!");
#endif
        return false;
    }

    mpu6050.initialize();
    delay_ms(WRITE_INTERVAL);

#if IMU_DEBUG_ENABLE
    uint8_t buffer=0;
    Board::get()->i2c_read_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, &buffer);
    log("MPU6050_RA_PWR_MGMT_1=%d", buffer);
    Board::get()->i2c_read_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_GYRO_CONFIG, &buffer);
    log("MPU6050_RA_GYRO_CONFIG=%d", buffer);
    Board::get()->i2c_read_byte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_CONFIG, &buffer);
    log("MPU6050_RA_ACCEL_CONFIG=%d", buffer);
#endif

    if (!mag.testConnection()) {
#if IMU_DEBUG_ENABLE
        log("HMC5883L NOT FOUND!");
#endif
        return false;
    }

    mag.initialize();
    delay_ms(WRITE_INTERVAL);
    mag.setGain(HMC5883L_GAIN_1090);
    delay_ms(WRITE_INTERVAL);
    mag.setDataRate(HMC5883L_RATE_75);
    delay_ms(WRITE_INTERVAL);
    mag.setSampleAveraging(HMC5883L_AVERAGING_1);
    delay_ms(WRITE_INTERVAL);
    mag.setMode(HMC5883L_MODE_SINGLE);
    delay_ms(WRITE_INTERVAL);

#if IMU_DEBUG_ENABLE
    Board::get()->i2c_read_byte(HMC5883L_DEFAULT_ADDRESS, HMC5883L_RA_CONFIG_B, &buffer);
    log("HMC5883L_RA_CONFIG_B=%d", buffer);
    Board::get()->i2c_read_byte(HMC5883L_DEFAULT_ADDRESS, HMC5883L_RA_CONFIG_A, &buffer);
    log("HMC5883L_RA_CONFIG_A=%d", buffer);
    Board::get()->i2c_read_byte(HMC5883L_DEFAULT_ADDRESS, HMC5883L_RA_MODE, &buffer);
    log("HMC5883L_RA_MODE=%d", buffer);
#endif

    log("GY87 INIT SUCCESS!");

    return true;
}

#define CYCLE_COUNT 2
void GY87::get_data(float* imu_data)
{
    static int i=0;

    if (i%CYCLE_COUNT==0) {
        mpu6050.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    } else {
        mag.getHeading(&mx, &my, &mz);
        mag.setMode(HMC5883L_MODE_SINGLE);
    }
    i++;

#if IMU_DEBUG_ENABLE
//    log("[%d %d %d] [%d %d %d] [%d %d %d]", ax, ay, az, gx, gy, gz, mx, my, mz);
#endif

    imu_data[0] = ax*ADX_SCALE;
    imu_data[1] = ay*ADX_SCALE;
    imu_data[2] = az*ADX_SCALE;
    imu_data[3] = gx*GRY_SCALE;
    imu_data[4] = gy*GRY_SCALE;
    imu_data[5] = gz*GRY_SCALE;
    imu_data[6] = mx*HMC5883L_SCALE;
    imu_data[7] = my*HMC5883L_SCALE;
    imu_data[8] = mz*HMC5883L_SCALE;
}