#include "qmi8658a.h"
#include <esp_log.h>
#include <cmath>
#include <cstring>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

#define TAG "QMI8658A"

QMI8658A::QMI8658A(i2c_master_bus_handle_t i2c_bus, uint8_t addr) 
    : I2cDevice(i2c_bus, addr), 
      state_(QMI8658A_STATE_UNINITIALIZED),
      last_error_(QMI8658A_OK),
      acc_scale_(1.0f), 
      gyro_scale_(1.0f),
      is_calibrating_(false),
      calibration_start_time_(0),
      calibration_duration_(0),
      calibration_sample_count_(0),
      buffer_task_handle_(nullptr),
      buffer_enabled_(false),
      buffer_interval_ms_(0),
      interrupt_pin_(GPIO_NUM_NC),
      interrupt_type_(QMI8658A_INT_DISABLE),
      interrupt_enabled_(false),
      fifo_enabled_(false) {
    // 默认配置 - 修正ODR设置以匹配实际寄存器值
    config_.mode = QMI8658A_MODE_DUAL;
    config_.acc_range = QMI8658A_ACC_RANGE_4G;  // 匹配CTRL2寄存器值0x16
    config_.gyro_range = QMI8658A_GYRO_RANGE_512DPS;  // 匹配CTRL3寄存器值0x56
    config_.acc_odr = QMI8658A_ODR_125HZ;  // 匹配实际寄存器值0x06
    config_.gyro_odr = QMI8658A_ODR_125HZ;  // 匹配实际寄存器值0x06
    
    // 初始化缓冲区结构
    memset(&data_buffer_, 0, sizeof(data_buffer_));
    data_buffer_.mutex = nullptr;
    
    // 初始化校准数据
    memset(&calibration_, 0, sizeof(calibration_));
    memset(calibration_acc_sum_, 0, sizeof(calibration_acc_sum_));
    memset(calibration_gyro_sum_, 0, sizeof(calibration_gyro_sum_));
    
    // 初始化FIFO配置
    memset(&fifo_config_, 0, sizeof(fifo_config_));
}

qmi8658a_error_t QMI8658A::Initialize(const qmi8658a_config_t* config) {
    ESP_LOGI(TAG, "Initializing QMI8658A sensor...");
    
    state_ = QMI8658A_STATE_INITIALIZING;
    
    // 执行自检
    qmi8658a_error_t result = PerformSelfTest();
    if (result != QMI8658A_OK) {
        ESP_LOGE(TAG, "Self-test failed during initialization");
        state_ = QMI8658A_STATE_ERROR;
        return result;
    }
    
    // 软件复位
    result = SoftReset();
    if (result != QMI8658A_OK) {
        ESP_LOGE(TAG, "Software reset failed");
        state_ = QMI8658A_STATE_ERROR;
        return result;
    }
    
    // 保存配置
    if (config) {
        config_ = *config;
    }
    
    // 计算比例因子
    CalculateScaleFactors();
    
    // 配置加速度计
    result = SetAccelConfig(config_.acc_range, config_.acc_odr);
    if (result != QMI8658A_OK) {
        ESP_LOGE(TAG, "Failed to configure accelerometer");
        state_ = QMI8658A_STATE_ERROR;
        return result;
    }
    
    // 配置陀螺仪
    result = SetGyroConfig(config_.gyro_range, config_.gyro_odr);
    if (result != QMI8658A_OK) {
        ESP_LOGE(TAG, "Failed to configure gyroscope");
        state_ = QMI8658A_STATE_ERROR;
        return result;
    }
    
    // 设置工作模式
    uint8_t mode_val = config_.mode;
    result = SetMode(static_cast<qmi8658a_mode_t>(mode_val));
    if (result != QMI8658A_OK) {
        ESP_LOGE(TAG, "Failed to set mode");
        state_ = QMI8658A_STATE_ERROR;
        return result;
    }
    
    // 等待传感器稳定
    vTaskDelay(pdMS_TO_TICKS(50));
    
    // 验证关键寄存器配置
    uint8_t expected_ctrl2 = (config_.acc_range << 4) | config_.acc_odr;
    result = VerifyRegisterValue(QMI8658A_CTRL2, expected_ctrl2, "CTRL2");
    if (result != QMI8658A_OK) {
        ESP_LOGE(TAG, "CTRL2 verification failed");
        state_ = QMI8658A_STATE_ERROR;
        return result;
    }

    uint8_t expected_ctrl3 = (config_.gyro_range << 4) | config_.gyro_odr;
    result = VerifyRegisterValue(QMI8658A_CTRL3, expected_ctrl3, "CTRL3 (Gyro Config)");
    if (result != QMI8658A_OK) {
        state_ = QMI8658A_STATE_ERROR;
        return result;
    }
    
    result = VerifyRegisterValue(QMI8658A_CTRL7, mode_val, "CTRL7 (Mode)");
    if (result != QMI8658A_OK) {
        state_ = QMI8658A_STATE_ERROR;
        return result;
    }
    
    // 等待数据就绪
    if (config_.mode != QMI8658A_MODE_DISABLE) {
        ESP_LOGI(TAG, "Waiting for sensor data to be ready...");
        result = WaitForDataReady(2000); // 2秒超时
        if (result != QMI8658A_OK) {
            ESP_LOGW(TAG, "Data ready timeout, but continuing initialization");
            // 不将此作为致命错误，继续初始化
        }
    }
    
    // 诊断寄存器状态
    ESP_LOGI(TAG, "=== QMI8658A Register Diagnostics ===");
    uint8_t ctrl1 = ReadReg(QMI8658A_CTRL1);
    uint8_t ctrl2 = ReadReg(QMI8658A_CTRL2);
    uint8_t ctrl3 = ReadReg(QMI8658A_CTRL3);
    uint8_t ctrl7 = ReadReg(QMI8658A_CTRL7);
    uint8_t status0 = ReadReg(QMI8658A_STATUS0);
    uint8_t status1 = ReadReg(QMI8658A_STATUS1);
    
    ESP_LOGI(TAG, "CTRL1: 0x%02X, CTRL2: 0x%02X, CTRL3: 0x%02X, CTRL7: 0x%02X", 
             ctrl1, ctrl2, ctrl3, ctrl7);
    ESP_LOGI(TAG, "STATUS0: 0x%02X, STATUS1: 0x%02X", status0, status1);
    ESP_LOGI(TAG, "=====================================");
    
    // 等待数据就绪
    ESP_LOGI(TAG, "Waiting for data ready...");
    uint32_t wait_count = 0;
    const uint32_t max_wait = 100; // 最多等待1秒
    
    while (wait_count < max_wait) {
        uint8_t status = ReadReg(QMI8658A_STATUS0);
        if (status & 0x03) { // 检查加速度计或陀螺仪数据就绪
            ESP_LOGI(TAG, "Data ready after %lu ms", wait_count * 10);
            break;
        }
        vTaskDelay(pdMS_TO_TICKS(10));
        wait_count++;
    }
    
    if (wait_count >= max_wait) {
        ESP_LOGW(TAG, "Data ready timeout, but initialization completed");
    }
    
    state_ = QMI8658A_STATE_READY;
    ESP_LOGI(TAG, "QMI8658A initialization completed successfully");
    
    return QMI8658A_OK;
}

