/**
 * ==================================================
 *  @file imu_lis3dhtr.c
 *  @brief TODO 描述该文件的功能
 *  @author wangb
 *  @date 2026-01-27 12:21
 *  @version 1.0
 *  
 *  @details 本文件包含...
 *  
 *  @copyright Copyright (c) 2026 wangb. All Rights Reserved.
 *  
 *  @license 不开源
 * ==================================================
 */


#include "mems_lis3dhtr.h"

#include "cmsis_os2.h"
#include "elog.h"
#include "i2c.h"


volatile uint8_t lis3dh_int_flag = 0;
static uint8_t last_int_src = 0;  // 记录上一次中断源
/* ================= 内部 I2C 读写 ================= */

void lis3dh_read(uint8_t reg, uint8_t *data, uint16_t len)
{
  /* 多字节读必须置位 bit7 */
  if (len > 1)
    reg |= 0x80;

  HAL_I2C_Mem_Read(&hi2c1,
                   LIS3DHTR_I2C_ADDR,
                   reg,
                   I2C_MEMADD_SIZE_8BIT,
                   data,
                   len,
                   1000);
}

void lis3dh_write(uint8_t reg, uint8_t data)
{
  HAL_I2C_Mem_Write(&hi2c1,
                    LIS3DHTR_I2C_ADDR,
                    reg,
                    I2C_MEMADD_SIZE_8BIT,
                    &data,
                    1,
                    1000);
}
/* ================= 清除所有中断标志 ================= */
void lis3dh_clear_all_int_flags(void) {
    uint8_t dummy;

    // 读取INT1_SRC清除中断锁存（关键！）
    lis3dh_read(LIS3DH_REG_INT1_SRC, &dummy, 1);

    // 读取状态寄存器确保数据就绪标志清除
    lis3dh_read(LIS3DH_REG_STATUS, &dummy, 1);

    // 读取REFERENCE寄存器重置高通滤波器
    lis3dh_read(LIS3DH_REG_REFERENCE, &dummy, 1);

    last_int_src = 0;
}

/* ================= LIS3DH 中断初始化 ================= */
static void lis3dh_int1_fall_init(void) {
    uint8_t val;

    /* 第一步：禁用所有中断并清除标志 */
    lis3dh_write(LIS3DH_REG_CTRL3, 0x00);
    lis3dh_write(LIS3DH_REG_INT1_CFG, 0x00);
    osDelay(10);

    /* 第二步：清除所有可能的中断标志 */
    lis3dh_clear_all_int_flags();
    osDelay(10);

    /* 第三步：配置CTRL2 - 高通滤波器 */
    // 0x11 = 100Hz截止频率，HP使能
    lis3dh_write(LIS3DH_REG_CTRL2, 0x11);

    /* 第四步：配置CTRL3 - INT1引脚输出AOI1中断 */
    val = 0x40; // I1_AOI1 = 1
    lis3dh_write(LIS3DH_REG_CTRL3, val);

    /* 第五步：INT1_CFG - 配置检测逻辑 */
    // 0x2A = 0010 1010：只检测Z轴高阈值和低阈值
    // 0x3F = 0011 1111：检测所有轴（根据你的日志0x25/0x65包含多个轴）
    val = 0x2A; // 先用Z轴测试
    lis3dh_write(LIS3DH_REG_INT1_CFG, val);

    /* 第六步：INT1_THS - 阈值设置 */
    val = 15; // 15 * 16mg = 240mg ≈ 0.24g（自由落体阈值）
    lis3dh_write(LIS3DH_REG_INT1_THS, val);

    /* 第七步：INT1_DUR - 持续时间（去抖动） */
    val = 2; // 2 * 1/ODR = 20ms @ 100Hz
    lis3dh_write(LIS3DH_REG_INT1_DUR, val);

    /* 第八步：配置CTRL5 - 锁存中断 */
    // 0x0A = 0000 1010：LIR_INT1=1（锁存），INT1高阈值使能
    lis3dh_write(LIS3DH_REG_CTRL5, 0x0A);

    /* 第九步：再次清除所有标志 */
    lis3dh_clear_all_int_flags();

    elog_info("MEMS", "INT1 fall detect configured (Latch Mode)");
}

