#ifndef GYROSCOPE_H
#define GYROSCOPE_H

#include <esp_log.h>
#include <esp_check.h>
#include <esp_timer.h>
#include <driver/gpio.h>
#include "i2c_bus.h"
#include "freertos/FreeRTOS.h"
#include "freertos/event_groups.h"
#include <string.h>
#include <stdio.h>
#include <math.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <freertos/queue.h>
#include <freertos/semphr.h>
#include "esp_err.h"
#include "config.h"
#include "io_expansion_board.h"
#include <functional>
#include "esp_timer.h"

// 加速度数据结构体
typedef struct {
    float x;  // X轴加速度 (g)
    float y;  // Y轴加速度 (g)
    float z;  // Z轴加速度 (g)
} AccelerationData;

// 一阶滤波系数
#define WEIGHTFACTOR 0.93f

/* 陀螺仪寄存器定义 */
#define CTRL1_XL        0x10    // 加速度计控制寄存器1
#define CTRL2_G         0x11    // 陀螺仪控制寄存器2  
#define CTRL3_C         0x12    // 控制寄存器3
#define CTRL4_C         0x13    // 控制寄存器4
#define CTRL5_C         0x14    // 控制寄存器5
#define CTRL6_C         0x15    // 控制寄存器6
#define CTRL7_G         0x16    // 陀螺仪控制寄存器7
#define CTRL8_XL        0x17    // 加速度计控制寄存器8
#define CTRL9_XL        0x18    // 加速度计控制寄存器9
#define CTRL10_C        0x19    // 控制寄存器10
#define INT1_CTRL       0x0D    // 中断1控制寄存器
#define WAKE_UP_DUR     0x5B    // 唤醒持续时间寄存器
#define WAKE_UP_THS     0x5C    // 唤醒阈值寄存器
#define TAP_CFG         0x58    // 敲击配置寄存器
#define TAP_THS_6D      0x59    // 6D方向阈值寄存器
#define INT_DUR2        0x5A    // 中断持续时间寄存器
#define MD1_CFG         0x5E    // 中断1配置寄存器
#define MD2_CFG         0x5F    // 中断2配置寄存器
#define FUNC_SRC1       0x53    // 功能源寄存器1
#define WAKE_UP_SRC     0x1B    // 唤醒源寄存器

#define ACCEL_X_L 0x28  // OUTX_L_XL
#define ACCEL_X_H 0x29  // OUTX_H_XL
#define ACCEL_Y_L 0x2A  // OUTY_L_XL
#define ACCEL_Y_H 0x2B  // OUTY_H_XL
#define ACCEL_Z_L 0x2C  // OUTZ_L_XL
#define ACCEL_Z_H 0x2D  // OUTZ_H_XL

#define ACCEL_SENSITIVITY_2G 0.061f // 2g 灵敏度 (g/LSB)

#define I2C_MASTER_TIMEOUT_MS         1000            // I2C主设备超时时间(毫秒)

/**
 * @class Gyroscope
 * @brief 陀螺仪驱动类,封装芯片操作
 */
#define TAG_GYROSCOPE "Gyroscope"
class Gyroscope
{
public:
    /**
     * @brief 构造函数
     */
    Gyroscope();

    /**
     * @brief 析构函数
     */
    ~Gyroscope();

    /**
     * @brief 初始化中断引脚
     * @param gpio_num 中断引脚编号
     * @return esp_err_t 初始化结果
     */
    esp_err_t InterruptGpioInit(gpio_num_t gpio_num);

    /**
     * @brief 创建陀螺仪读取任务
     */
    void CreateGyroReadTask();

    /**
     * @brief 初始化陀螺仪
     * @return esp_err_t 初始化结果
     */
    esp_err_t Init(i2c_bus_handle_t bus_handle);

    /**
     * @brief 清除中断标志
     * @return esp_err_t 清除结果
     */
    esp_err_t ClearInterruptFlags();


    /**
     * @brief 获取当前加速度数据
     * @param data 指向AccelerationData结构体的指针,用于存储读取到的加速度数据
     * @return esp_err_t 获取结果
     */
    esp_err_t GetAcceleration(AccelerationData* data);

