#ifndef QMI8658A_H
#define QMI8658A_H

#include "driver/i2c_master.h"
#include "driver/gpio.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/semphr.h"
#include "esp_timer.h"
#include <cstdint>
#include <inttypes.h>
#include "i2c_device.h"

// QMI8658A I2C地址定义
#define QMI8658A_I2C_ADDRESS    0x6A  // 修改为0x6A，适配新PCB板（SA0接VDD）

// QMI8658A寄存器地址定义
#define QMI8658A_WHO_AM_I       0x00
#define QMI8658A_REVISION_ID    0x01
#define QMI8658A_CTRL1          0x02
#define QMI8658A_CTRL2          0x03
#define QMI8658A_CTRL3          0x04
#define QMI8658A_CTRL4          0x05
#define QMI8658A_CTRL5          0x06
#define QMI8658A_CTRL6          0x07
#define QMI8658A_CTRL7          0x08
#define QMI8658A_CTRL8          0x09
#define QMI8658A_CTRL9          0x0A
#define QMI8658A_RESET          0x60

// 数据寄存器
#define QMI8658A_TEMP_L         0x33
#define QMI8658A_TEMP_H         0x34
#define QMI8658A_AX_L           0x35
#define QMI8658A_AX_H           0x36
#define QMI8658A_AY_L           0x37
#define QMI8658A_AY_H           0x38
#define QMI8658A_AZ_L           0x39
#define QMI8658A_AZ_H           0x3A
#define QMI8658A_GX_L           0x3B
#define QMI8658A_GX_H           0x3C
#define QMI8658A_GY_L           0x3D
#define QMI8658A_GY_H           0x3E
#define QMI8658A_GZ_L           0x3F
#define QMI8658A_GZ_H           0x40

// 状态寄存器
#define QMI8658A_STATUSINT      0x2D
#define QMI8658A_STATUS0        0x2E
#define QMI8658A_STATUS1        0x2F

// 设备ID
#define QMI8658A_CHIP_ID        0x05

// 工作模式
typedef enum {
    QMI8658A_MODE_DISABLE = 0x00,
    QMI8658A_MODE_ACC_ONLY = 0x01,
    QMI8658A_MODE_GYRO_ONLY = 0x02,
    QMI8658A_MODE_DUAL = 0x03
} qmi8658a_mode_t;

// 加速度计量程
typedef enum {
    QMI8658A_ACC_RANGE_2G = 0x00,
    QMI8658A_ACC_RANGE_4G = 0x01,
    QMI8658A_ACC_RANGE_8G = 0x02,
    QMI8658A_ACC_RANGE_16G = 0x03
} qmi8658a_acc_range_t;

// 陀螺仪量程
typedef enum {
    QMI8658A_GYRO_RANGE_16DPS = 0x00,
    QMI8658A_GYRO_RANGE_32DPS = 0x01,
    QMI8658A_GYRO_RANGE_64DPS = 0x02,
    QMI8658A_GYRO_RANGE_128DPS = 0x03,
    QMI8658A_GYRO_RANGE_256DPS = 0x04,
    QMI8658A_GYRO_RANGE_512DPS = 0x05,
    QMI8658A_GYRO_RANGE_1024DPS = 0x06,
    QMI8658A_GYRO_RANGE_2048DPS = 0x07
} qmi8658a_gyro_range_t;

// 输出数据率
typedef enum {
    QMI8658A_ODR_8000HZ = 0x00,
    QMI8658A_ODR_4000HZ = 0x01,
    QMI8658A_ODR_2000HZ = 0x02,
    QMI8658A_ODR_1000HZ = 0x03,
    QMI8658A_ODR_500HZ = 0x04,
    QMI8658A_ODR_250HZ = 0x05,
    QMI8658A_ODR_125HZ = 0x06,
    QMI8658A_ODR_62_5HZ = 0x07,
    QMI8658A_ODR_31_25HZ = 0x08
} qmi8658a_odr_t;

// 传感器数据结构 - 优化版本，使用数组存储
typedef struct {
    union {
        struct {
            float acc_x;    // 加速度X轴 (g)
            float acc_y;    // 加速度Y轴 (g)
            float acc_z;    // 加速度Z轴 (g)
        };
        float accel[3];     // 加速度数组 [x, y, z] (g)
    };
    union {
        struct {
            float gyro_x;   // 陀螺仪X轴 (dps)
            float gyro_y;   // 陀螺仪Y轴 (dps)
            float gyro_z;   // 陀螺仪Z轴 (dps)
        };
        float gyro[3];      // 陀螺仪数组 [x, y, z] (dps)
    };
    float temperature;      // 温度 (°C)
    uint64_t timestamp;     // 时间戳 (微秒)
    bool valid;             // 数据有效性标志
} qmi8658a_data_t;