uint8_t QMI8658A::GetChipId() {
    uint8_t chip_id = ReadReg(QMI8658A_WHO_AM_I);
    if (chip_id == 0xFF) {
        ESP_LOGE(TAG, "Failed to read chip ID register, I2C communication error");
        return 0xFF;
    }
    return chip_id;
}

uint8_t QMI8658A::GetRevisionId() {
    return ReadReg(QMI8658A_REVISION_ID);
}

// 静态连接检测方法（用于生产测试）
bool QMI8658A::CheckConnection(i2c_master_bus_handle_t i2c_bus, uint8_t* detected_address) {
    // 可能的QMI8658A I2C地址
    uint8_t possible_addresses[] = {0x6A, 0x6B};
    uint8_t num_addresses = sizeof(possible_addresses) / sizeof(possible_addresses[0]);
    
    for (uint8_t i = 0; i < num_addresses; i++) {
        uint8_t addr = possible_addresses[i];
        
        // 创建临时I2C设备句柄进行测试
        i2c_master_dev_handle_t dev_handle;
        i2c_device_config_t dev_cfg = {
            .dev_addr_length = I2C_ADDR_BIT_LEN_7,
            .device_address = addr,
            .scl_speed_hz = 400000,  // 400kHz
        };
        
        esp_err_t ret = i2c_master_bus_add_device(i2c_bus, &dev_cfg, &dev_handle);
        if (ret != ESP_OK) {
            continue;  // 尝试下一个地址
        }
        
        // 尝试读取WHO_AM_I寄存器
        uint8_t reg_addr = QMI8658A_WHO_AM_I;
        uint8_t chip_id = 0;
        
        ret = i2c_master_transmit_receive(dev_handle, &reg_addr, 1, &chip_id, 1, 1000);
        
        // 清理设备句柄
        i2c_master_bus_rm_device(dev_handle);
        
        if (ret == ESP_OK && chip_id == QMI8658A_CHIP_ID) {
            // 找到有效的QMI8658A设备
            if (detected_address != nullptr) {
                *detected_address = addr;
            }
            return true;
        }
    }
    
    return false;  // 未找到有效设备
}

qmi8658a_error_t QMI8658A::SoftReset() {
    ESP_LOGI(TAG, "Performing soft reset...");
    
    // 使用带验证的写入函数
    qmi8658a_error_t result = WriteRegWithVerification(QMI8658A_CTRL1, 0xB0);
    if (result != QMI8658A_OK) {
        ESP_LOGE(TAG, "Failed to perform soft reset");
        return result;
    }
    
    // 等待复位完成
    vTaskDelay(pdMS_TO_TICKS(20)); // 增加等待时间确保复位完成
    
    // 验证复位是否成功 - 检查芯片ID
    uint8_t chip_id = GetChipId();
    if (chip_id != QMI8658A_CHIP_ID) {
        ESP_LOGE(TAG, "Soft reset verification failed: chip ID = 0x%02X", chip_id);
        return SetError(QMI8658A_ERROR_INIT_FAILED);
    }
    
    ESP_LOGI(TAG, "Soft reset completed and verified successfully");
    return QMI8658A_OK;
}

qmi8658a_error_t QMI8658A::SetMode(qmi8658a_mode_t mode) {
    ESP_LOGI(TAG, "Setting mode: %d", mode);
    
    // 使用带验证的写入函数，最多重试3次
    qmi8658a_error_t result = WriteRegWithVerification(QMI8658A_CTRL7, mode, 3);
    if (result != QMI8658A_OK) {
        ESP_LOGE(TAG, "Failed to set mode: %d", mode);
        return result;
    }
    
    // 更新配置
    config_.mode = mode;
    
    ESP_LOGI(TAG, "Mode set successfully: CTRL7=0x%02X", mode);
    return QMI8658A_OK;
}

qmi8658a_error_t QMI8658A::SetAccelConfig(qmi8658a_acc_range_t range, qmi8658a_odr_t odr) {
    ESP_LOGI(TAG, "Setting accelerometer config: range=%d, odr=%d", range, odr);
    
    uint8_t ctrl2_val = (range << 4) | odr;
    
    // 使用带验证的写入函数，最多重试3次
    qmi8658a_error_t result = WriteRegWithVerification(QMI8658A_CTRL2, ctrl2_val, 3);
    if (result != QMI8658A_OK) {
        ESP_LOGE(TAG, "Failed to set accelerometer config: range=%d, odr=%d", range, odr);
        return result;
    }
    
    // 更新配置
    config_.acc_range = range;
    config_.acc_odr = odr;
    
    ESP_LOGI(TAG, "Accelerometer config set successfully: CTRL2=0x%02X", ctrl2_val);
    return QMI8658A_OK;
}