/* ================= LIS3DH 初始化 ================= */
uint8_t imu_lis3dhtr_init(void) {
    uint8_t whoami = 0;
    lis3dh_read(LIS3DH_REG_WHOAMI, &whoami, 1);
    elog_info("MEMS", "LIS3DHTR WHO_AM_I = 0x%02X", whoami);

    if (whoami != 0x33)
        return 0;

    /* CTRL_REG1: 100Hz ODR, XYZ 使能 */
    lis3dh_write(LIS3DH_REG_CTRL1, 0x57);

    /* CTRL_REG4: BDU=1, HR=1, FS=±2g */
    lis3dh_write(LIS3DH_REG_CTRL4, 0xA0);

    /* 初始化 INT1 跌落检测 */
    lis3dh_int1_fall_init();

    /* 读回验证 */
    uint8_t c1, c2, c3, c4, c5, cfg;
    lis3dh_read(LIS3DH_REG_CTRL1, &c1, 1);
    lis3dh_read(LIS3DH_REG_CTRL2, &c2, 1);
    lis3dh_read(LIS3DH_REG_CTRL3, &c3, 1);
    lis3dh_read(LIS3DH_REG_CTRL4, &c4, 1);
    lis3dh_read(LIS3DH_REG_CTRL5, &c5, 1);
    lis3dh_read(LIS3DH_REG_INT1_CFG, &cfg, 1);

    elog_info("MEMS", "CTRL1=0x%02X CTRL2=0x%02X CTRL3=0x%02X", c1, c2, c3);
    elog_info("MEMS", "CTRL4=0x%02X CTRL5=0x%02X INT1_CFG=0x%02X", c4, c5, cfg);

    return 1;
}

/* ================= 读取加速度 ================= */

uint8_t imu_lis3dhtr_read_accel(lis3dh_accel_t *accel)
{
    uint8_t status;
    lis3dh_read(LIS3DH_REG_STATUS, &status, 1);

    if ((status & 0x08) == 0)
        return 0; // 新数据未准备好

    uint8_t buf[6];
    lis3dh_read(LIS3DH_REG_OUT_X_L, buf, 6);

    int16_t raw_x = ((int16_t)buf[1] << 8 | buf[0]) >> 4;
    int16_t raw_y = ((int16_t)buf[3] << 8 | buf[2]) >> 4;
    int16_t raw_z = ((int16_t)buf[5] << 8 | buf[4]) >> 4;

    accel->x_g = raw_x * 0.001f; // ±2g HR模式，1LSB≈1mg
    accel->y_g = raw_y * 0.001f;
    accel->z_g = raw_z * 0.001f;

    return 1;
}

/* ================= 外部中断回调 ================= */
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
    if (GPIO_Pin == lis3dhtr_Pin) {
        uint8_t src;

        // 1. 读取中断源（这会清除锁存中断）
        lis3dh_read(LIS3DH_REG_INT1_SRC, &src, 1);

        // 2. 有效性检查：源寄存器必须有中断标志
        if (src == 0x00) {
            // 可能是假中断，额外操作确保清除
            uint8_t dummy;
            lis3dh_read(LIS3DH_REG_INT1_SRC, &dummy, 1);
            return;
        }

        // 3. 去重：避免相同中断源连续触发
        if (src == last_int_src) {
            // 相同中断，可能是未及时处理，强制清除
            elog_debug("MEMS", "Duplicate INT1 SRC = 0x%02X", src);
            return;
        }

        // 4. 记录并处理
        last_int_src = src;
        elog_warn("MEMS", "INT1 SRC = 0x%02X", src);

        // 5. 解析中断类型（用于调试和高级处理）
        if (src & 0x40) elog_debug("MEMS", "Z high triggered");
        if (src & 0x20) elog_debug("MEMS", "Z low triggered");
        if (src & 0x10) elog_debug("MEMS", "Y high triggered");
        if (src & 0x08) elog_debug("MEMS", "Y low triggered");
        if (src & 0x04) elog_debug("MEMS", "X high triggered");
        if (src & 0x02) elog_debug("MEMS", "X low triggered");

        // 6. 设置处理标志
        lis3dh_int_flag = 1;
    }
}