// 错误代码定义
typedef enum {
    QMI8658A_OK = 0,
    QMI8658A_ERROR_INVALID_PARAM = -1,
    QMI8658A_ERROR_I2C_COMM = -2,
    QMI8658A_ERROR_CHIP_ID = -3,
    QMI8658A_ERROR_INIT_FAILED = -4,
    QMI8658A_ERROR_CONFIG_FAILED = -5,
    QMI8658A_ERROR_DATA_NOT_READY = -6,
    QMI8658A_ERROR_TIMEOUT = -7
} qmi8658a_error_t;

// 传感器状态
typedef enum {
    QMI8658A_STATE_UNINITIALIZED = 0,
    QMI8658A_STATE_INITIALIZING,
    QMI8658A_STATE_READY,
    QMI8658A_STATE_ERROR
} qmi8658a_state_t;

// 配置结构体
typedef struct {
    qmi8658a_acc_range_t acc_range;
    qmi8658a_gyro_range_t gyro_range;
    qmi8658a_odr_t acc_odr;
    qmi8658a_odr_t gyro_odr;
    qmi8658a_mode_t mode;
    bool enable_interrupt;      // 是否启用中断
    uint8_t interrupt_pin;      // 中断引脚
    bool auto_calibration;      // 是否启用自动校准
    float acc_offset[3];        // 加速度计偏移校准
    float gyro_offset[3];       // 陀螺仪偏移校准
} qmi8658a_config_t;

// 校准数据结构
typedef struct {
    float acc_bias[3];          // 加速度计偏置
    float gyro_bias[3];         // 陀螺仪偏置
    float acc_scale[3];         // 加速度计缩放因子
    float gyro_scale[3];        // 陀螺仪缩放因子
    bool is_calibrated;         // 是否已校准
    uint32_t calibration_time;  // 校准时间戳
} qmi8658a_calibration_t;

// 数据缓冲配置
#define QMI8658A_BUFFER_SIZE 32
#define QMI8658A_FIFO_SIZE 16

// 中断配置
typedef enum {
    QMI8658A_INT_DISABLE = 0,
    QMI8658A_INT_DATA_READY = 1,
    QMI8658A_INT_FIFO_WATERMARK = 2,
    QMI8658A_INT_FIFO_FULL = 3,
    QMI8658A_INT_MOTION_DETECT = 4
} qmi8658a_interrupt_t;

// 数据缓冲结构
typedef struct {
    qmi8658a_data_t data[QMI8658A_BUFFER_SIZE];
    uint32_t head;
    uint32_t tail;
    uint32_t count;
    bool overflow;
    SemaphoreHandle_t mutex;
} qmi8658a_buffer_t;

// FIFO配置结构
typedef struct {
    bool enable;
    uint8_t watermark;
    qmi8658a_interrupt_t interrupt_type;
    gpio_num_t interrupt_pin;
} qmi8658a_fifo_config_t;

class QMI8658A : public I2cDevice {
public:
    QMI8658A(i2c_master_bus_handle_t i2c_bus, uint8_t addr = QMI8658A_I2C_ADDRESS);
    ~QMI8658A();

    // 初始化和配置
    qmi8658a_error_t Initialize(const qmi8658a_config_t* config = nullptr);
    qmi8658a_error_t UpdateConfiguration(const qmi8658a_config_t* new_config);
    qmi8658a_error_t ValidateConfiguration(const qmi8658a_config_t* config);
    qmi8658a_error_t GetConfiguration(qmi8658a_config_t* config);
    
    // 运行时配置修改
    qmi8658a_error_t SetAccelRange(qmi8658a_acc_range_t range);
    qmi8658a_error_t SetGyroRange(qmi8658a_gyro_range_t range);
    qmi8658a_error_t SetAccelODR(qmi8658a_odr_t odr);
    qmi8658a_error_t SetGyroODR(qmi8658a_odr_t odr);
    qmi8658a_error_t SetOperationMode(qmi8658a_mode_t mode);
    
    // 中断和FIFO配置
    qmi8658a_error_t ConfigureInterrupt(qmi8658a_interrupt_t int_type, gpio_num_t pin);
    qmi8658a_error_t EnableFIFO(const qmi8658a_fifo_config_t* fifo_config);
    qmi8658a_error_t DisableFIFO();
    qmi8658a_error_t ReadFIFO(qmi8658a_data_t* data_array, uint8_t max_count, uint8_t* actual_count);
    