qmi8658a_error_t QMI8658A::SetGyroConfig(qmi8658a_gyro_range_t range, qmi8658a_odr_t odr) {
    ESP_LOGI(TAG, "Setting gyroscope config: range=%d, odr=%d", range, odr);
    
    uint8_t ctrl3_val = (range << 4) | odr;
    
    // 使用带验证的写入函数，最多重试3次
    qmi8658a_error_t result = WriteRegWithVerification(QMI8658A_CTRL3, ctrl3_val, 3);
    if (result != QMI8658A_OK) {
        ESP_LOGE(TAG, "Failed to set gyroscope config: range=%d, odr=%d", range, odr);
        return result;
    }
    
    // 更新配置
    config_.gyro_range = range;
    config_.gyro_odr = odr;
    
    ESP_LOGI(TAG, "Gyroscope config set successfully: CTRL3=0x%02X", ctrl3_val);
    return QMI8658A_OK;
}

void QMI8658A::CalculateScaleFactors() {
    // 计算加速度计比例因子 (g)
    switch (config_.acc_range) {
        case QMI8658A_ACC_RANGE_2G:
            acc_scale_ = 2.0f / 32768.0f;
            break;
        case QMI8658A_ACC_RANGE_4G:
            acc_scale_ = 4.0f / 32768.0f;
            break;
        case QMI8658A_ACC_RANGE_8G:
            acc_scale_ = 8.0f / 32768.0f;
            break;
        case QMI8658A_ACC_RANGE_16G:
            acc_scale_ = 16.0f / 32768.0f;
            break;
    }
    
    // 计算陀螺仪比例因子 (dps)
    switch (config_.gyro_range) {
        case QMI8658A_GYRO_RANGE_16DPS:
            gyro_scale_ = 16.0f / 32768.0f;
            break;
        case QMI8658A_GYRO_RANGE_32DPS:
            gyro_scale_ = 32.0f / 32768.0f;
            break;
        case QMI8658A_GYRO_RANGE_64DPS:
            gyro_scale_ = 64.0f / 32768.0f;
            break;
        case QMI8658A_GYRO_RANGE_128DPS:
            gyro_scale_ = 128.0f / 32768.0f;
            break;
        case QMI8658A_GYRO_RANGE_256DPS:
            gyro_scale_ = 256.0f / 32768.0f;
            break;
        case QMI8658A_GYRO_RANGE_512DPS:
            gyro_scale_ = 512.0f / 32768.0f;
            break;
        case QMI8658A_GYRO_RANGE_1024DPS:
            gyro_scale_ = 1024.0f / 32768.0f;
            break;
        case QMI8658A_GYRO_RANGE_2048DPS:
            gyro_scale_ = 2048.0f / 32768.0f;
            break;
    }
    
    ESP_LOGI(TAG, "Scale factors - Acc: %.6f, Gyro: %.6f", acc_scale_, gyro_scale_);
}

int16_t QMI8658A::ReadInt16(uint8_t reg_low) {
    uint8_t data[2];
    ReadRegs(reg_low, data, 2);
    return (int16_t)((data[1] << 8) | data[0]);
}

qmi8658a_error_t QMI8658A::ReadAccelData(float* acc_x, float* acc_y, float* acc_z) {
    if (!acc_x || !acc_y || !acc_z) {
        return SetError(QMI8658A_ERROR_INVALID_PARAM);
    }
    
    if (state_ != QMI8658A_STATE_READY) {
        return SetError(QMI8658A_ERROR_DATA_NOT_READY);
    }
    
    int16_t raw_x = ReadInt16(QMI8658A_AX_L);
    int16_t raw_y = ReadInt16(QMI8658A_AY_L);
    int16_t raw_z = ReadInt16(QMI8658A_AZ_L);
    
    *acc_x = raw_x * acc_scale_;
    *acc_y = raw_y * acc_scale_;
    *acc_z = raw_z * acc_scale_;
    
    return QMI8658A_OK;
}

qmi8658a_error_t QMI8658A::ReadGyroData(float* gyro_x, float* gyro_y, float* gyro_z) {
    if (!gyro_x || !gyro_y || !gyro_z) {
        return SetError(QMI8658A_ERROR_INVALID_PARAM);
    }
    
    if (state_ != QMI8658A_STATE_READY) {
        return SetError(QMI8658A_ERROR_DATA_NOT_READY);
    }
    
    int16_t raw_x = ReadInt16(QMI8658A_GX_L);
    int16_t raw_y = ReadInt16(QMI8658A_GY_L);
    int16_t raw_z = ReadInt16(QMI8658A_GZ_L);
    
    *gyro_x = raw_x * gyro_scale_;
    *gyro_y = raw_y * gyro_scale_;
    *gyro_z = raw_z * gyro_scale_;
    
    return QMI8658A_OK;
}

qmi8658a_error_t QMI8658A::ReadTemperature(float* temperature) {
    if (!temperature) {
        return SetError(QMI8658A_ERROR_INVALID_PARAM);
    }
    
    if (state_ != QMI8658A_STATE_READY) {
        return SetError(QMI8658A_ERROR_DATA_NOT_READY);
    }
    
    int16_t raw_temp = ReadInt16(QMI8658A_TEMP_L);
    // 温度转换公式：温度 = raw_temp / 256.0 (°C)
    *temperature = raw_temp / 256.0f;
    return QMI8658A_OK;
}




qmi8658a_error_t QMI8658A::InitializeBuffer() {
    // 初始化缓冲区
    memset(&data_buffer_, 0, sizeof(data_buffer_));
    data_buffer_.mutex = xSemaphoreCreateMutex();
    if (data_buffer_.mutex == NULL) {
        return SetError(QMI8658A_ERROR_INIT_FAILED);
    }
    
    buffer_enabled_ = false;
    buffer_task_handle_ = NULL;
    
    ESP_LOGI(TAG, "Data buffer initialized");
    return QMI8658A_OK;
}

