#include "imu_qmi8658.h"
#include "esp_log.h"
#include "qmi8658.h"
#include "config.h"

static const char *TAG = "imu_qmi8658";
static qmi8658_dev_t g_imu_dev;

qmi8658_dev_t* imu_qmi8658_get_dev(void)
{
    return &g_imu_dev;
}

esp_err_t imu_qmi8658_init(i2c_master_bus_handle_t i2c_bus)
{
    uint8_t primary_addr = IMU_I2C_ADDR_HIGH ? QMI8658_ADDRESS_HIGH : QMI8658_ADDRESS_LOW;
    uint8_t fallback_addr = IMU_I2C_ADDR_HIGH ? QMI8658_ADDRESS_LOW : QMI8658_ADDRESS_HIGH;
    uint8_t actual_addr = primary_addr;

    /* QMI8658 初始化 */
    esp_err_t ret = qmi8658_init(&g_imu_dev, i2c_bus, primary_addr);
    if (ret != ESP_OK) {
        ESP_LOGW(TAG, "⚠️ QMI8658 地址 0x%02X 初始化失败，尝试地址 0x%02X", primary_addr, fallback_addr);
        ret = qmi8658_init(&g_imu_dev, i2c_bus, fallback_addr);
        if (ret != ESP_OK) {
            ESP_LOGE(TAG, "❌ QMI8658 地址 0x%02X 初始化仍然失败", fallback_addr);
            return ret;
        }
        actual_addr = fallback_addr;
    }

    /* 配置参数 */
    qmi8658_set_accel_range(&g_imu_dev, QMI8658_ACCEL_RANGE_4G);
    qmi8658_set_accel_odr(&g_imu_dev, QMI8658_ACCEL_ODR_500HZ);
    qmi8658_set_accel_unit_mps2(&g_imu_dev, true);

    qmi8658_set_gyro_range(&g_imu_dev, QMI8658_GYRO_RANGE_256DPS);
    qmi8658_set_gyro_odr(&g_imu_dev, QMI8658_GYRO_ODR_500HZ);

    ESP_LOGI(TAG, "✅ QMI8658 初始化成功，地址 0x%02X", actual_addr);

    return ESP_OK;
}

esp_err_t imu_qmi8658_read(imu_qmi8658_data_t *out_data)
{
    if (!out_data) return ESP_ERR_INVALID_ARG;
    
    bool ready = false;
    if (qmi8658_is_data_ready(&g_imu_dev, &ready) != ESP_OK || !ready) {
        return ESP_ERR_INVALID_STATE;
    }
    
    qmi8658_data_t data;
    if (qmi8658_read_sensor_data(&g_imu_dev, &data) != ESP_OK) {
        return ESP_FAIL;
    }

    out_data->accelX = data.accelX;
    out_data->accelY = data.accelY;
    out_data->accelZ = data.accelZ;
    out_data->gyroX = data.gyroX;
    out_data->gyroY = data.gyroY;
    out_data->gyroZ = data.gyroZ;
    
    return ESP_OK;
}