    // 数据缓冲管理
    qmi8658a_error_t InitializeBuffer();
    qmi8658a_error_t StartBufferedReading(uint32_t interval_ms);
    qmi8658a_error_t StopBufferedReading();
    qmi8658a_error_t GetBufferedData(qmi8658a_data_t* data, uint32_t max_count, uint32_t* actual_count);
    qmi8658a_error_t ClearBuffer();
    uint32_t GetBufferCount();
    bool IsBufferOverflow();
    
    // 校准功能
    qmi8658a_error_t StartCalibration(uint32_t duration_ms = 5000);
    qmi8658a_error_t GetCalibrationStatus(bool* is_calibrating, float* progress);
    qmi8658a_error_t ApplyCalibration(const qmi8658a_calibration_t* calibration);
    qmi8658a_error_t GetCalibrationData(qmi8658a_calibration_t* calibration);
    qmi8658a_error_t SaveCalibrationToNVS();
    qmi8658a_error_t LoadCalibrationFromNVS();

    // 原有方法保持不变
    qmi8658a_error_t SoftReset();
    qmi8658a_error_t SetMode(qmi8658a_mode_t mode);
    qmi8658a_error_t SetAccelConfig(qmi8658a_acc_range_t range, qmi8658a_odr_t odr);
    qmi8658a_error_t SetGyroConfig(qmi8658a_gyro_range_t range, qmi8658a_odr_t odr);

    // 数据读取
    qmi8658a_error_t ReadSensorData(qmi8658a_data_t* data);
    qmi8658a_error_t ReadAccelData(float* acc_x, float* acc_y, float* acc_z);
    qmi8658a_error_t ReadGyroData(float* gyro_x, float* gyro_y, float* gyro_z);
    qmi8658a_error_t ReadTemperature(float* temperature);

    // 状态和诊断方法
    qmi8658a_state_t GetState() const { return state_; }
    qmi8658a_error_t GetLastError() const { return last_error_; }
    bool IsDataReady();
    void DumpRegisters();
    void RunBaselineDiagnostics(uint16_t samples = 200, uint16_t interval_ms = 10);
    
    // 芯片信息
    uint8_t GetChipId();
    uint8_t GetRevisionId();
    
    // 静态连接检测方法（用于生产测试）
    static bool CheckConnection(i2c_master_bus_handle_t i2c_bus, uint8_t* detected_address = nullptr);

private:
    qmi8658a_config_t config_;
    qmi8658a_calibration_t calibration_;
    qmi8658a_state_t state_;
    qmi8658a_error_t last_error_;
    
    float acc_scale_;
    float gyro_scale_;
    
    // 校准相关
    bool is_calibrating_;
    uint32_t calibration_start_time_;
    uint32_t calibration_duration_;
    float calibration_acc_sum_[3];
    float calibration_gyro_sum_[3];
    uint32_t calibration_sample_count_;
    
    // 缓冲区相关
    qmi8658a_buffer_t data_buffer_;
    TaskHandle_t buffer_task_handle_;
    bool buffer_enabled_;
    uint32_t buffer_interval_ms_;
    
    // 中断相关
    gpio_num_t interrupt_pin_;
    qmi8658a_interrupt_t interrupt_type_;
    bool interrupt_enabled_;
    
    // FIFO相关
    qmi8658a_fifo_config_t fifo_config_;
    bool fifo_enabled_;

    // 错误处理和验证函数
    qmi8658a_error_t SetError(qmi8658a_error_t error);
    void CalculateScaleFactors();
    void UpdateScaleFactors();
    qmi8658a_error_t ApplyConfigurationChanges();
    
    // 新增：寄存器验证和重试机制
    qmi8658a_error_t WriteRegWithVerification(uint8_t reg, uint8_t value, uint8_t max_retries = 3);
    qmi8658a_error_t VerifyRegisterValue(uint8_t reg, uint8_t expected_value, const char* reg_name);
    qmi8658a_error_t WaitForDataReady(uint32_t timeout_ms = 1000);
    qmi8658a_error_t PerformSelfTest();
    
    // 缓冲区和中断处理
    static void BufferTask(void* parameter);
    static void IRAM_ATTR InterruptHandler(void* arg);
    qmi8658a_error_t AddToBuffer(const qmi8658a_data_t* data);
    qmi8658a_error_t GetFromBuffer(qmi8658a_data_t* data);
    
    // 工具函数
    int16_t ReadInt16(uint8_t reg);
};

#endif // QMI8658A_H