#include "sensor_MY_MPU6050.h"
#include "MPU6050.h"
#define DBG_ENABLE
#define DBG_SECTION_NAME "MPU6050"
#define DBG_LEVEL DBG_LOG
#include <rtdbg.h>


static rt_size_t MPU6050_fetch_data(struct rt_sensor_device *sensor, void *buf, rt_size_t len)
{
    struct rt_sensor_data *data = (struct rt_sensor_data *)buf;
    struct MPU6050_Data raw_data;

    rt_kprintf("MPU6050_fetch_data type: %d\n", sensor->info.type);

    if (sensor->info.type == RT_SENSOR_CLASS_ACCE)
    {
        MPU6050_Get_accel(&raw_data);
        data->type = RT_SENSOR_CLASS_ACCE;
        data->data.acce.x = raw_data.AccX;
        data->data.acce.y = raw_data.AccY;
        data->data.acce.z = raw_data.AccZ;
        data->timestamp = rt_sensor_get_ts();
    }
    // else if (sensor->info.type == RT_SENSOR_CLASS_GYRO) 
    // {
    //     MPU6050_Get_gyro(&raw_data);
    //     data->type = RT_SENSOR_CLASS_GYRO;
    //     data->data.gyro.x = raw_data.GyroX;
    //     data->data.gyro.y = raw_data.GyroY;
    //     data->data.gyro.z = raw_data.GyroZ;
    //     data->timestamp = rt_sensor_get_ts();
    // }
    // else if (sensor->info.type == RT_SENSOR_CLASS_TEMP)
    // {
    //     MPU6050_Get_temp(&raw_data);
    //     data->type = RT_SENSOR_CLASS_TEMP;
    //     data->data.temp = raw_data.Temp / 340.0 + 36.53;
    //     data->timestamp = rt_sensor_get_ts();
    // }
    else
        return 0;

    return 1;
}

static rt_err_t MPU6050_control(struct rt_sensor_device *sensor, int cmd, void *arg)
{
    rt_err_t result = RT_EOK;
    switch (cmd)
    {
    case RT_SENSOR_CTRL_GET_ID:
        *(rt_uint8_t *)arg = MPU6050_GetID();
        break;
    default:
        return -RT_ERROR;
    }
    return result;
}

static struct rt_sensor_ops sensor_ops =
{
    MPU6050_fetch_data,
    MPU6050_control
};

int rt_hw_MPU6050_init(const char *name, struct rt_sensor_config *cfg)
{
    rt_int8_t result;
    rt_sensor_t sensor_acce = RT_NULL, sensor_gyro = RT_NULL, sensor_temp = RT_NULL;

    /* accelerometer sensor register */
    {
        sensor_acce = rt_calloc(1, sizeof(struct rt_sensor_device));
        if (sensor_acce == RT_NULL)
            return -1;

        sensor_acce->info.type       = RT_SENSOR_CLASS_ACCE;
        sensor_acce->info.vendor     = RT_SENSOR_VENDOR_UNKNOWN;
        sensor_acce->info.model      = "MPU6050_acc";
        sensor_acce->info.unit       = RT_SENSOR_UNIT_MG;
        sensor_acce->info.intf_type  = RT_SENSOR_INTF_I2C;
        sensor_acce->info.range_max  = 16000;
        sensor_acce->info.range_min  = 2000;
        sensor_acce->info.period_min = 5;

        rt_memcpy(&sensor_acce->config, cfg, sizeof(struct rt_sensor_config));
        sensor_acce->ops = &sensor_ops;

        result = rt_hw_sensor_register(sensor_acce, name, RT_DEVICE_FLAG_RDWR, RT_NULL);
        if (result != RT_EOK)
        {
            LOG_E("device register err code: %d", result);
        }
    }

    // {
    //     sensor_gyro = rt_calloc(1, sizeof(struct rt_sensor_device));
    //     if (sensor_gyro == RT_NULL)
    //         return -1;

    //     sensor_gyro->info.type       = RT_SENSOR_CLASS_GYRO;
    //     sensor_gyro->info.vendor     = RT_SENSOR_VENDOR_UNKNOWN;
    //     sensor_gyro->info.model      = "mpu6xxx_gyro";
    //     sensor_gyro->info.unit       = RT_SENSOR_UNIT_MDPS;
    //     sensor_gyro->info.intf_type  = RT_SENSOR_INTF_I2C;
    //     sensor_gyro->info.range_max  = 2000000;
    //     sensor_gyro->info.range_min  = 250000;
    //     sensor_gyro->info.period_min = 5;

    //     rt_memcpy(&sensor_gyro->config, cfg, sizeof(struct rt_sensor_config));
    //     sensor_gyro->ops = &sensor_ops;
    //     result = rt_hw_sensor_register(sensor_gyro, name, RT_DEVICE_FLAG_RDWR, RT_NULL);
    //     if (result != RT_EOK)
    //     {
    //         LOG_E("device register err code: %d", result);
    //     }
    // }

    // {
    //     sensor_temp = rt_calloc(1, sizeof(struct rt_sensor_device));
    //     if (sensor_temp == RT_NULL)
    //         return -1;

    //     sensor_temp->info.type       = RT_SENSOR_CLASS_TEMP;
    //     sensor_temp->info.vendor     = RT_SENSOR_VENDOR_UNKNOWN;
    //     sensor_temp->info.model      = "mpu6xxx_temp";
    //     sensor_temp->info.unit       = RT_SENSOR_UNIT_MDPS;
    //     sensor_temp->info.intf_type  = RT_SENSOR_INTF_I2C;
    //     sensor_temp->info.range_max  = 2000000;
    //     sensor_temp->info.range_min  = 250000;
    //     sensor_temp->info.period_min = 5;

    //     rt_memcpy(&sensor_temp->config, cfg, sizeof(struct rt_sensor_config));
    //     sensor_temp->ops = &sensor_ops;
    //     result = rt_hw_sensor_register(sensor_temp, name, RT_DEVICE_FLAG_RDWR, RT_NULL);
    //     if (result != RT_EOK)
    //     {
    //         LOG_E("device register err code: %d", result);
    //     }
    // }

    LOG_I("sensor init success");

    return RT_EOK;
}