qmi8658a_error_t QMI8658A::StartBufferedReading(uint32_t interval_ms) {
    if (state_ != QMI8658A_STATE_READY) {
        return SetError(QMI8658A_ERROR_DATA_NOT_READY);
    }
    
    if (buffer_enabled_) {
        ESP_LOGW(TAG, "Buffered reading already started");
        return QMI8658A_OK;
    }
    
    buffer_interval_ms_ = interval_ms;
    buffer_enabled_ = true;
    
    // 创建缓冲任务
    BaseType_t result = xTaskCreate(
        BufferTask,
        "qmi8658a_buffer",
        4096,
        this,
        5,
        &buffer_task_handle_
    );
    
    if (result != pdPASS) {
        buffer_enabled_ = false;
        return SetError(QMI8658A_ERROR_INIT_FAILED);
    }
    
    ESP_LOGI(TAG, "Started buffered reading with %" PRIu32 " ms interval", interval_ms);
    return QMI8658A_OK;
}

qmi8658a_error_t QMI8658A::StopBufferedReading() {
    if (!buffer_enabled_) {
        return QMI8658A_OK;
    }
    
    buffer_enabled_ = false;
    
    if (buffer_task_handle_ != NULL) {
        vTaskDelete(buffer_task_handle_);
        buffer_task_handle_ = NULL;
    }
    
    ESP_LOGI(TAG, "Stopped buffered reading");
    return QMI8658A_OK;
}

qmi8658a_error_t QMI8658A::GetBufferedData(qmi8658a_data_t* data, uint32_t max_count, uint32_t* actual_count) {
    if (!data || !actual_count) {
        return SetError(QMI8658A_ERROR_INVALID_PARAM);
    }
    
    *actual_count = 0;
    
    if (xSemaphoreTake(data_buffer_.mutex, pdMS_TO_TICKS(100)) != pdTRUE) {
        return SetError(QMI8658A_ERROR_TIMEOUT);
    }
    
    uint32_t count = 0;
    while (count < max_count && data_buffer_.count > 0) {
        data[count] = data_buffer_.data[data_buffer_.tail];
        data_buffer_.tail = (data_buffer_.tail + 1) % QMI8658A_BUFFER_SIZE;
        data_buffer_.count--;
        count++;
    }
    
    *actual_count = count;
    xSemaphoreGive(data_buffer_.mutex);
    
    return QMI8658A_OK;
}

qmi8658a_error_t QMI8658A::ClearBuffer() {
    if (xSemaphoreTake(data_buffer_.mutex, pdMS_TO_TICKS(100)) != pdTRUE) {
        return SetError(QMI8658A_ERROR_TIMEOUT);
    }
    
    data_buffer_.head = 0;
    data_buffer_.tail = 0;
    data_buffer_.count = 0;
    data_buffer_.overflow = false;
    
    xSemaphoreGive(data_buffer_.mutex);
    
    ESP_LOGI(TAG, "Buffer cleared");
    return QMI8658A_OK;
}

uint32_t QMI8658A::GetBufferCount() {
    if (xSemaphoreTake(data_buffer_.mutex, pdMS_TO_TICKS(10)) != pdTRUE) {
        return 0;
    }
    
    uint32_t count = data_buffer_.count;
    xSemaphoreGive(data_buffer_.mutex);
    
    return count;
}

bool QMI8658A::IsBufferOverflow() {
    if (xSemaphoreTake(data_buffer_.mutex, pdMS_TO_TICKS(10)) != pdTRUE) {
        return false;
    }
    
    bool overflow = data_buffer_.overflow;
    xSemaphoreGive(data_buffer_.mutex);
    
    return overflow;
}

qmi8658a_error_t QMI8658A::ConfigureInterrupt(qmi8658a_interrupt_t int_type, gpio_num_t pin) {
    interrupt_type_ = int_type;
    interrupt_pin_ = pin;
    
    if (int_type == QMI8658A_INT_DISABLE) {
        interrupt_enabled_ = false;
        return QMI8658A_OK;
    }
    
    // 配置GPIO中断
    gpio_config_t io_conf = {};
    io_conf.intr_type = GPIO_INTR_POSEDGE;
    io_conf.mode = GPIO_MODE_INPUT;
    io_conf.pin_bit_mask = (1ULL << pin);
    io_conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
    io_conf.pull_up_en = GPIO_PULLUP_ENABLE;
    
    esp_err_t ret = gpio_config(&io_conf);
    if (ret != ESP_OK) {
        return SetError(QMI8658A_ERROR_INIT_FAILED);
    }
    
    // 安装中断服务
    ret = gpio_install_isr_service(0);
    if (ret != ESP_OK && ret != ESP_ERR_INVALID_STATE) {
        return SetError(QMI8658A_ERROR_INIT_FAILED);
    }
    
    // 添加中断处理程序
    ret = gpio_isr_handler_add(pin, InterruptHandler, this);
    if (ret != ESP_OK) {
        return SetError(QMI8658A_ERROR_INIT_FAILED);
    }
    
    // 配置传感器中断
    uint8_t int_config = 0;
    switch (int_type) {
        case QMI8658A_INT_DATA_READY:
            int_config = 0x01;
            break;
        case QMI8658A_INT_FIFO_WATERMARK:
            int_config = 0x02;
            break;
        case QMI8658A_INT_FIFO_FULL:
            int_config = 0x04;
            break;
        case QMI8658A_INT_MOTION_DETECT:
            int_config = 0x08;
            break;
        default:
            break;
    }
    
    WriteReg(0x56, int_config); // INT_EN寄存器
    
    interrupt_enabled_ = true;
    ESP_LOGI(TAG, "Interrupt configured: type=%d, pin=%d", int_type, pin);
    
    return QMI8658A_OK;
}

qmi8658a_error_t QMI8658A::EnableFIFO(const qmi8658a_fifo_config_t* fifo_config) {
    if (!fifo_config) {
        return SetError(QMI8658A_ERROR_INVALID_PARAM);
    }
    
    fifo_config_ = *fifo_config;
    
    // 配置FIFO
    uint8_t fifo_ctrl = 0x40; // 启用FIFO
    if (fifo_config->watermark > 0 && fifo_config->watermark <= QMI8658A_FIFO_SIZE) {
        fifo_ctrl |= (fifo_config->watermark & 0x1F);
    }
    
    WriteReg(0x13, fifo_ctrl); // FIFO_CTRL寄存器
    
    // 如果配置了中断，设置中断
    if (fifo_config->interrupt_type != QMI8658A_INT_DISABLE) {
        qmi8658a_error_t result = ConfigureInterrupt(fifo_config->interrupt_type, fifo_config->interrupt_pin);
        if (result != QMI8658A_OK) {
            return result;
        }
    }
    
    fifo_enabled_ = true;
    ESP_LOGI(TAG, "FIFO enabled with watermark: %d", fifo_config->watermark);
    
    return QMI8658A_OK;
}

