#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(QMI8658A_ACC_RANGE_4G, QMI8658A_ODR_125HZ);
    if (result != QMI8658A_OK) {
        ESP_LOGE(TAG, "Failed to configure accelerometer");
        state_ = QMI8658A_STATE_ERROR;
        return result;
    }
    
    // 配置陀螺仪 - 使用更保守的设置
    result = SetGyroConfig(QMI8658A_GYRO_RANGE_256DPS, QMI8658A_ODR_125HZ);
    if (result != QMI8658A_OK) {
        ESP_LOGE(TAG, "Failed to configure gyroscope");
        state_ = QMI8658A_STATE_ERROR;
        return result;
    }
    
    // 配置CTRL1 - 使能数据就绪/中断相关功能以确保STATUS0有效
    result = WriteRegWithVerification(QMI8658A_CTRL1, 0x60);
    if (result != QMI8658A_OK) {
        ESP_LOGW(TAG, "Failed to configure CTRL1 register");
    } else {
        VerifyRegisterValue(QMI8658A_CTRL1, 0x60, "CTRL1");
    }
    
    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;
    }
    
    // 添加额外的寄存器配置
    // 配置CTRL5寄存器 - 设置加速度计和陀螺仪的带宽滤波
    result = WriteRegWithVerification(QMI8658A_CTRL5, 0x35);
    if (result != QMI8658A_OK) {
        ESP_LOGW(TAG, "Failed to configure CTRL5 register");
    }
    else {
        VerifyRegisterValue(QMI8658A_CTRL5, 0x35, "CTRL5");
    }
    
    // 配置CTRL6寄存器 - 设置陀螺仪的LPF
    result = WriteRegWithVerification(QMI8658A_CTRL6, 0x00);
    if (result != QMI8658A_OK) {
        ESP_LOGW(TAG, "Failed to configure CTRL6 register");
    }
    
    // 等待传感器稳定
    vTaskDelay(pdMS_TO_TICKS(100)); // 增加等待时间到100ms
    
    // 验证关键寄存器配置
    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;
    }
    
    uint8_t expected_ctrl7 = (mode_val == QMI8658A_MODE_DISABLE) ? 0x00 : (uint8_t)(0x80 | mode_val);
    result = VerifyRegisterValue(QMI8658A_CTRL7, expected_ctrl7, "CTRL7 (Mode)");
    if (result != QMI8658A_OK) {
        state_ = QMI8658A_STATE_ERROR;
        return result;
    }
    
    // 诊断寄存器状态
    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 ctrl5 = ReadReg(QMI8658A_CTRL5);
    uint8_t ctrl6 = ReadReg(QMI8658A_CTRL6);
    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, CTRL5: 0x%02X, CTRL6: 0x%02X, CTRL7: 0x%02X", 
             ctrl1, ctrl2, ctrl3, ctrl5, ctrl6, ctrl7);
    ESP_LOGI(TAG, "STATUS0: 0x%02X, STATUS1: 0x%02X", status0, status1);
    ESP_LOGI(TAG, "=====================================");
    
    // 等待数据就绪 - 只进行一次等待
    if (config_.mode != QMI8658A_MODE_DISABLE) {
        ESP_LOGI(TAG, "Waiting for sensor data to be 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;
    UpdateScaleFactors();
    ESP_LOGI(TAG, "QMI8658A initialization completed successfully");
    DumpRegisters();
    
    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_RESET, 0xB0);
    if (result != QMI8658A_OK) {
        ESP_LOGE(TAG, "Failed to perform soft reset");
        return result;
    }
    
    // 等待复位完成 - 大幅增加等待时间以确保传感器完全稳定
    vTaskDelay(pdMS_TO_TICKS(200)); // 增加到200ms等待时间
    
    // 验证复位是否成功 - 检查芯片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);
    
    uint8_t value = 0x00;
    switch (mode) {
        case QMI8658A_MODE_DISABLE: value = 0x00; break;
        case QMI8658A_MODE_ACC_ONLY: value = 0x81; break;
        case QMI8658A_MODE_GYRO_ONLY: value = 0x82; break;
        case QMI8658A_MODE_DUAL: value = 0x83; break;
    }
    qmi8658a_error_t result = WriteRegWithVerification(QMI8658A_CTRL7, value, 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", value);
    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;
    UpdateScaleFactors();
    
    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;
    UpdateScaleFactors();
    
    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);
    }
    
    uint8_t buf[6];
    ReadRegs(QMI8658A_AX_L, buf, 6);
    int16_t raw_x = (int16_t)((buf[1] << 8) | buf[0]);
    int16_t raw_y = (int16_t)((buf[3] << 8) | buf[2]);
    int16_t raw_z = (int16_t)((buf[5] << 8) | buf[4]);
    
    // 应用缩放和校准
    float ax = raw_x * acc_scale_;
    float ay = raw_y * acc_scale_;
    float az = raw_z * acc_scale_;
    if (calibration_.is_calibrated) {
        float bx = calibration_.acc_bias[0];
        float by = calibration_.acc_bias[1];
        float bz = calibration_.acc_bias[2];
        bool valid_bias = fabs(bx) < 0.5f && fabs(by) < 0.5f && fabs(bz) < 0.5f;
        if (valid_bias) {
            ax -= bx;
            ay -= by;
            az -= bz;
        }
    }
    *acc_x = ax;
    *acc_y = ay;
    *acc_z = az;
    
    // 数据合理性检查 - 加速度值不应超过设置的量程
    float max_acc = 4.0f; // 假设使用4G量程
    if (fabs(*acc_x) > max_acc * 1.1f || fabs(*acc_y) > max_acc * 1.1f || fabs(*acc_z) > max_acc * 1.1f) {
        ESP_LOGE(TAG, "Invalid accelerometer readings: [%.3f, %.3f, %.3f] g", *acc_x, *acc_y, *acc_z);
        return SetError(QMI8658A_ERROR_DATA_NOT_READY);
    }
    
    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);
    }
    
    uint8_t buf[6];
    ReadRegs(QMI8658A_GX_L, buf, 6);
    int16_t raw_x = (int16_t)((buf[1] << 8) | buf[0]);
    int16_t raw_y = (int16_t)((buf[3] << 8) | buf[2]);
    int16_t raw_z = (int16_t)((buf[5] << 8) | buf[4]);
    
    float gx = raw_x * gyro_scale_;
    float gy = raw_y * gyro_scale_;
    float gz = raw_z * gyro_scale_;
    if (calibration_.is_calibrated) {
        float bx = calibration_.gyro_bias[0];
        float by = calibration_.gyro_bias[1];
        float bz = calibration_.gyro_bias[2];
        bool valid_bias = fabs(bx) < 50.0f && fabs(by) < 50.0f && fabs(bz) < 50.0f;
        if (valid_bias) {
            gx -= bx;
            gy -= by;
            gz -= bz;
        }
    }
    *gyro_x = gx;
    *gyro_y = gy;
    *gyro_z = gz;
    
    float max_gyro = 400.0f;
    if (fabs(*gyro_x) > max_gyro || fabs(*gyro_y) > max_gyro || fabs(*gyro_z) > max_gyro) {
        ESP_LOGW(TAG, "Gyro readings out of expected range: [%.3f, %.3f, %.3f] dps", *gyro_x, *gyro_y, *gyro_z);
        // 不再直接返回错误，而是继续处理但记录警告
    }
    
    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);
    }
    
    uint8_t tl = ReadReg(QMI8658A_TEMP_L);
    uint8_t th = ReadReg(QMI8658A_TEMP_H);
    *temperature = (float)th + ((float)tl / 256.0f);
    
    if (*temperature < -40.0f || *temperature > 125.0f) {
        ESP_LOGW(TAG, "Temperature reading out of expected range: %.2f°C", *temperature);
        *temperature = 25.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 (data_buffer_.mutex == NULL) {
        qmi8658a_error_t r = InitializeBuffer();
        if (r != QMI8658A_OK) {
            return r;
        }
    }
    
    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);
    }
    
    // 增加重试机制
    const int max_retries = 3;
    for (int retry = 0; retry < max_retries; retry++) {
        // 检查数据是否就绪
        if (!IsDataReady()) {
            // 优化警告日志输出：降低级别并减少频率
            static int data_not_ready_count = 0;
            static int last_log_time = 0;
            int current_time = esp_timer_get_time();
            
            // 每10次警告或每1秒才输出一次日志，降低日志级别为DEBUG
            if (data_not_ready_count % 10 == 0 || (current_time - last_log_time) > 1000000) {
                ESP_LOGD(TAG, "Sensor data not ready (计数: %d), STATUS0: 0x%02X", 
                         data_not_ready_count, ReadReg(QMI8658A_STATUS0));
                last_log_time = current_time;
            }
            data_not_ready_count++;
            
            // 添加小延迟后重试
            if (retry < max_retries - 1) {
                vTaskDelay(pdMS_TO_TICKS(5));
                continue;
            }
            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;
        bool data_valid = true;
        
        // 读取加速度数据
        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);
                data_valid = false;
            } else {
                static float prev_acc_x = 0, prev_acc_y = 0, prev_acc_z = 0;
                static bool acc_initialized = false;
                if (!acc_initialized) {
                    prev_acc_x = data->acc_x;
                    prev_acc_y = data->acc_y;
                    prev_acc_z = data->acc_z;
                    data->acc_x = prev_acc_x; data->acc_y = prev_acc_y; data->acc_z = prev_acc_z;
                    acc_initialized = true;
                } else {
                    const float alpha = 0.6f;
                    float filtered_x = alpha * prev_acc_x + (1 - alpha) * data->acc_x;
                    float filtered_y = alpha * prev_acc_y + (1 - alpha) * data->acc_y;
                    float filtered_z = alpha * prev_acc_z + (1 - alpha) * data->acc_z;
                    prev_acc_x = filtered_x; prev_acc_y = filtered_y; prev_acc_z = filtered_z;
                    data->acc_x = filtered_x; data->acc_y = filtered_y; data->acc_z = filtered_z;
                }
                if (fabs(data->acc_x) < 0.03f) data->acc_x = 0.0f;
                if (fabs(data->acc_y) < 0.03f) data->acc_y = 0.0f;
                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);
                // 不再因为陀螺仪读取错误而标记整个数据无效，而是继续处理
            } else {
                static float prev_gyro_x = 0, prev_gyro_y = 0, prev_gyro_z = 0;
                static bool gyro_initialized = false;
                if (!gyro_initialized) {
                    prev_gyro_x = data->gyro_x;
                    prev_gyro_y = data->gyro_y;
                    prev_gyro_z = data->gyro_z;
                    data->gyro_x = prev_gyro_x; data->gyro_y = prev_gyro_y; data->gyro_z = prev_gyro_z;
                    gyro_initialized = true;
                } else {
                    const float alpha = 0.5f;
                    float filtered_x = alpha * prev_gyro_x + (1 - alpha) * data->gyro_x;
                    float filtered_y = alpha * prev_gyro_y + (1 - alpha) * data->gyro_y;
                    float filtered_z = alpha * prev_gyro_z + (1 - alpha) * data->gyro_z;
                    prev_gyro_x = filtered_x; prev_gyro_y = filtered_y; prev_gyro_z = filtered_z;
                    data->gyro_x = filtered_x; data->gyro_y = filtered_y; data->gyro_z = filtered_z;
                }
                if (fabs(data->gyro_x) < 0.8f) data->gyro_x = 0.0f;
                if (fabs(data->gyro_y) < 0.8f) data->gyro_y = 0.0f;
                if (fabs(data->gyro_z) < 0.8f) data->gyro_z = 0.0f;
                ESP_LOGV(TAG, "Gyro data after filtering: [%.3f, %.3f, %.3f] dps", 
                         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);
            data_valid = false;
        } else {
            ESP_LOGD(TAG, "Temperature: %.2f°C", data->temperature);
        }
        
        // 如果数据有效，完成读取
        if (data_valid) {
            data->valid = true;
            ESP_LOGD(TAG, "Successfully read sensor data (retry: %d)", retry);
            return QMI8658A_OK;
        }
        
        // 如果数据无效且未到最大重试次数，等待后重试
        if (retry < max_retries - 1) {
            ESP_LOGW(TAG, "Invalid sensor data, retrying... (%d/%d)", retry + 1, max_retries);
            vTaskDelay(pdMS_TO_TICKS(10));
        }
    }
    
    // 所有重试均失败
    ESP_LOGE(TAG, "Failed to read valid sensor data after %d retries", max_retries);
    return SetError(QMI8658A_ERROR_DATA_NOT_READY);
}

