#include "veml7700.h"
#include <cstring>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

#define TAG "VEML7700"

// ============================================================================
// 增益对应的系数表（用于Lux计算）
// ============================================================================
static const float gain_factors[] = {
    1.0f,   // GAIN_1   (x1)
    2.0f,   // GAIN_2   (x2)
    0.125f, // GAIN_D8  (x1/8)
    0.25f,  // GAIN_D4  (x1/4)
};

// 积分时间对应的系数表
// 索引通过 it 枚举值映射
static float GetItFactor(veml7700_it_t it) {
    switch (it) {
        case VEML7700_IT_25MS:  return 0.25f;
        case VEML7700_IT_50MS:  return 0.5f;
        case VEML7700_IT_100MS: return 1.0f;
        case VEML7700_IT_200MS: return 2.0f;
        case VEML7700_IT_400MS: return 4.0f;
        case VEML7700_IT_800MS: return 8.0f;
        default: return 1.0f;
    }
}

// 积分时间对应的等待时间（ms）
static uint32_t GetItDelayMs(veml7700_it_t it) {
    switch (it) {
        case VEML7700_IT_25MS:  return 25;
        case VEML7700_IT_50MS:  return 50;
        case VEML7700_IT_100MS: return 100;
        case VEML7700_IT_200MS: return 200;
        case VEML7700_IT_400MS: return 400;
        case VEML7700_IT_800MS: return 800;
        default: return 100;
    }
}

// ============================================================================
// 构造函数
// ============================================================================
VEML7700::VEML7700(i2c_master_bus_handle_t i2c_bus, uint8_t addr)
    : I2cDevice(i2c_bus, addr),
      gain_(VEML7700_GAIN_1),
      it_(VEML7700_IT_100MS),
      pers_(VEML7700_PERS_1),
      int_enable_(false),
      shutdown_(true),
      als_conf_(0x0001)  // 默认关机状态
{
}

// ============================================================================
// 16-bit 寄存器读写
// VEML7700 协议：写 = [cmd] + [data_low] + [data_high]
//               读 = [cmd] → [data_low] + [data_high]
// ============================================================================
esp_err_t VEML7700::WriteReg16(uint8_t cmd, uint16_t value) {
    uint8_t buffer[3] = {
        cmd,
        (uint8_t)(value & 0xFF),        // 低字节
        (uint8_t)((value >> 8) & 0xFF)  // 高字节
    };
    esp_err_t ret = i2c_master_transmit(i2c_device_, buffer, 3, 100);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "写寄存器 0x%02X 失败: %s", cmd, esp_err_to_name(ret));
    }
    return ret;
}

esp_err_t VEML7700::ReadReg16(uint8_t cmd, uint16_t* value) {
    uint8_t rx[2] = {0};
    esp_err_t ret = i2c_master_transmit_receive(i2c_device_, &cmd, 1, rx, 2, 100);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "读寄存器 0x%02X 失败: %s", cmd, esp_err_to_name(ret));
        *value = 0;
        return ret;
    }
    *value = (uint16_t)(rx[0] | (rx[1] << 8));
    return ESP_OK;
}

// ============================================================================
// 更新ALS配置寄存器
// ALS_CONF 位域：
//   [15:13] 保留
//   [12:11] ALS_GAIN
//   [10]    保留
//   [9:6]   ALS_IT
//   [5:4]   ALS_PERS
//   [3]     保留
//   [2]     ALS_INT_EN
//   [1]     保留
//   [0]     ALS_SD (1=关机, 0=运行)
// ============================================================================
esp_err_t VEML7700::UpdateALSConf() {
    als_conf_ = 0;
    als_conf_ |= ((uint16_t)gain_ & 0x03) << 11;
    als_conf_ |= ((uint16_t)it_ & 0x0F) << 6;
    als_conf_ |= ((uint16_t)pers_ & 0x03) << 4;
    als_conf_ |= int_enable_ ? (1 << 2) : 0;
    als_conf_ |= shutdown_ ? 1 : 0;

    return WriteReg16(VEML7700_REG_ALS_CONF, als_conf_);
}