qmi8658a_error_t QMI8658A::DisableFIFO() {
    WriteReg(0x13, 0x00); // 禁用FIFO
    fifo_enabled_ = false;
    ESP_LOGI(TAG, "FIFO disabled");
    return QMI8658A_OK;
}

qmi8658a_error_t QMI8658A::ReadFIFO(qmi8658a_data_t* data_array, uint8_t max_count, uint8_t* actual_count) {
    if (!data_array || !actual_count) {
        return SetError(QMI8658A_ERROR_INVALID_PARAM);
    }
    
    *actual_count = 0;
    
    if (!fifo_enabled_) {
        return SetError(QMI8658A_ERROR_DATA_NOT_READY);
    }
    
    // 读取FIFO状态
    uint8_t fifo_status = ReadReg(0x14); // FIFO_STATUS寄存器
    uint8_t fifo_count = fifo_status & 0x1F;
    
    if (fifo_count == 0) {
        return QMI8658A_OK;
    }
    
    uint8_t read_count = (fifo_count < max_count) ? fifo_count : max_count;
    
    for (uint8_t i = 0; i < read_count; i++) {
        qmi8658a_error_t result = ReadSensorData(&data_array[i]);
        if (result != QMI8658A_OK) {
            return result;
        }
    }
    
    *actual_count = read_count;
    return QMI8658A_OK;
}

qmi8658a_error_t QMI8658A::AddToBuffer(const qmi8658a_data_t* data) {
    if (!data) {
        return QMI8658A_ERROR_INVALID_PARAM;
    }
    
    if (xSemaphoreTake(data_buffer_.mutex, pdMS_TO_TICKS(10)) != pdTRUE) {
        return QMI8658A_ERROR_TIMEOUT;
    }
    
    if (data_buffer_.count >= QMI8658A_BUFFER_SIZE) {
        // 缓冲区满，覆盖最旧的数据
        data_buffer_.tail = (data_buffer_.tail + 1) % QMI8658A_BUFFER_SIZE;
        data_buffer_.overflow = true;
    } else {
        data_buffer_.count++;
    }
    
    data_buffer_.data[data_buffer_.head] = *data;
    data_buffer_.head = (data_buffer_.head + 1) % QMI8658A_BUFFER_SIZE;
    
    xSemaphoreGive(data_buffer_.mutex);
    
    return QMI8658A_OK;
}

void QMI8658A::BufferTask(void* parameter) {
    QMI8658A* sensor = static_cast<QMI8658A*>(parameter);
    qmi8658a_data_t data;
    
    while (sensor->buffer_enabled_) {
        if (sensor->ReadSensorData(&data) == QMI8658A_OK) {
            sensor->AddToBuffer(&data);
            
            // 如果正在校准，添加到校准数据
            if (sensor->is_calibrating_) {
                sensor->calibration_acc_sum_[0] += data.acc_x;
                sensor->calibration_acc_sum_[1] += data.acc_y;
                sensor->calibration_acc_sum_[2] += data.acc_z;
                sensor->calibration_gyro_sum_[0] += data.gyro_x;
                sensor->calibration_gyro_sum_[1] += data.gyro_y;
                sensor->calibration_gyro_sum_[2] += data.gyro_z;
                sensor->calibration_sample_count_++;
            }
        }
        
        vTaskDelay(pdMS_TO_TICKS(sensor->buffer_interval_ms_));
    }
    
    vTaskDelete(NULL);
}

void IRAM_ATTR QMI8658A::InterruptHandler(void* arg) {
    QMI8658A* sensor = static_cast<QMI8658A*>(arg);
    
    // 在中断中只做最小的处理
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
    
    // 可以在这里设置一个标志或发送通知
    // 实际的数据读取应该在任务中进行
    
    // 使用sensor参数避免未使用变量警告
    (void)sensor;
    (void)xHigherPriorityTaskWoken;
}

qmi8658a_error_t QMI8658A::ReadSensorData(qmi8658a_data_t* data) {
    if (!data) {
        ESP_LOGE(TAG, "Data pointer is null");
        return SetError(QMI8658A_ERROR_INVALID_PARAM);
    }
    
    if (state_ != QMI8658A_STATE_READY) {
        ESP_LOGE(TAG, "Sensor not ready, current state: %d", state_);
        return SetError(QMI8658A_ERROR_DATA_NOT_READY);
    }
    
    // 检查数据是否就绪
    if (!IsDataReady()) {
        ESP_LOGW(TAG, "Sensor data not ready, STATUS0: 0x%02X", ReadReg(0x2D));
        return SetError(QMI8658A_ERROR_DATA_NOT_READY);
    }
    
    // 初始化数据结构
    memset(data, 0, sizeof(qmi8658a_data_t));
    data->timestamp = esp_timer_get_time();
    data->valid = false;
    
    qmi8658a_error_t result;
    
    // 读取加速度数据
    if (config_.mode == QMI8658A_MODE_ACC_ONLY || config_.mode == QMI8658A_MODE_DUAL) {
        result = ReadAccelData(&data->acc_x, &data->acc_y, &data->acc_z);
        if (result != QMI8658A_OK) {
            ESP_LOGE(TAG, "Failed to read accelerometer data, error: %d", result);
            return result;
        }
        ESP_LOGD(TAG, "Accel data: X=%.3f, Y=%.3f, Z=%.3f", data->acc_x, data->acc_y, data->acc_z);
    }
    
    // 读取陀螺仪数据
    if (config_.mode == QMI8658A_MODE_GYRO_ONLY || config_.mode == QMI8658A_MODE_DUAL) {
        result = ReadGyroData(&data->gyro_x, &data->gyro_y, &data->gyro_z);
        if (result != QMI8658A_OK) {
            ESP_LOGE(TAG, "Failed to read gyroscope data, error: %d", result);
            return result;
        }
        ESP_LOGD(TAG, "Gyro data: X=%.3f, Y=%.3f, Z=%.3f", data->gyro_x, data->gyro_y, data->gyro_z);
    }
    
    // 读取温度数据
    result = ReadTemperature(&data->temperature);
    if (result != QMI8658A_OK) {
        ESP_LOGE(TAG, "Failed to read temperature data, error: %d", result);
        return result;
    }
    
    ESP_LOGD(TAG, "Temperature: %.2f°C", data->temperature);
    data->valid = true;
    ESP_LOGD(TAG, "Successfully read sensor data");
    return QMI8658A_OK;
}