bool QMI8658A::IsDataReady() {
    if (state_ != QMI8658A_STATE_READY) {
        return false;
    }
    
    uint8_t status = ReadReg(QMI8658A_STATUS0);
    bool acc_ready = (status & 0x01) != 0;
    bool gyro_ready = (status & 0x02) != 0;
    switch (config_.mode) {
        case QMI8658A_MODE_ACC_ONLY:
            return acc_ready;
        case QMI8658A_MODE_GYRO_ONLY:
            return gyro_ready;
        case QMI8658A_MODE_DUAL:
            // 任一就绪即可开始读取，读取本身使用突发读保证快照一致性
            return acc_ready || gyro_ready;
        default:
            return false;
    }
}

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

void QMI8658A::DumpRegisters() {
    uint8_t chip = ReadReg(QMI8658A_WHO_AM_I);
    uint8_t rev = ReadReg(QMI8658A_REVISION_ID);
    uint8_t c1 = ReadReg(QMI8658A_CTRL1);
    uint8_t c2 = ReadReg(QMI8658A_CTRL2);
    uint8_t c3 = ReadReg(QMI8658A_CTRL3);
    uint8_t c4 = ReadReg(QMI8658A_CTRL4);
    uint8_t c5 = ReadReg(QMI8658A_CTRL5);
    uint8_t c6 = ReadReg(QMI8658A_CTRL6);
    uint8_t c7 = ReadReg(QMI8658A_CTRL7);
    uint8_t c8 = ReadReg(QMI8658A_CTRL8);
    uint8_t c9 = ReadReg(QMI8658A_CTRL9);
    uint8_t s0 = ReadReg(QMI8658A_STATUS0);
    uint8_t s1 = ReadReg(QMI8658A_STATUS1);
    ESP_LOGI(TAG, "ID:0x%02X REV:0x%02X", chip, rev);
    ESP_LOGI(TAG, "C1:0x%02X C2:0x%02X C3:0x%02X C4:0x%02X C5:0x%02X C6:0x%02X C7:0x%02X C8:0x%02X C9:0x%02X", c1,c2,c3,c4,c5,c6,c7,c8,c9);
    ESP_LOGI(TAG, "S0:0x%02X S1:0x%02X", s0, s1);
}

