#include "gSensor/mpu6050.h"
#include "gSensor/gSensor_manage.h"
#include "app_config.h"

#if TCFG_MPU6050_EN

#define ACCEL_ONLY_LOW_POWER    1

u8 mpu6050_register_read(u8 addr, u8 *data)
{
    _gravity_sensor_get_ndata(I2C_ADDR_MPU6050_R, addr, data, 1);
    return 0;
}

u8 mpu6050_register_write(u8 addr, u8 data)
{
    gravity_sensor_command(I2C_ADDR_MPU6050_W, addr, data);
    return 0;
}

u8 mpu6050_read_nbyte_data(u8 addr, u8 *data, u8 len)
{
    return _gravity_sensor_get_ndata(I2C_ADDR_MPU6050_R, addr, data, len);
}

u8 mpu6050_get_xyz_data(u8 addr, void *xyz_data)
{
    u8 buf[6], read_len;
    axis_info_t *data = (axis_info_t *)xyz_data;
    read_len = mpu6050_read_nbyte_data(addr, buf, 6);
    if (read_len == 6) {
        data->x = ((u16)buf[0] << 8) | buf[1];
        data->y = ((u16)buf[2] << 8) | buf[3];
        data->z = ((u16)buf[4] << 8) | buf[5];

        data->x = 2 * ACCEL_OF_GRAVITY * ACCEL_DATA_GAIN * data->x / 32768;
        data->y = 2 * ACCEL_OF_GRAVITY * ACCEL_DATA_GAIN * data->y / 32768;
        data->z = 2 * ACCEL_OF_GRAVITY * ACCEL_DATA_GAIN * data->z / 32768;
    }
    return read_len;
}

extern void step_cal_init();
extern int step_cal();
u8 mpu6050_init()
{
    u8 res = 0;
    u8 data;
    res = mpu6050_register_read(MPU6050_RA_WHO_AM_I, &data);               //读ID
    if (data == MPU_ADDR) {
        g_printf("read MPU6050 ID suss");
    } else {
        g_printf("read MPU6050 ID err");
        return -1;
    }

    data = 0x80;
    mpu6050_register_write(MPU6050_RA_PWR_MGMT_1, data);                   //复位
    os_time_dly(10);

    data = 0x00;
    data |= BIT(3);                                                        //关闭温度传感器
#if ACCEL_ONLY_LOW_POWER
    data |= BIT(5);                                                        //设置accel-only低功耗模式
#endif
    mpu6050_register_write(MPU6050_RA_PWR_MGMT_1, data);                   //退出休眠

    data = 0x00;
    data |= (BIT(0) | BIT(1) | BIT(2));                                    //关闭陀螺仪
#if ACCEL_ONLY_LOW_POWER
    data |= (3 << 6);                                                      //accel-only低功耗模式下，唤醒频率为40Hz
#endif
    mpu6050_register_write(MPU6050_RA_PWR_MGMT_2, data);

    data = ACCEL_RANGE_2G << 3;
    mpu6050_register_write(MPU6050_RA_ACCEL_CONFIG, data);                 //加速度计量程

#if (!ACCEL_ONLY_LOW_POWER)
    data = 0x06;
    mpu6050_register_write(MPU6050_RA_CONFIG, data);                       //设置陀螺仪输出速率

    data = MPU6050_GYRO_OUT_RATE / MPU6050_SAMPLE_RATE - 1;
    mpu6050_register_write(MPU6050_RA_SMPLRT_DIV, data);                   //设置采样率, 采样率=陀螺仪输出速率/(1+SMPLRT_DIV)

    data = 0x00;
    mpu6050_register_write(MPU6050_RA_INT_ENABLE, data);                   //关闭中断

    data = 0x00;
    mpu6050_register_write(MPU6050_RA_USER_CTRL, data);                    //关闭主机IIC
#endif

    step_cal_init();
    sys_timer_add(NULL, step_cal, 50);
    return 0;
}

int mpu6050_ctl(u8 cmd, void *arg)
{
    int res = -1;
    switch (cmd) {
    case GET_ACCEL_DATA:
        res = mpu6050_get_xyz_data(MPU6050_RA_ACCEL_XOUT_H, arg);
        break;
    }
    return res;//>=0:ok,,<0:err
}

REGISTER_GRAVITY_SENSOR(gSensor) = {
    .logo = "mpu6050",
    .gravity_sensor_init  = mpu6050_init,
    .gravity_sensor_check = NULL,
    .gravity_sensor_ctl   = mpu6050_ctl,
};

#endif //TCFG_MPU6050_EN