bool QMI8658A::IsDataReady() {
    if (state_ != QMI8658A_STATE_READY) {
        return false;
    }
    
    // 读取状态寄存器检查数据是否准备就绪
    uint8_t status = ReadReg(0x2D); // STATUS0寄存器
    return (status & 0x03) != 0; // 检查加速度计和陀螺仪数据就绪位
}

qmi8658a_error_t QMI8658A::SetError(qmi8658a_error_t error) {
    last_error_ = error;
    return error;
}

// 新增：带验证的寄存器写入函数
qmi8658a_error_t QMI8658A::WriteRegWithVerification(uint8_t reg, uint8_t value, uint8_t max_retries) {
    for (uint8_t retry = 0; retry < max_retries; retry++) {
        // 写入寄存器
        esp_err_t ret = WriteRegWithError(reg, value);
        if (ret != ESP_OK) {
            ESP_LOGE(TAG, "Failed to write register 0x%02X (attempt %d/%d): %s", 
                     reg, retry + 1, max_retries, esp_err_to_name(ret));
            if (retry == max_retries - 1) {
                return SetError(QMI8658A_ERROR_I2C_COMM);
            }
            vTaskDelay(pdMS_TO_TICKS(10)); // 等待10ms后重试
            continue;
        }
        
        // 等待写入完成
        vTaskDelay(pdMS_TO_TICKS(5));
        
        // 读回验证
        uint8_t read_value = ReadReg(reg);
        if (read_value == 0xFF) {
            ESP_LOGE(TAG, "Failed to read back register 0x%02X for verification (attempt %d/%d)", 
                     reg, retry + 1, max_retries);
            if (retry == max_retries - 1) {
                return SetError(QMI8658A_ERROR_I2C_COMM);
            }
            vTaskDelay(pdMS_TO_TICKS(10));
            continue;
        }
        
        if (read_value == value) {
            ESP_LOGI(TAG, "Register 0x%02X successfully written and verified: 0x%02X", reg, value);
            return QMI8658A_OK;
        } else {
            ESP_LOGW(TAG, "Register 0x%02X verification failed (attempt %d/%d): wrote 0x%02X, read 0x%02X", 
                     reg, retry + 1, max_retries, value, read_value);
            if (retry == max_retries - 1) {
                return SetError(QMI8658A_ERROR_CONFIG_FAILED);
            }
            vTaskDelay(pdMS_TO_TICKS(10));
        }
    }
    
    return SetError(QMI8658A_ERROR_CONFIG_FAILED);
}

// 新增：验证寄存器值函数
qmi8658a_error_t QMI8658A::VerifyRegisterValue(uint8_t reg, uint8_t expected_value, const char* reg_name) {
    uint8_t actual_value = ReadReg(reg);
    if (actual_value == 0xFF) {
        ESP_LOGE(TAG, "Failed to read %s register (0x%02X) for verification", reg_name, reg);
        return SetError(QMI8658A_ERROR_I2C_COMM);
    }
    
    if (actual_value != expected_value) {
        ESP_LOGE(TAG, "%s register (0x%02X) verification failed: expected 0x%02X, got 0x%02X", 
                 reg_name, reg, expected_value, actual_value);
        return SetError(QMI8658A_ERROR_CONFIG_FAILED);
    }
    
    ESP_LOGI(TAG, "%s register (0x%02X) verified successfully: 0x%02X", reg_name, reg, actual_value);
    return QMI8658A_OK;
}

// 新增：等待数据就绪函数
qmi8658a_error_t QMI8658A::WaitForDataReady(uint32_t timeout_ms) {
    uint32_t start_time = esp_timer_get_time() / 1000; // 转换为毫秒
    uint32_t elapsed_time = 0;
    
    ESP_LOGI(TAG, "Waiting for data ready (timeout: %lu ms)...", timeout_ms);
    
    while (elapsed_time < timeout_ms) {
        uint8_t status0 = ReadReg(QMI8658A_STATUS0);
        if (status0 == 0xFF) {
            ESP_LOGE(TAG, "Failed to read STATUS0 register while waiting for data ready");
            return SetError(QMI8658A_ERROR_I2C_COMM);
        }
        
        // 检查加速度计和陀螺仪数据是否就绪
        bool acc_ready = (status0 & 0x01) != 0;
        bool gyro_ready = (status0 & 0x02) != 0;
        
        if (acc_ready && gyro_ready) {
            ESP_LOGI(TAG, "Data ready detected after %lu ms (STATUS0: 0x%02X)", elapsed_time, status0);
            return QMI8658A_OK;
        }
        
        vTaskDelay(pdMS_TO_TICKS(10)); // 等待10ms
        elapsed_time = (esp_timer_get_time() / 1000) - start_time;
    }
    
    ESP_LOGE(TAG, "Timeout waiting for data ready after %lu ms", timeout_ms);
    return SetError(QMI8658A_ERROR_TIMEOUT);
}