    /**
     * @brief 创建500ms周期定时器
     * @return esp_err_t 创建结果
     */
    esp_err_t CreateTimer();

    /**
     * @brief 停止并删除定时器
     */
    void DeleteTimer();

    /**
     * @brief 注册摇晃检测回调函数
     * @param callback 当检测到摇晃时调用的回调函数
     */
    void OnShake(std::function<void()> callback);

    /**
     * @brief 注册拿起检测回调函数
     * @param callback 当检测到设备被拿起时调用的回调函数
     */
    void OnPickUp(std::function<void()> callback);

    /**
     * @brief 注册放置检测回调函数
     * @param callback 当检测到设备被放置时调用的回调函数
     */
    void OnPutDown(std::function<void()> callback);

    /**
     * @brief 注册倾斜检测回调函数
     * @param callback 当检测到设备倾斜时调用的回调函数
     */
    void OnTilt(std::function<void()> callback);

    i2c_bus_handle_t bus_handle_ = nullptr;    // I2C总线句柄
    volatile uint8_t interrupt_count_ = 0;     // 中断计数
    volatile bool interrupt_flag_ = false;     // 中断标志位)(判断是否需要处理)

private:
    static void GpioIsrHandler(void* arg);    // 中断处理函数
    static void GyroReadTask(void* pvParameters);
    static void TimerCallback(void* arg);    // 定时器回调函数
    
    void TimerInterruptHandler();  // 定时器中断处理函数
    
    esp_err_t WriteRegister(uint8_t reg, uint8_t data);
    esp_err_t ReadRegister(uint8_t reg, uint8_t* data);
    esp_err_t ReadRegisters(uint8_t reg, uint8_t* data, size_t length);

    /**
     * @brief 读取原始加速度数据
     * @param x 指向X轴加速度存储的指针
     * @param y 指向Y轴加速度存储的指针
     * @param z 指向Z轴加速度存储的指针
     * @return esp_err_t 读取结果
     */
    esp_err_t ReadRawAcceleration(int16_t* x, int16_t* y, int16_t* z);

    std::function<void()> on_shake_callback_;  // 摇晃回调函数
    std::function<void()> on_pickup_callback_; // 拿起回调函数
    std::function<void()> on_putdown_callback_; // 放置回调函数
    std::function<void()> on_tilt_callback_;   // 倾斜回调函数
    
    esp_timer_handle_t timer_handle_ = nullptr;  // 定时器句柄

    /**
     * @brief 分配静态任务所需的内存
     * @return bool 分配是否成功
     */
    bool AllocateStaticTaskMemory();
    
    /**
     * @brief 释放静态任务分配的内存
     */
    void FreeStaticTaskMemory();

    TaskHandle_t xGyroscopeTaskHandle_ = nullptr;
    StackType_t* pGyroscopeTaskStackBuffer_ = nullptr;    // 任务栈指针（PSRAM）
    StaticTask_t* pxGyroscopeTaskBuffer_ = nullptr;       // 任务控制块指针（内部RAM）

    // 滤波相关成员变量
    int16_t last_filtered_x_ = 0;     // 上一次滤波后的X轴值
    int16_t last_filtered_y_ = 0;     // 上一次滤波后的Y轴值
    int16_t last_filtered_z_ = 0;     // 上一次滤波后的Z轴值
    bool is_first_sample_ = true;     // 是否为第一次采样标志

    // 状态检测相关成员变量
    float accel_data_y_last_ = 0.0f;  // 保存上一次Y轴数据
    float total_accel_last_ = 1.0f;   // 保存上一次合成加速度值，初始值为1g（平放时的典型值）
    uint8_t stable_count_ = 0;        // 稳定状态计数器
    uint8_t lift_count_ = 0;          // 拿起动作计数器
    uint8_t putdown_count_ = 0;       // 放置动作计数器
    bool is_stable_ = false;          // 是否处于稳定平放状态
    bool is_lifted_ = false;          // 是否处于被拿起状态
};

#endif // GYROSCOPE_H