// ============================================================================
// 初始化
// ============================================================================
esp_err_t VEML7700::Init() {
    ESP_LOGI(TAG, "初始化 VEML7700 环境光传感器...");

    // 先关机再配置（datasheet建议）
    shutdown_ = true;
    esp_err_t ret = UpdateALSConf();
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "VEML7700 初始化失败，I2C通信错误");
        return ret;
    }

    // 设置默认参数：增益x1，积分时间100ms
    gain_ = VEML7700_GAIN_1;
    it_ = VEML7700_IT_100MS;
    pers_ = VEML7700_PERS_1;
    int_enable_ = false;

    // 关闭省电模式
    ret = SetPowerSaving(VEML7700_PSM_1, false);
    if (ret != ESP_OK) return ret;

    // 唤醒传感器
    shutdown_ = false;
    ret = UpdateALSConf();
    if (ret != ESP_OK) return ret;

    // 等待首次采样完成
    vTaskDelay(pdMS_TO_TICKS(GetSampleDelayMs() + 10));

    ESP_LOGI(TAG, "VEML7700 初始化成功 (增益:x1, 积分时间:100ms)");
    return ESP_OK;
}

// ============================================================================
// 配置接口
// ============================================================================
esp_err_t VEML7700::SetGain(veml7700_gain_t gain) {
    gain_ = gain;
    return UpdateALSConf();
}

esp_err_t VEML7700::SetIntegrationTime(veml7700_it_t it) {
    it_ = it;
    return UpdateALSConf();
}

esp_err_t VEML7700::SetPersistence(veml7700_pers_t pers) {
    pers_ = pers;
    return UpdateALSConf();
}

esp_err_t VEML7700::SetInterruptEnable(bool enable) {
    int_enable_ = enable;
    return UpdateALSConf();
}

esp_err_t VEML7700::Shutdown(bool sd) {
    shutdown_ = sd;
    return UpdateALSConf();
}

esp_err_t VEML7700::SetPowerSaving(veml7700_psm_t mode, bool enable) {
    uint16_t psm_val = 0;
    psm_val |= ((uint16_t)mode & 0x03) << 1;
    psm_val |= enable ? 1 : 0;
    return WriteReg16(VEML7700_REG_PSM, psm_val);
}

esp_err_t VEML7700::SetThresholdHigh(uint16_t threshold) {
    return WriteReg16(VEML7700_REG_ALS_WH, threshold);
}

esp_err_t VEML7700::SetThresholdLow(uint16_t threshold) {
    return WriteReg16(VEML7700_REG_ALS_WL, threshold);
}

// ============================================================================
// 数据读取
// ============================================================================
esp_err_t VEML7700::ReadALSRaw(uint16_t* raw) {
    return ReadReg16(VEML7700_REG_ALS, raw);
}

esp_err_t VEML7700::ReadWhiteRaw(uint16_t* raw) {
    return ReadReg16(VEML7700_REG_WHITE, raw);
}

// ============================================================================
// 分辨率计算
// 基准分辨率：增益x1 + 积分时间100ms = 0.0576 lux/count
// 其他配置按比例缩放
// ============================================================================
float VEML7700::GetResolution() const {
    float base_resolution = 0.0576f;  // lux/count @ gain=1, it=100ms
    float g = gain_factors[gain_];
    float t = GetItFactor(it_);
    return base_resolution / (g * t);
}

// ============================================================================
// 非线性校正（Vishay应用笔记推荐的多项式补偿）
// 当 Lux > 1000 时，传感器响应偏离线性，需要校正
// ============================================================================
float VEML7700::CorrectLux(float lux) const {
    // Vishay官方多项式校正公式（Horner法）
    // lux_corrected = lux * (1.0023 + lux * (8.1488e-5 + lux * (-9.3924e-9 + lux * 6.0135e-13)))
    return lux * (1.0023f + lux * (8.1488e-5f + lux * (-9.3924e-9f + lux * 6.0135e-13f)));
}

esp_err_t VEML7700::ReadALSLux(float* lux) {
    uint16_t raw = 0;
    esp_err_t ret = ReadALSRaw(&raw);
    if (ret != ESP_OK) {
        *lux = 0;
        return ret;
    }
    float raw_lux = (float)raw * GetResolution();
    *lux = CorrectLux(raw_lux);
    return ESP_OK;
}