// 新增：自检函数
qmi8658a_error_t QMI8658A::PerformSelfTest() {
    ESP_LOGI(TAG, "Performing self-test...");
    
    // 检查芯片ID
    uint8_t chip_id = GetChipId();
    if (chip_id != QMI8658A_CHIP_ID) {
        ESP_LOGE(TAG, "Self-test failed: Invalid chip ID 0x%02X", chip_id);
        return SetError(QMI8658A_ERROR_CHIP_ID);
    }
    
    // 检查关键寄存器的可读性
    uint8_t test_regs[] = {QMI8658A_WHO_AM_I, QMI8658A_REVISION_ID, QMI8658A_STATUS0, QMI8658A_STATUS1};
    const char* test_reg_names[] = {"WHO_AM_I", "REVISION_ID", "STATUS0", "STATUS1"};
    
    for (int i = 0; i < 4; i++) {
        uint8_t value = ReadReg(test_regs[i]);
        if (value == 0xFF) {
            ESP_LOGE(TAG, "Self-test failed: Cannot read %s register (0x%02X)", 
                     test_reg_names[i], test_regs[i]);
            return SetError(QMI8658A_ERROR_I2C_COMM);
        }
        ESP_LOGI(TAG, "Self-test: %s (0x%02X) = 0x%02X", test_reg_names[i], test_regs[i], value);
    }
    
    ESP_LOGI(TAG, "Self-test completed successfully");
    return QMI8658A_OK;
}

qmi8658a_error_t QMI8658A::UpdateConfiguration(const qmi8658a_config_t* new_config) {
    if (!new_config) {
        return SetError(QMI8658A_ERROR_INVALID_PARAM);
    }
    
    // 验证新配置
    qmi8658a_error_t result = ValidateConfiguration(new_config);
    if (result != QMI8658A_OK) {
        return result;
    }
    
    // 保存旧配置以便回滚
    qmi8658a_config_t old_config = config_;
    config_ = *new_config;
    
    // 应用配置更改
    result = ApplyConfigurationChanges();
    if (result != QMI8658A_OK) {
        // 回滚到旧配置
        config_ = old_config;
        return result;
    }
    
    ESP_LOGI(TAG, "Configuration updated successfully");
    return QMI8658A_OK;
}

qmi8658a_error_t QMI8658A::ValidateConfiguration(const qmi8658a_config_t* config) {
    if (!config) {
        return QMI8658A_ERROR_INVALID_PARAM;
    }
    
    // 验证加速度计量程
    if (config->acc_range > QMI8658A_ACC_RANGE_16G) {
        ESP_LOGE(TAG, "Invalid accelerometer range: %d", config->acc_range);
        return QMI8658A_ERROR_INVALID_PARAM;
    }
    
    // 验证陀螺仪量程
    if (config->gyro_range > QMI8658A_GYRO_RANGE_2048DPS) {
        ESP_LOGE(TAG, "Invalid gyroscope range: %d", config->gyro_range);
        return QMI8658A_ERROR_INVALID_PARAM;
    }
    
    // 验证ODR设置
    if (config->acc_odr > QMI8658A_ODR_8000HZ || config->gyro_odr > QMI8658A_ODR_8000HZ) {
        ESP_LOGE(TAG, "Invalid ODR setting");
        return QMI8658A_ERROR_INVALID_PARAM;
    }
    
    // 验证工作模式
    if (config->mode > QMI8658A_MODE_DUAL) {
        ESP_LOGE(TAG, "Invalid operation mode: %d", config->mode);
        return QMI8658A_ERROR_INVALID_PARAM;
    }
    
    return QMI8658A_OK;
}

qmi8658a_error_t QMI8658A::GetConfiguration(qmi8658a_config_t* config) {
    if (!config) {
        return SetError(QMI8658A_ERROR_INVALID_PARAM);
    }
    
    *config = config_;
    return QMI8658A_OK;
}

qmi8658a_error_t QMI8658A::SetAccelRange(qmi8658a_acc_range_t range) {
    if (range > QMI8658A_ACC_RANGE_16G) {
        return SetError(QMI8658A_ERROR_INVALID_PARAM);
    }
    
    config_.acc_range = range;
    qmi8658a_error_t result = SetAccelConfig(range, config_.acc_odr);
    if (result == QMI8658A_OK) {
        UpdateScaleFactors();
    }
    return result;
}

qmi8658a_error_t QMI8658A::SetGyroRange(qmi8658a_gyro_range_t range) {
    if (range > QMI8658A_GYRO_RANGE_2048DPS) {
        return SetError(QMI8658A_ERROR_INVALID_PARAM);
    }
    
    config_.gyro_range = range;
    qmi8658a_error_t result = SetGyroConfig(range, config_.gyro_odr);
    if (result == QMI8658A_OK) {
        UpdateScaleFactors();
    }
    return result;
}

qmi8658a_error_t QMI8658A::SetAccelODR(qmi8658a_odr_t odr) {
    if (odr > QMI8658A_ODR_8000HZ) {
        return SetError(QMI8658A_ERROR_INVALID_PARAM);
    }
    
    config_.acc_odr = odr;
    return SetAccelConfig(config_.acc_range, odr);
}

qmi8658a_error_t QMI8658A::SetGyroODR(qmi8658a_odr_t odr) {
    if (odr > QMI8658A_ODR_8000HZ) {
        return SetError(QMI8658A_ERROR_INVALID_PARAM);
    }
    
    config_.gyro_odr = odr;
    return SetGyroConfig(config_.gyro_range, odr);
}

qmi8658a_error_t QMI8658A::SetOperationMode(qmi8658a_mode_t mode) {
    if (mode > QMI8658A_MODE_DUAL) {
        return SetError(QMI8658A_ERROR_INVALID_PARAM);
    }
    
    config_.mode = mode;
    return SetMode(mode);
}

qmi8658a_error_t QMI8658A::StartCalibration(uint32_t duration_ms) {
    if (state_ != QMI8658A_STATE_READY) {
        return SetError(QMI8658A_ERROR_DATA_NOT_READY);
    }
    
    is_calibrating_ = true;
    calibration_start_time_ = esp_timer_get_time() / 1000; // 转换为毫秒
    calibration_duration_ = duration_ms;
    calibration_sample_count_ = 0;
    
    // 清零累加器
    memset(calibration_acc_sum_, 0, sizeof(calibration_acc_sum_));
    memset(calibration_gyro_sum_, 0, sizeof(calibration_gyro_sum_));
    
    ESP_LOGI(TAG, "Started calibration for %" PRIu32 " ms", duration_ms);
    return QMI8658A_OK;
}

