#include "qmi8658a_test.h"
#include "qmi8658a.h"
#include <cstring>
#include <inttypes.h>

// ESP-IDF 头文件
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/i2c_master.h"
#define TAG "QMI8658A_TEST"

// 模拟I2C错误的测试类
class QMI8658A_TestHarness : public QMI8658A {
public:
    QMI8658A_TestHarness(i2c_master_bus_handle_t i2c_bus, uint8_t addr) 
        : QMI8658A(i2c_bus, addr), 
          simulate_i2c_error_(false),
          simulate_chip_id_error_(false),
          simulate_timeout_error_(false),
          error_injection_count_(0),
          max_error_injections_(0) {}

    // 错误注入控制
    void SetI2CErrorSimulation(bool enable, uint32_t max_errors = 1) {
        simulate_i2c_error_ = enable;
        max_error_injections_ = max_errors;
        error_injection_count_ = 0;
    }

    void SetChipIdErrorSimulation(bool enable) {
        simulate_chip_id_error_ = enable;
    }

    void SetTimeoutErrorSimulation(bool enable) {
        simulate_timeout_error_ = enable;
    }

    // 重写I2C方法以模拟错误
    esp_err_t WriteRegWithError(uint8_t reg, uint8_t value) {
        if (simulate_i2c_error_ && error_injection_count_ < max_error_injections_) {
            error_injection_count_++;
            ESP_LOGW(TAG, "Simulating I2C write error for reg 0x%02X (injection %u/%u)", 
                     reg, (unsigned)error_injection_count_, (unsigned)max_error_injections_);
            return ESP_FAIL;
        }
        return QMI8658A::WriteRegWithError(reg, value);
    }
    
    uint8_t ReadReg(uint8_t reg) {
        if (simulate_chip_id_error_ && reg == QMI8658A_WHO_AM_I) {
            ESP_LOGW(TAG, "Simulating chip ID error");
            return 0xFF; // 错误的芯片ID
        }
        
        if (simulate_timeout_error_ && reg == QMI8658A_STATUS0) {
            ESP_LOGW(TAG, "Simulating timeout (data not ready)");
            return 0x00; // 数据未准备好
        }
        
        return QMI8658A::ReadReg(reg);
    }

    // 获取错误注入统计
    uint32_t GetErrorInjectionCount() const {
        return error_injection_count_;
    }

private:
    bool simulate_i2c_error_;
    bool simulate_chip_id_error_;
    bool simulate_timeout_error_;
    uint32_t error_injection_count_;
    uint32_t max_error_injections_;
};

// 测试用例结构
struct TestCase {
    const char* name;
    bool (*test_func)(QMI8658A_TestHarness& sensor);
};

// 测试正常初始化
bool test_normal_initialization(QMI8658A_TestHarness& sensor) {
    ESP_LOGI(TAG, "Testing normal initialization...");
    
    qmi8658a_config_t config = {
        .acc_range = QMI8658A_ACC_RANGE_4G,
        .gyro_range = QMI8658A_GYRO_RANGE_512DPS,
        .acc_odr = QMI8658A_ODR_125HZ,
        .gyro_odr = QMI8658A_ODR_125HZ,
        .mode = QMI8658A_MODE_DUAL,
        .enable_interrupt = false,
        .interrupt_pin = 0,
        .auto_calibration = false
    };
    
    qmi8658a_error_t result = sensor.Initialize(&config);
    
    if (result == QMI8658A_OK) {
        ESP_LOGI(TAG, "✓ Normal initialization passed");
        return true;
    } else {
        ESP_LOGE(TAG, "✗ Normal initialization failed with error: %d", result);
        return false;
    }
}

// 测试I2C通信错误恢复
bool test_i2c_error_recovery(QMI8658A_TestHarness& sensor) {
    ESP_LOGI(TAG, "Testing I2C error recovery...");
    
    // 模拟前2次I2C写入失败，第3次成功
    sensor.SetI2CErrorSimulation(true, 2);
    
    qmi8658a_config_t config = {
        .acc_range = QMI8658A_ACC_RANGE_4G,
        .gyro_range = QMI8658A_GYRO_RANGE_512DPS,
        .acc_odr = QMI8658A_ODR_125HZ,
        .gyro_odr = QMI8658A_ODR_125HZ,
        .mode = QMI8658A_MODE_DUAL,
        .enable_interrupt = false,
        .interrupt_pin = 0,
        .auto_calibration = false
    };
    
    qmi8658a_error_t result = sensor.Initialize(&config);
    
    // 关闭错误模拟
    sensor.SetI2CErrorSimulation(false);
    
    if (result == QMI8658A_OK && sensor.GetErrorInjectionCount() == 2) {
        ESP_LOGI(TAG, "✓ I2C error recovery passed (recovered after %u errors)", 
                 (unsigned)sensor.GetErrorInjectionCount());
        return true;
    } else {
        ESP_LOGE(TAG, "✗ I2C error recovery failed. Result: %d, Errors injected: %u", 
                 result, (unsigned)sensor.GetErrorInjectionCount());
        return false;
    }
}

