#include "sensors.h"

#include <math.h>

#include "config.h"
#include "driver/i2c_master.h"
#include "esp_check.h"
#include "esp_log.h"

static const char *TAG = "guguji_sensors";

#define QMI_REG_WHO_AM_I 0x00
#define QMI_REG_CTRL1 0x02
#define QMI_REG_CTRL2 0x03
#define QMI_REG_CTRL3 0x04
#define QMI_REG_CTRL5 0x06
#define QMI_REG_CTRL7 0x08
#define QMI_REG_DATA_START 0x35

#define QMC_REG_CHIP_ID 0x00
#define QMC_REG_DATA_START 0x01
#define QMC_REG_STATUS 0x09
#define QMC_REG_CTRL1 0x0A
#define QMC_REG_CTRL2 0x0B

static bool g_qmi_ok = false;
static bool g_qmc_ok = false;
static i2c_master_bus_handle_t g_i2c_bus = NULL;
static i2c_master_dev_handle_t g_qmi_device = NULL;
static i2c_master_dev_handle_t g_qmc_device = NULL;

static esp_err_t i2c_write_u8(i2c_master_dev_handle_t device, uint8_t reg, uint8_t value)
{
    uint8_t payload[2] = {reg, value};
    return i2c_master_transmit(device, payload, sizeof(payload), 50);
}

static esp_err_t i2c_read(i2c_master_dev_handle_t device, uint8_t reg, uint8_t *data, size_t length)
{
    return i2c_master_transmit_receive(device, &reg, 1, data, length, 50);
}

static int16_t le_i16(const uint8_t *data)
{
    return (int16_t)((uint16_t)data[0] | ((uint16_t)data[1] << 8));
}

esp_err_t guguji_sensors_init(void)
{
    i2c_master_bus_config_t bus_config = {
        .i2c_port = GUGUJI_I2C_PORT,
        .sda_io_num = GUGUJI_I2C_SDA_GPIO,
        .scl_io_num = GUGUJI_I2C_SCL_GPIO,
        .clk_source = I2C_CLK_SRC_DEFAULT,
        .glitch_ignore_cnt = 7,
        .flags.enable_internal_pullup = true,
    };
    ESP_RETURN_ON_ERROR(i2c_new_master_bus(&bus_config, &g_i2c_bus), TAG, "创建 I2C master bus 失败");

    i2c_device_config_t qmi_config = {
        .dev_addr_length = I2C_ADDR_BIT_LEN_7,
        .device_address = GUGUJI_QMI8658A_ADDR,
        .scl_speed_hz = GUGUJI_I2C_FREQ_HZ,
    };
    ESP_RETURN_ON_ERROR(
        i2c_master_bus_add_device(g_i2c_bus, &qmi_config, &g_qmi_device),
        TAG,
        "添加 QMI8658A I2C 设备失败");

    i2c_device_config_t qmc_config = {
        .dev_addr_length = I2C_ADDR_BIT_LEN_7,
        .device_address = GUGUJI_QMC6309_ADDR,
        .scl_speed_hz = GUGUJI_I2C_FREQ_HZ,
    };
    ESP_RETURN_ON_ERROR(
        i2c_master_bus_add_device(g_i2c_bus, &qmc_config, &g_qmc_device),
        TAG,
        "添加 QMC6309 I2C 设备失败");

    uint8_t who_am_i = 0;
    if (i2c_read(g_qmi_device, QMI_REG_WHO_AM_I, &who_am_i, 1) == ESP_OK && who_am_i == 0x05) {
        // CTRL1: 开启地址自增并使用 little-endian，方便一次读完 AX~GZ。
        i2c_write_u8(g_qmi_device, QMI_REG_CTRL1, 0x40);
        // CTRL2: 加速度 ±8g，ODR 约 112Hz；CTRL3: 陀螺仪 ±512dps，ODR 约 112Hz。
        i2c_write_u8(g_qmi_device, QMI_REG_CTRL2, 0x26);
        i2c_write_u8(g_qmi_device, QMI_REG_CTRL3, 0x56);
        // 打开加速度和陀螺仪低通滤波。
        i2c_write_u8(g_qmi_device, QMI_REG_CTRL5, 0x11);
        // 使能 accel + gyro。
        i2c_write_u8(g_qmi_device, QMI_REG_CTRL7, 0x03);
        g_qmi_ok = true;
        ESP_LOGI(TAG, "QMI8658A 初始化成功");
    } else {
        ESP_LOGW(TAG, "QMI8658A 初始化失败，who_am_i=0x%02X", who_am_i);
    }

    uint8_t chip_id = 0;
    if (i2c_read(g_qmc_device, QMC_REG_CHIP_ID, &chip_id, 1) == ESP_OK && chip_id == 0x90) {
        // 数据手册推荐的连续测量配置：32Gauss 量程、Set/Reset 开、OSR1/OSR2=8。
        i2c_write_u8(g_qmc_device, QMC_REG_CTRL2, 0x00);
        i2c_write_u8(g_qmc_device, QMC_REG_CTRL1, 0x63);
        g_qmc_ok = true;
        ESP_LOGI(TAG, "QMC6309 初始化成功");
    } else {
        ESP_LOGW(TAG, "QMC6309 初始化失败，chip_id=0x%02X", chip_id);
    }

    return ESP_OK;
}