qmi8658a_error_t QMI8658A::GetCalibrationStatus(bool* is_calibrating, float* progress) {
    if (!is_calibrating || !progress) {
        return SetError(QMI8658A_ERROR_INVALID_PARAM);
    }
    
    *is_calibrating = is_calibrating_;
    
    if (is_calibrating_) {
        uint32_t current_time = esp_timer_get_time() / 1000;
        uint32_t elapsed = current_time - calibration_start_time_;
        *progress = (float)elapsed / calibration_duration_;
        
        if (elapsed >= calibration_duration_) {
            // 校准完成，计算偏置
            if (calibration_sample_count_ > 0) {
                for (int i = 0; i < 3; i++) {
                    calibration_.acc_bias[i] = calibration_acc_sum_[i] / calibration_sample_count_;
                    calibration_.gyro_bias[i] = calibration_gyro_sum_[i] / calibration_sample_count_;
                }
                calibration_.is_calibrated = true;
                calibration_.calibration_time = current_time;
            }
            
            is_calibrating_ = false;
            *is_calibrating = false;
            *progress = 1.0f;
            
            ESP_LOGI(TAG, "Calibration completed with %" PRIu32 " samples", calibration_sample_count_);
        }
    } else {
        *progress = 0.0f;
    }
    
    return QMI8658A_OK;
}

qmi8658a_error_t QMI8658A::ApplyCalibration(const qmi8658a_calibration_t* calibration) {
    if (!calibration) {
        return SetError(QMI8658A_ERROR_INVALID_PARAM);
    }
    
    calibration_ = *calibration;
    ESP_LOGI(TAG, "Applied calibration data");
    return QMI8658A_OK;
}

qmi8658a_error_t QMI8658A::GetCalibrationData(qmi8658a_calibration_t* calibration) {
    if (!calibration) {
        return SetError(QMI8658A_ERROR_INVALID_PARAM);
    }
    
    *calibration = calibration_;
    return QMI8658A_OK;
}

void QMI8658A::UpdateScaleFactors() {
    // 根据加速度计量程计算比例因子
    switch (config_.acc_range) {
        case QMI8658A_ACC_RANGE_2G:
            acc_scale_ = 2.0f / 32768.0f;
            break;
        case QMI8658A_ACC_RANGE_4G:
            acc_scale_ = 4.0f / 32768.0f;
            break;
        case QMI8658A_ACC_RANGE_8G:
            acc_scale_ = 8.0f / 32768.0f;
            break;
        case QMI8658A_ACC_RANGE_16G:
            acc_scale_ = 16.0f / 32768.0f;
            break;
        default:
            acc_scale_ = 2.0f / 32768.0f;
            break;
    }
    
    // 根据陀螺仪量程计算比例因子
    switch (config_.gyro_range) {
        case QMI8658A_GYRO_RANGE_16DPS:
            gyro_scale_ = 16.0f / 32768.0f;
            break;
        case QMI8658A_GYRO_RANGE_32DPS:
            gyro_scale_ = 32.0f / 32768.0f;
            break;
        case QMI8658A_GYRO_RANGE_64DPS:
            gyro_scale_ = 64.0f / 32768.0f;
            break;
        case QMI8658A_GYRO_RANGE_128DPS:
            gyro_scale_ = 128.0f / 32768.0f;
            break;
        case QMI8658A_GYRO_RANGE_256DPS:
            gyro_scale_ = 256.0f / 32768.0f;
            break;
        case QMI8658A_GYRO_RANGE_512DPS:
            gyro_scale_ = 512.0f / 32768.0f;
            break;
        case QMI8658A_GYRO_RANGE_1024DPS:
            gyro_scale_ = 1024.0f / 32768.0f;
            break;
        case QMI8658A_GYRO_RANGE_2048DPS:
            gyro_scale_ = 2048.0f / 32768.0f;
            break;
        default:
            gyro_scale_ = 256.0f / 32768.0f;
            break;
    }
}

qmi8658a_error_t QMI8658A::ApplyConfigurationChanges() {
    qmi8658a_error_t result;
    
    // 应用加速度计配置
    result = SetAccelConfig(config_.acc_range, config_.acc_odr);
    if (result != QMI8658A_OK) {
        ESP_LOGE(TAG, "Failed to configure accelerometer");
        state_ = QMI8658A_STATE_ERROR;
        return result;
    }
    
    // 应用陀螺仪配置
    result = SetGyroConfig(config_.gyro_range, config_.gyro_odr);
    if (result != QMI8658A_OK) {
        ESP_LOGE(TAG, "Failed to configure gyroscope");
        state_ = QMI8658A_STATE_ERROR;
        return result;
    }

    // 设置工作模式
    uint8_t mode_val = config_.mode;
    result = SetMode(static_cast<qmi8658a_mode_t>(mode_val));
    if (result != QMI8658A_OK) {
        ESP_LOGE(TAG, "Failed to set mode");
        state_ = QMI8658A_STATE_ERROR;
        return result;
    }
    
    // 更新比例因子
    UpdateScaleFactors();
    
    return QMI8658A_OK;
}

QMI8658A::~QMI8658A() {
    // 停止缓冲读取
    if (buffer_enabled_) {
        StopBufferedReading();
    }
    
    // 停止校准
    if (is_calibrating_) {
        is_calibrating_ = false;
    }
    
    // 清理缓冲区 - 只有在mutex已创建时才释放
    if (data_buffer_.mutex != nullptr) {
        vSemaphoreDelete(data_buffer_.mutex);
        data_buffer_.mutex = nullptr;
    }
    
    // 禁用FIFO
    if (fifo_enabled_) {
        DisableFIFO();
    }
    
    // 设置为禁用模式 - 只有在传感器已初始化时才调用
    if (state_ != QMI8658A_STATE_UNINITIALIZED) {
        SetMode(QMI8658A_MODE_DISABLE);
    }
    
    ESP_LOGI(TAG, "QMI8658A destructor completed");
}