esp_err_t VEML7700::ReadWhiteLux(float* lux) {
    uint16_t raw = 0;
    esp_err_t ret = ReadWhiteRaw(&raw);
    if (ret != ESP_OK) {
        *lux = 0;
        return ret;
    }
    float raw_lux = (float)raw * GetResolution();
    *lux = CorrectLux(raw_lux);
    return ESP_OK;
}

esp_err_t VEML7700::ReadAll(veml7700_data_t* data) {
    esp_err_t ret = ReadALSRaw(&data->als_raw);
    if (ret != ESP_OK) return ret;

    ret = ReadWhiteRaw(&data->white_raw);
    if (ret != ESP_OK) return ret;

    float resolution = GetResolution();
    float als_raw_lux = (float)data->als_raw * resolution;
    float white_raw_lux = (float)data->white_raw * resolution;
    data->als_lux = CorrectLux(als_raw_lux);
    data->white_lux = CorrectLux(white_raw_lux);

    return ESP_OK;
}

// ============================================================================
// 中断状态读取
// ============================================================================
esp_err_t VEML7700::ReadInterruptStatus(bool* high_triggered, bool* low_triggered) {
    uint16_t status = 0;
    esp_err_t ret = ReadReg16(VEML7700_REG_ALS_INT, &status);
    if (ret != ESP_OK) return ret;

    *high_triggered = (status >> 14) & 0x01;
    *low_triggered = (status >> 15) & 0x01;
    return ESP_OK;
}

// ============================================================================
// 获取采样等待时间
// ============================================================================
uint32_t VEML7700::GetSampleDelayMs() const {
    return GetItDelayMs(it_);
}

// ============================================================================
// 自动量程实现
// 算法参考 Vishay 应用笔记和 tedyapo/arduino-VEML7700 驱动
//
// 策略：从高灵敏度（长积分+高增益）开始测量
//   - 如果计数值太高（>10000，接近饱和），降低积分时间
//   - 如果计数值太低（<200，精度不够），提高灵敏度
//   - 找到计数值在 200~10000 之间的最优参数组合
//
// 增益遍历顺序（灵敏度从高到低）: x2 → x1 → x(1/4) → x(1/8)
// 积分时间遍历顺序（从100ms开始往上探，再往下降）
// ============================================================================

// 增益搜索顺序：灵敏度从高到低
static const veml7700_gain_t kGainOrder[] = {
    VEML7700_GAIN_2, VEML7700_GAIN_1, VEML7700_GAIN_D4, VEML7700_GAIN_D8
};
static const int kGainOrderCount = sizeof(kGainOrder) / sizeof(kGainOrder[0]);

// 积分时间搜索顺序：从100ms开始向上到800ms
static const veml7700_it_t kItOrderUp[] = {
    VEML7700_IT_100MS, VEML7700_IT_200MS, VEML7700_IT_400MS, VEML7700_IT_800MS
};
static const int kItOrderUpCount = sizeof(kItOrderUp) / sizeof(kItOrderUp[0]);

// 积分时间搜索顺序：从100ms开始向下到25ms
static const veml7700_it_t kItOrderDown[] = {
    VEML7700_IT_100MS, VEML7700_IT_50MS, VEML7700_IT_25MS
};
static const int kItOrderDownCount = sizeof(kItOrderDown) / sizeof(kItOrderDown[0]);

// 自动量程计数阈值
static const uint16_t kAutoCountLow = 200;      // 低于此值精度不足
static const uint16_t kAutoCountHigh = 10000;    // 高于此值接近饱和

esp_err_t VEML7700::ApplyConfigAndWait(veml7700_gain_t gain, veml7700_it_t it) {
    // 先关机
    shutdown_ = true;
    gain_ = gain;
    it_ = it;
    esp_err_t ret = UpdateALSConf();
    if (ret != ESP_OK) return ret;

    // 开机
    shutdown_ = false;
    ret = UpdateALSConf();
    if (ret != ESP_OK) return ret;

    // 等待 2 倍积分时间确保新数据就绪（datasheet 建议）
    vTaskDelay(pdMS_TO_TICKS(GetItDelayMs(it) * 2 + 10));
    return ESP_OK;
}