static void update_imu(guguji_sensor_state_t *state)
{
    uint8_t raw[12] = {0};
    state->imu_ok = false;
    if (!g_qmi_ok || i2c_read(g_qmi_device, QMI_REG_DATA_START, raw, sizeof(raw)) != ESP_OK) {
        return;
    }

    const int16_t ax = le_i16(&raw[0]);
    const int16_t ay = le_i16(&raw[2]);
    const int16_t az = le_i16(&raw[4]);
    const int16_t gx = le_i16(&raw[6]);
    const int16_t gy = le_i16(&raw[8]);
    const int16_t gz = le_i16(&raw[10]);

    const float accel_scale = 9.80665f / 4096.0f;       // ±8g
    const float gyro_scale = 3.14159265358979323846f / 180.0f / 64.0f; // ±512dps

    state->accel_m_s2[0] = (float)ax * accel_scale;
    state->accel_m_s2[1] = (float)ay * accel_scale;
    state->accel_m_s2[2] = (float)az * accel_scale;
    state->gyro_rad_s[0] = (float)gx * gyro_scale;
    state->gyro_rad_s[1] = (float)gy * gyro_scale;
    state->gyro_rad_s[2] = (float)gz * gyro_scale;
    state->imu_ok = true;
}

static void update_mag(guguji_sensor_state_t *state)
{
    uint8_t status = 0;
    state->mag_ok = false;
    if (!g_qmc_ok || i2c_read(g_qmc_device, QMC_REG_STATUS, &status, 1) != ESP_OK) {
        return;
    }
    if ((status & 0x01u) == 0) {
        return;
    }

    uint8_t raw[6] = {0};
    if (i2c_read(g_qmc_device, QMC_REG_DATA_START, raw, sizeof(raw)) != ESP_OK) {
        return;
    }
    // QMC6309 典型分辨率约 1mGs/LSB，即 0.1uT/LSB。
    state->mag_u_t[0] = (float)le_i16(&raw[0]) * 0.1f;
    state->mag_u_t[1] = (float)le_i16(&raw[2]) * 0.1f;
    state->mag_u_t[2] = (float)le_i16(&raw[4]) * 0.1f;
    state->mag_ok = true;
}

void guguji_sensors_update(guguji_sensor_state_t *state, float dt_s)
{
    update_imu(state);
    update_mag(state);

    if (!state->imu_ok) {
        return;
    }

    const float ax = state->accel_m_s2[0];
    const float ay = state->accel_m_s2[1];
    const float az = state->accel_m_s2[2];
    const float accel_roll = atan2f(ay, az);
    const float accel_pitch = atan2f(-ax, sqrtf(ay * ay + az * az));

    // 简单互补滤波：真实部署第一步只需要稳定的 roll/pitch 保护。
    const float alpha = 0.98f;
    state->rpy_rad[0] = alpha * (state->rpy_rad[0] + state->gyro_rad_s[0] * dt_s) + (1.0f - alpha) * accel_roll;
    state->rpy_rad[1] = alpha * (state->rpy_rad[1] + state->gyro_rad_s[1] * dt_s) + (1.0f - alpha) * accel_pitch;
    state->rpy_rad[2] += state->gyro_rad_s[2] * dt_s;

    if (state->mag_ok) {
        const float heading = atan2f(state->mag_u_t[1], state->mag_u_t[0]);
        state->rpy_rad[2] = 0.995f * state->rpy_rad[2] + 0.005f * heading;
    }
}