void QMI8658A::RunBaselineDiagnostics(uint16_t samples, uint16_t interval_ms) {
    if (state_ != QMI8658A_STATE_READY) {
        ESP_LOGE(TAG, "Sensor not ready for diagnostics");
        return;
    }
    double ax_sum = 0, ay_sum = 0, az_sum = 0;
    double gx_sum = 0, gy_sum = 0, gz_sum = 0;
    double ax_sq = 0, ay_sq = 0, az_sq = 0;
    double gx_sq = 0, gy_sq = 0, gz_sq = 0;
    uint16_t ok = 0;
    for (uint16_t i = 0; i < samples; i++) {
        qmi8658a_data_t d;
        qmi8658a_error_t r = ReadSensorData(&d);
        if (r == QMI8658A_OK && d.valid) {
            ax_sum += d.acc_x; ay_sum += d.acc_y; az_sum += d.acc_z;
            gx_sum += d.gyro_x; gy_sum += d.gyro_y; gz_sum += d.gyro_z;
            ax_sq += d.acc_x * d.acc_x; ay_sq += d.acc_y * d.acc_y; az_sq += d.acc_z * d.acc_z;
            gx_sq += d.gyro_x * d.gyro_x; gy_sq += d.gyro_y * d.gyro_y; gz_sq += d.gyro_z * d.gyro_z;
            ok++;
        }
        vTaskDelay(pdMS_TO_TICKS(interval_ms));
    }
    if (ok == 0) {
        ESP_LOGE(TAG, "No valid samples for diagnostics");
        return;
    }
    double ax_mean = ax_sum / ok, ay_mean = ay_sum / ok, az_mean = az_sum / ok;
    double gx_mean = gx_sum / ok, gy_mean = gy_sum / ok, gz_mean = gz_sum / ok;
    double ax_std = sqrt(ax_sq / ok - ax_mean * ax_mean);
    double ay_std = sqrt(ay_sq / ok - ay_mean * ay_mean);
    double az_std = sqrt(az_sq / ok - az_mean * az_mean);
    double gx_std = sqrt(gx_sq / ok - gx_mean * gx_mean);
    double gy_std = sqrt(gy_sq / ok - gy_mean * gy_mean);
    double gz_std = sqrt(gz_sq / ok - gz_mean * gz_mean);
    double acc_mag = sqrt(ax_mean * ax_mean + ay_mean * ay_mean + az_mean * az_mean);
    double acc_bias = fabs(acc_mag - 1.0);
    ESP_LOGI(TAG, "Baseline accel mean: [%.4f, %.4f, %.4f] g", ax_mean, ay_mean, az_mean);
    ESP_LOGI(TAG, "Baseline accel std:  [%.4f, %.4f, %.4f] g", ax_std, ay_std, az_std);
    ESP_LOGI(TAG, "Accel |mean|-1g: %.5f g", acc_bias);
    ESP_LOGI(TAG, "Baseline gyro mean:  [%.4f, %.4f, %.4f] dps", gx_mean, gy_mean, gz_mean);
    ESP_LOGI(TAG, "Baseline gyro std:   [%.4f, %.4f, %.4f] dps", gx_std, gy_std, gz_std);
}
// 新增：带验证的寄存器写入函数
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);
    
    // 分阶段检查：先检查任一传感器就绪，再检查同时就绪
    bool any_ready = false;
    
    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) {
            if (!any_ready) {
                ESP_LOGI(TAG, "At least one sensor ready after %lu ms (STATUS0: 0x%02X)", elapsed_time, status0);
                any_ready = true;
            }
            
            // 如果任一传感器就绪且已等待至少100ms，就返回成功
            // 这解决了某些情况下一个传感器就绪另一个未就绪的问题
            if (any_ready && elapsed_time > 100) {
                ESP_LOGI(TAG, "At least one sensor ready, proceeding after %lu ms (STATUS0: 0x%02X)", elapsed_time, status0);
                return QMI8658A_OK;
            }
        }
        
        // 根据是否已检测到传感器就绪调整等待时间
        uint8_t delay_time = any_ready ? 5 : 10; // 已检测到就绪则减少等待时间
        vTaskDelay(pdMS_TO_TICKS(delay_time));
        elapsed_time = (esp_timer_get_time() / 1000) - start_time;
    }
    
    ESP_LOGI(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) {
                float mean_ax = calibration_acc_sum_[0] / calibration_sample_count_;
                float mean_ay = calibration_acc_sum_[1] / calibration_sample_count_;
                float mean_az = calibration_acc_sum_[2] / calibration_sample_count_;
                float mean_gx = calibration_gyro_sum_[0] / calibration_sample_count_;
                float mean_gy = calibration_gyro_sum_[1] / calibration_sample_count_;
                float mean_gz = calibration_gyro_sum_[2] / calibration_sample_count_;
                calibration_.acc_bias[0] = mean_ax;
                calibration_.acc_bias[1] = mean_ay;
                calibration_.acc_bias[2] = mean_az - 1.0f;
                calibration_.gyro_bias[0] = mean_gx;
                calibration_.gyro_bias[1] = mean_gy;
                calibration_.gyro_bias[2] = mean_gz;
                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");
}