esp_err_t VEML7700::AutoRangeMeasure(ReadRawFunc read_func, veml7700_auto_data_t* result) {
    uint16_t counts = 0;
    esp_err_t ret;

    // 阶段1：从中等灵敏度开始（100ms + x2增益），逐步提高积分时间
    for (int it_idx = 0; it_idx < kItOrderUpCount; it_idx++) {
        for (int g_idx = 0; g_idx < kGainOrderCount; g_idx++) {
            ret = ApplyConfigAndWait(kGainOrder[g_idx], kItOrderUp[it_idx]);
            if (ret != ESP_OK) return ret;

            ret = (this->*read_func)(&counts);
            if (ret != ESP_OK) return ret;

            // 计数值在合理范围内，直接返回
            if (counts >= kAutoCountLow && counts <= kAutoCountHigh) {
                goto done;
            }

            // 计数值太高，需要降低灵敏度
            if (counts > kAutoCountHigh) {
                // 尝试降低积分时间
                goto reduce_sensitivity;
            }

            // 计数值太低，尝试下一个更高的增益（循环继续）
        }
    }

    // 用最高灵敏度仍然太低，直接用当前结果（弱光环境）
    goto done;

reduce_sensitivity:
    // 阶段2：计数值过高，逐步缩短积分时间
    for (int it_idx = 1; it_idx < kItOrderDownCount; it_idx++) {
        ret = ApplyConfigAndWait(gain_, kItOrderDown[it_idx]);
        if (ret != ESP_OK) return ret;

        ret = (this->*read_func)(&counts);
        if (ret != ESP_OK) return ret;

        if (counts <= kAutoCountHigh) {
            goto done;
        }
    }

    // 最短积分时间 + 当前增益仍然太高，降低增益
    for (int g_idx = 0; g_idx < kGainOrderCount; g_idx++) {
        if (kGainOrder[g_idx] == gain_) continue;  // 跳过当前增益
        // 只尝试更低灵敏度的增益
        float current_factor = gain_factors[gain_];
        float try_factor = gain_factors[kGainOrder[g_idx]];
        if (try_factor >= current_factor) continue;

        ret = ApplyConfigAndWait(kGainOrder[g_idx], VEML7700_IT_25MS);
        if (ret != ESP_OK) return ret;

        ret = (this->*read_func)(&counts);
        if (ret != ESP_OK) return ret;

        if (counts <= kAutoCountHigh) {
            goto done;
        }
    }

    // 最低灵敏度仍然饱和，用当前结果（极强光环境）

done:
    result->raw = counts;
    result->gain = gain_;
    result->it = it_;

    float resolution = GetResolution();
    float raw_lux = (float)counts * resolution;
    result->lux = CorrectLux(raw_lux);

    ESP_LOGI(TAG, "自动量程完成: gain=%d, it=%d, raw=%u, lux=%.2f",
             gain_, it_, counts, result->lux);

    return ESP_OK;
}

esp_err_t VEML7700::ReadAutoALSLux(veml7700_auto_data_t* result) {
    return AutoRangeMeasure(&VEML7700::ReadALSRaw, result);
}

esp_err_t VEML7700::ReadAutoWhiteLux(veml7700_auto_data_t* result) {
    return AutoRangeMeasure(&VEML7700::ReadWhiteRaw, result);
}

esp_err_t VEML7700::ReadAutoAll(veml7700_auto_data_t* als_result, veml7700_auto_data_t* white_result) {
    // 先用ALS通道做自动量程找到最优参数
    esp_err_t ret = AutoRangeMeasure(&VEML7700::ReadALSRaw, als_result);
    if (ret != ESP_OK) return ret;

    // 用同一组参数直接读取White通道（不再重新调参，避免重复等待）
    uint16_t white_raw = 0;
    ret = ReadWhiteRaw(&white_raw);
    if (ret != ESP_OK) return ret;

    white_result->raw = white_raw;
    white_result->gain = gain_;
    white_result->it = it_;
    float resolution = GetResolution();
    float raw_lux = (float)white_raw * resolution;
    white_result->lux = CorrectLux(raw_lux);

    return ESP_OK;
}