// 测试芯片ID错误检测
bool test_chip_id_error_detection(QMI8658A_TestHarness& sensor) {
    ESP_LOGI(TAG, "Testing chip ID error detection...");
    
    sensor.SetChipIdErrorSimulation(true);
    
    qmi8658a_config_t config = {
        .acc_range = QMI8658A_ACC_RANGE_4G,
        .gyro_range = QMI8658A_GYRO_RANGE_512DPS,
        .acc_odr = QMI8658A_ODR_125HZ,
        .gyro_odr = QMI8658A_ODR_125HZ,
        .mode = QMI8658A_MODE_DUAL,
        .enable_interrupt = false,
        .interrupt_pin = 0,
        .auto_calibration = false
    };
    
    qmi8658a_error_t result = sensor.Initialize(&config);
    
    sensor.SetChipIdErrorSimulation(false);
    
    if (result == QMI8658A_ERROR_CHIP_ID) {
        ESP_LOGI(TAG, "✓ Chip ID error detection passed");
        return true;
    } else {
        ESP_LOGE(TAG, "✗ Chip ID error detection failed. Expected: %d, Got: %d", 
                 QMI8658A_ERROR_CHIP_ID, result);
        return false;
    }
}

// 测试配置验证
bool test_configuration_validation(QMI8658A_TestHarness& sensor) {
    ESP_LOGI(TAG, "Testing configuration validation...");
    
    // 测试无效配置
    qmi8658a_config_t invalid_config = {
        .acc_range = static_cast<qmi8658a_acc_range_t>(0xFF), // 无效范围
        .gyro_range = QMI8658A_GYRO_RANGE_512DPS,
        .acc_odr = QMI8658A_ODR_125HZ,
        .gyro_odr = QMI8658A_ODR_125HZ,
        .mode = QMI8658A_MODE_DUAL,
        .enable_interrupt = false,
        .interrupt_pin = 0,
        .auto_calibration = false
    };
    
    qmi8658a_error_t result = sensor.Initialize(&invalid_config);
    
    if (result == QMI8658A_ERROR_INVALID_PARAM) {
        ESP_LOGI(TAG, "✓ Configuration validation passed");
        return true;
    } else {
        ESP_LOGE(TAG, "✗ Configuration validation failed. Expected: %d, Got: %d", 
                 QMI8658A_ERROR_INVALID_PARAM, result);
        return false;
    }
}

// 测试数据读取错误处理
bool test_data_read_error_handling(QMI8658A_TestHarness& sensor) {
    ESP_LOGI(TAG, "Testing data read error handling...");
    
    // 首先正常初始化
    qmi8658a_config_t config = {
        .acc_range = QMI8658A_ACC_RANGE_4G,
        .gyro_range = QMI8658A_GYRO_RANGE_512DPS,
        .acc_odr = QMI8658A_ODR_125HZ,
        .gyro_odr = QMI8658A_ODR_125HZ,
        .mode = QMI8658A_MODE_DUAL,
        .enable_interrupt = false,
        .interrupt_pin = 0,
        .auto_calibration = false
    };
    
    qmi8658a_error_t result = sensor.Initialize(&config);
    if (result != QMI8658A_OK) {
        ESP_LOGE(TAG, "✗ Failed to initialize sensor for data read test");
        return false;
    }
    
    // 模拟超时错误
    sensor.SetTimeoutErrorSimulation(true);
    
    qmi8658a_data_t data;
    result = sensor.ReadSensorData(&data);
    
    sensor.SetTimeoutErrorSimulation(false);
    
    if (result == QMI8658A_ERROR_TIMEOUT || result == QMI8658A_ERROR_DATA_NOT_READY) {
        ESP_LOGI(TAG, "✓ Data read error handling passed");
        return true;
    } else {
        ESP_LOGE(TAG, "✗ Data read error handling failed. Got: %d", result);
        return false;
    }
}

// 运行所有测试
void run_qmi8658a_robustness_tests(i2c_master_bus_handle_t i2c_bus) {
    ESP_LOGI(TAG, "=== QMI8658A Robustness Tests ===");
    
    QMI8658A_TestHarness sensor(i2c_bus, QMI8658A_I2C_ADDRESS);
    
    TestCase test_cases[] = {
        {"Normal Initialization", test_normal_initialization},
        {"I2C Error Recovery", test_i2c_error_recovery},
        {"Chip ID Error Detection", test_chip_id_error_detection},
        {"Configuration Validation", test_configuration_validation},
        {"Data Read Error Handling", test_data_read_error_handling}
    };
    
    uint32_t total_tests = sizeof(test_cases) / sizeof(TestCase);
    uint32_t passed_tests = 0;
    
    for (uint32_t i = 0; i < total_tests; i++) {
        ESP_LOGI(TAG, "\n--- Test %u/%u: %s ---", (unsigned)(i + 1), (unsigned)total_tests, test_cases[i].name);
        
        if (test_cases[i].test_func(sensor)) {
            passed_tests++;
        }
        
        // 测试间隔
        vTaskDelay(pdMS_TO_TICKS(100));
    }
    
    ESP_LOGI(TAG, "\n=== Test Results ===");
    ESP_LOGI(TAG, "Passed: %u/%u tests", (unsigned)passed_tests, (unsigned)total_tests);
    ESP_LOGI(TAG, "Success Rate: %.1f%%", (float)passed_tests / total_tests * 100.0f);
    
    if (passed_tests == total_tests) {
        ESP_LOGI(TAG, "🎉 All tests passed! QMI8658A robustness validation successful.");
    } else {
        ESP_LOGW(TAG, "⚠️  Some tests failed. Please review the error handling implementation.");
    }
}