#include "rtthread.h"
#include "bf0_hal.h"
#include "drv_io.h"
#include "stdio.h"
#include "string.h"
#include <board.h>
#include "MPU6050.h"
#include "sensor_my_MPU6050.h"
#include "sensor.h"


uint8_t id;
struct rt_sensor_data acce_data;
#define SENSOR_NAME "acce_MPU"
rt_device_t dev;

int main(void)
{
    HAL_PIN_Set(PAD_PA27, I2C1_SCL, PIN_NOPULL, 1);
    HAL_PIN_Set(PAD_PA20, I2C1_SDA, PIN_NOPULL, 1);

    i2c1_Init();
    MPU6050_Init();
    
    rt_size_t res;
    dev = rt_device_find("acce_MPU");   
    if (dev == RT_NULL)
    {
        rt_kprintf("can't find %s device!\n", SENSOR_NAME);
        return -1;
    }
    else 
    {
        rt_kprintf("%s device finded!\n", SENSOR_NAME);
    }

    res = rt_device_open(dev, RT_DEVICE_FLAG_RDONLY);
    if(res != RT_EOK)
    {
        rt_kprintf("open %s device failed! res: %d\n", SENSOR_NAME, res);
        return -1;
    }
    else
    {
        rt_kprintf("%s device opened!\n", SENSOR_NAME);
    }
    rt_device_control(dev, RT_SENSOR_CTRL_GET_ID, &id);
    rt_kprintf("MPU6050 ID: 0x%02X\n", id);
    
    

    while (1)
    {
        res = rt_device_read(dev, 0, &acce_data, 1);
        if (res != 1)
            rt_kprintf("read acc data failed! res: %d\n", res);    
        else
            rt_kprintf("acce: x:%d, y:%d, z:%d\n", acce_data.data.acce.x, acce_data.data.acce.y, acce_data.data.acce.z);
        // MPU6050_Get_accel(&data);
        // MPU6050_Get_gyro(&data);
        // MPU6050_Get_temp(&data);

        // float Temp = (data.Temp / 340.0 + 36.53);
        // rt_kprintf("AccX: %6d, AccY: %6d, AccZ: %6d\n", data.AccX, data.AccY, data.AccZ);
        // rt_kprintf("GyroX: %6d, GyroY: %6d, GyroZ: %6d\n", data.GyroX, data.GyroY, data.GyroZ);
        // rt_kprintf("Temp: %6.2f\n", Temp);
        // rt_thread_mdelay(1000);
        rt_thread_mdelay(1000);
    }
}

static int MPU6050_Regist(void)
{
    struct rt_sensor_config cfg;
    cfg.intf.dev_name = "i2c1";
    rt_hw_MPU6050_init("MPU6050", &cfg);
}
INIT_APP_EXPORT(MPU6050_Regist);