/**
 * @file    VL53L4CD_Driver.c
 * @brief   VL53L4CD 测距传感器驱动实现
 *
 * 初始化配置参考 ST VL53L4CD ULD / Pololu Arduino 库，
 * 使用 16 位寄存器地址，底层走项目现有硬件 I2C0。
 */
#include "VL53L4CD_Driver.h"
#include "i2c.h"
#include "systick.h"

/* 当前传感器 I2C 地址，默认 0x29 */
static uint8_t g_vl53l4cd_addr = VL53L4CD_I2C_ADDR;

/* VL53L4CD 默认配置，0x30..0x87 共 88 字节，来自 ST ULD */
static const uint8_t g_vl53l4cd_default_config[88] = {
    0x11, 0x02, 0x00, 0x02, 0x08, 0x00, 0x08, 0x10,
    0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0xFF, 0x00,
    0x0F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x20, 0x0B,
    0x00, 0x00, 0x02, 0x14, 0x21, 0x00, 0x00, 0x05,
    0x00, 0x00, 0x00, 0x00, 0xC8, 0x00, 0x00, 0x38,
    0xFF, 0x01, 0x00, 0x08, 0x00, 0x00, 0x01, 0xCC,
    0x07, 0x01, 0xF1, 0x05, 0x00, 0xA0, 0x00, 0x80,
    0x08, 0x38, 0x00, 0x00, 0x00, 0x00, 0x0F, 0x89,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
    0x07, 0x05, 0x06, 0x06, 0x00, 0x00, 0x02, 0xC7,
    0xFF, 0x9B, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00
};

/**
 * @brief   连续写多个寄存器字节
 * @param[in]  reg: 起始 16 位寄存器地址
 * @param[in]  data: 数据指针
 * @param[in]  length: 字节数
 * @retval   true: 成功; false: 失败
 */
static bool vl53l4cd_write_regs(uint16_t reg, const uint8_t *data, uint8_t length)
{
    if (data == NULL || length == 0U) {
        return false;
    }
    return (i2c_write(g_vl53l4cd_addr, reg, (uint8_t *)data, length) == I2C_RESULT_SUCCESS);
}

bool vl53l4cd_read_reg8(uint16_t reg, uint8_t *value)
{
    if (value == NULL) {
        return false;
    }
    return (i2c_read(g_vl53l4cd_addr, reg, value, 1U) == I2C_RESULT_SUCCESS);
}

bool vl53l4cd_read_reg16(uint16_t reg, uint16_t *value)
{
    uint8_t buf[2];

    if (value == NULL) {
        return false;
    }
    if (i2c_read(g_vl53l4cd_addr, reg, buf, 2U) != I2C_RESULT_SUCCESS) {
        return false;
    }
    *value = (uint16_t)(((uint16_t)buf[0] << 8) | buf[1]);
    return true;
}

bool vl53l4cd_write_reg8(uint16_t reg, uint8_t value)
{
    return vl53l4cd_write_regs(reg, &value, 1U);
}

bool vl53l4cd_write_reg16(uint16_t reg, uint16_t value)
{
    uint8_t buf[2];

    buf[0] = (uint8_t)(value >> 8);
    buf[1] = (uint8_t)(value & 0xFF);
    return vl53l4cd_write_regs(reg, buf, 2U);
}

bool vl53l4cd_data_ready(void)
{
    uint8_t status = 0U;

    if (!vl53l4cd_read_reg8(VL53L4CD_REG_GPIO_TIO_HV_STATUS, &status)) {
        return false;
    }
    /* 默认中断极性配置下，bit0=0 表示新结果就绪 */
    return ((status & 0x01U) == 0U);
}

bool vl53l4cd_clear_interrupt(void)
{
    return vl53l4cd_write_reg8(VL53L4CD_REG_SYSTEM_INTERRUPT_CLEAR, 0x01U);
}

/**
 * @brief   等待新的测距结果就绪
 * @param[in]  timeout_ms: 超时时间，单位 ms
 * @retval   true: 结果就绪; false: 超时或读取失败
 */
static bool vl53l4cd_wait_for_data_ready(uint32_t timeout_ms)
{
    uint32_t elapsed_ms = 0U;

    while (elapsed_ms < timeout_ms) {
        if (vl53l4cd_data_ready()) {
            return true;
        }
        delay_ms(1);
        elapsed_ms++;
    }

    return false;
}

bool vl53l4cd_get_result(vl53l4cd_result_t *result)
{
    static const uint8_t status_rtn[24] = {
        255, 255, 255, 5, 2, 4, 1, 7, 3, 0,
        255, 255, 9, 13, 255, 255, 255, 255, 10, 6,
        255, 255, 11, 12
    };
    uint8_t buf[VL53L4CD_RESULT_BLOCK_SIZE];
    uint8_t raw_status;

    if (result == NULL) {
        return false;
    }
    if (i2c_read(g_vl53l4cd_addr, VL53L4CD_REG_RESULT_RANGE_STATUS,
                 buf, VL53L4CD_RESULT_BLOCK_SIZE) != I2C_RESULT_SUCCESS) {
        return false;
    }

    raw_status = buf[0] & 0x1FU;
    result->range_status = (raw_status < 24U) ? status_rtn[raw_status]
                                              : (uint8_t)VL53L4CD_STATUS_UNKNOWN;
    result->number_of_spad = buf[3];
    result->signal_rate_kcps = ((uint32_t)buf[5] << 8 | buf[6]) * 8U;
    result->ambient_rate_kcps = ((uint32_t)buf[7] << 8 | buf[8]) * 8U;
    result->sigma_mm = ((uint16_t)buf[9] << 8 | buf[10]) / 4U;
    result->distance_mm = (uint16_t)(((uint16_t)buf[13] << 8) | buf[14]);

    if (result->number_of_spad != 0U) {
        result->signal_per_spad_kcps = result->signal_rate_kcps / result->number_of_spad;
        result->ambient_per_spad_kcps = result->ambient_rate_kcps / result->number_of_spad;
    } else {
        result->signal_per_spad_kcps = 0U;
        result->ambient_per_spad_kcps = 0U;
    }

    return true;
}

bool vl53l4cd_set_range_timing(uint8_t timing_budget_ms, uint32_t inter_measurement_ms)
{
    uint16_t osc_frequency = 0U;
    uint32_t timing_budget_us;
    uint32_t macro_period_us;
    uint32_t tmp;
    uint16_t ls_byte;
    uint8_t ms_byte;

    if (timing_budget_ms < 10U || timing_budget_ms > 200U) {
        return false;
    }
    if (inter_measurement_ms != 0U) {
        /* 为节省 flash，本版本只支持连续模式 */
        return false;
    }
    if (!vl53l4cd_read_reg16(0x0006U, &osc_frequency) || osc_frequency == 0U) {
        return false;
    }

    timing_budget_us = (uint32_t)timing_budget_ms * 1000U;
    macro_period_us = ((uint32_t)2304U * (0x40000000U / osc_frequency)) >> 6U;

    /* 连续模式：测量周期写 0 */
    {
        uint8_t zero[4] = {0U, 0U, 0U, 0U};
        if (!vl53l4cd_write_regs(VL53L4CD_REG_INTERMEASUREMENT_MS, zero, 4U)) {
            return false;
        }
    }
    timing_budget_us -= 2500U;

    timing_budget_us <<= 12U;

    /* RANGE_CONFIG_A: 基于 16 的宏周期 */
    tmp = (macro_period_us * 16U) >> 6U;
    ls_byte = (uint16_t)(((timing_budget_us + (tmp >> 1U)) / tmp) - 1U);
    ms_byte = 0U;
    while (ls_byte > 0xFFU) {
        ls_byte >>= 1U;
        ms_byte++;
    }
    if (!vl53l4cd_write_reg16(VL53L4CD_REG_RANGE_CONFIG_A,
                              (uint16_t)((uint16_t)ms_byte << 8 | ls_byte))) {
        return false;
    }

    /* RANGE_CONFIG_B: 基于 12 的宏周期 */
    tmp = (macro_period_us * 12U) >> 6U;
    ls_byte = (uint16_t)(((timing_budget_us + (tmp >> 1U)) / tmp) - 1U);
    ms_byte = 0U;
    while (ls_byte > 0xFFU) {
        ls_byte >>= 1U;
        ms_byte++;
    }
    return vl53l4cd_write_reg16(VL53L4CD_REG_RANGE_CONFIG_B,
                                (uint16_t)((uint16_t)ms_byte << 8 | ls_byte));
}

bool vl53l4cd_set_offset(int16_t offset_mm)
{
    uint16_t reg_value;

    reg_value = (uint16_t)((int16_t)offset_mm * 4);

    return vl53l4cd_write_reg16(VL53L4CD_REG_RANGE_OFFSET_MM, reg_value) &&
           vl53l4cd_write_reg16(VL53L4CD_REG_INNER_OFFSET_MM, 0U) &&
           vl53l4cd_write_reg16(VL53L4CD_REG_OUTER_OFFSET_MM, 0U);
}

bool vl53l4cd_get_offset(int16_t *offset_mm)
{
    uint16_t reg_value;
    int16_t raw;

    if (offset_mm == NULL) {
        return false;
    }
    if (!vl53l4cd_read_reg16(VL53L4CD_REG_RANGE_OFFSET_MM, &reg_value)) {
        return false;
    }

    raw = (int16_t)reg_value;
    *offset_mm = (int16_t)(raw / 4);
    return true;
}

bool vl53l4cd_calibrate_offset(uint16_t target_distance_mm, uint16_t num_samples,
                               int16_t *measured_offset_mm)
{
    vl53l4cd_result_t result;
    int32_t distance_sum = 0;
    uint16_t valid_samples = 0;
    uint16_t i;
    int16_t average_distance;
    int16_t offset;

    if (measured_offset_mm == NULL ||
        target_distance_mm < 10U || target_distance_mm > 1000U ||
        num_samples < 5U || num_samples > 255U) {
        return false;
    }

    if (!vl53l4cd_stop_ranging() ||
        !vl53l4cd_set_offset(0) ||
        !vl53l4cd_start_ranging()) {
        return false;
    }

    /* 热机 10 帧，丢弃 */
    for (i = 0U; i < 10U; i++) {
        if (!vl53l4cd_wait_for_data_ready(VL53L4CD_DATA_TIMEOUT_MS)) {
            return false;
        }
        (void)vl53l4cd_clear_interrupt();
        (void)vl53l4cd_get_result(&result);
    }

    /* 统计有效测距样本 */
    for (i = 0U; i < num_samples; i++) {
        if (!vl53l4cd_wait_for_data_ready(VL53L4CD_DATA_TIMEOUT_MS)) {
            return false;
        }
        (void)vl53l4cd_clear_interrupt();
        if (vl53l4cd_get_result(&result) &&
            result.range_status == VL53L4CD_STATUS_RANGE_VALID) {
            distance_sum += (int32_t)result.distance_mm;
            valid_samples++;
        }
    }

    (void)vl53l4cd_stop_ranging();

    if (valid_samples == 0U) {
        return false;
    }

    average_distance = (int16_t)(distance_sum / (int32_t)valid_samples);
    offset = (int16_t)((int16_t)target_distance_mm - average_distance);

    if (!vl53l4cd_set_offset(offset)) {
        return false;
    }

    *measured_offset_mm = offset;
    return true;
}

bool vl53l4cd_set_xtalk(uint16_t xtalk_kcps)
{
    /* ST 固件上限：127 kcps（65536/512），寄存器值单位为 1/512 kcps */
    if (xtalk_kcps > 127U) {
        return false;
    }
    if (!vl53l4cd_write_reg16(VL53L4CD_REG_XTALK_X_PLANE_GRADIENT_KCPS, 0U) ||
        !vl53l4cd_write_reg16(VL53L4CD_REG_XTALK_Y_PLANE_GRADIENT_KCPS, 0U)) {
        return false;
    }
    return vl53l4cd_write_reg16(VL53L4CD_REG_XTALK_PLANE_OFFSET_KCPS,
                                (uint16_t)(xtalk_kcps << 9));
}

bool vl53l4cd_get_xtalk(uint16_t *xtalk_kcps)
{
    uint16_t reg_value;

    if (xtalk_kcps == NULL) {
        return false;
    }
    if (!vl53l4cd_read_reg16(VL53L4CD_REG_XTALK_PLANE_OFFSET_KCPS, &reg_value)) {
        return false;
    }
    *xtalk_kcps = (uint16_t)(reg_value >> 9);
    return true;
}

bool vl53l4cd_calibrate_xtalk(uint16_t target_distance_mm, uint16_t num_samples,
                              uint16_t *measured_xtalk_kcps)
{
    vl53l4cd_result_t result;
    uint32_t distance_sum = 0U;
    uint32_t signal_sum = 0U;
    uint32_t spad_sum = 0U;
    uint32_t valid_samples = 0U;
    uint16_t avg_distance;
    uint32_t avg_signal;
    uint32_t avg_spad;
    uint32_t signal_ratio;
    uint16_t xtalk_kcps;
    uint16_t i;

    if (measured_xtalk_kcps == NULL ||
        target_distance_mm < 10U || target_distance_mm > 5000U ||
        num_samples < 5U || num_samples > 255U) {
        return false;
    }

    /* 先禁用串扰补偿再采样 */
    if (!vl53l4cd_set_xtalk(0U)) {
        return false;
    }

    if (!vl53l4cd_stop_ranging() || !vl53l4cd_start_ranging()) {
        return false;
    }

    /* 热机 10 帧，丢弃 */
    for (i = 0U; i < 10U; i++) {
        if (!vl53l4cd_wait_for_data_ready(VL53L4CD_DATA_TIMEOUT_MS)) {
            (void)vl53l4cd_stop_ranging();
            return false;
        }
        (void)vl53l4cd_clear_interrupt();
        (void)vl53l4cd_get_result(&result);
    }

    /* 统计有效测距样本（ST 参考实现：跳过首个采样帧） */
    for (i = 0U; i < num_samples; i++) {
        if (!vl53l4cd_wait_for_data_ready(VL53L4CD_DATA_TIMEOUT_MS)) {
            (void)vl53l4cd_stop_ranging();
            return false;
        }
        (void)vl53l4cd_clear_interrupt();
        if (i > 0U &&
            vl53l4cd_get_result(&result) &&
            result.range_status == VL53L4CD_STATUS_RANGE_VALID) {
            distance_sum += result.distance_mm;
            signal_sum += result.signal_rate_kcps;
            spad_sum += result.number_of_spad;
            valid_samples++;
        }
    }

    (void)vl53l4cd_stop_ranging();

    if (valid_samples == 0U) {
        return false;
    }

    /* 定点数计算 xtalk = (1 - 平均距离/目标距离) * 平均信号/平均SPAD数 */
    avg_distance = (uint16_t)(distance_sum / valid_samples);
    avg_signal = signal_sum / valid_samples;
    avg_spad = spad_sum / valid_samples;
    if (avg_spad == 0U || avg_distance >= target_distance_mm) {
        return false;
    }

    /* 乘法上界安全：avg_signal<=524280, (target-avg_distance)<=4990, 乘积 < 2^32 */
    signal_ratio = (uint32_t)((avg_signal * (uint32_t)(target_distance_mm - avg_distance))
                              / target_distance_mm);
    xtalk_kcps = (uint16_t)((signal_ratio + (avg_spad / 2U)) / avg_spad);
    if (xtalk_kcps > 127U) {
        /* 超出传感器固件支持的最大串扰值，判定为校准失败 */
        return false;
    }

    if (!vl53l4cd_set_xtalk(xtalk_kcps)) {
        return false;
    }

    *measured_xtalk_kcps = xtalk_kcps;
    return true;
}

bool vl53l4cd_init(void)
{
    uint16_t model_id = 0U;
    uint8_t firmware_status = 0U;
    uint32_t elapsed_ms = 0U;
    uint16_t reg;
    uint8_t count;

    /* 上电后固件需要一点启动时间，先重试读取型号 */
    while (elapsed_ms < VL53L4CD_BOOT_TIMEOUT_MS) {
        if (vl53l4cd_read_reg16(VL53L4CD_REG_IDENTIFICATION_MODEL_ID, &model_id) &&
            model_id == 0xEBAAU) {
            break;
        }
        delay_ms(2);
        elapsed_ms += 2U;
    }
    if (model_id != 0xEBAAU) {
        return false;
    }

    /* 等待固件启动完成，FIRMWARE__SYSTEM_STATUS == 0x03 */
    elapsed_ms = 0U;
    while (elapsed_ms < VL53L4CD_BOOT_TIMEOUT_MS) {
        if (vl53l4cd_read_reg8(VL53L4CD_REG_FIRMWARE_SYSTEM_STATUS, &firmware_status) &&
            firmware_status == 0x03U) {
            break;
        }
        delay_ms(2);
        elapsed_ms += 2U;
    }
    if (firmware_status != 0x03U) {
        return false;
    }

    /* I2C/GPIO 上拉配置：本板按 3.3V AVDD 上拉处理 */
    if (!vl53l4cd_write_reg8(0x002DU, 0x00U) ||
        !vl53l4cd_write_reg8(0x002EU, 0x00U) ||
        !vl53l4cd_write_reg8(0x002FU, 0x00U)) {
        return false;
    }

    /* 写入 0x30..0x87 默认配置，分 3 次块写 */
    for (reg = 0x0030U; reg <= 0x0087U; reg += 30U) {
        uint8_t block[30];
        count = 0U;
        while (count < 30U && (reg + count) <= 0x0087U) {
            block[count] = g_vl53l4cd_default_config[reg + count - 0x0030U];
            count++;
        }
        if (!vl53l4cd_write_regs(reg, block, count)) {
            return false;
        }
    }

    /* 启动 VHV 校准并等待首个结果 */
    if (!vl53l4cd_write_reg8(VL53L4CD_REG_SYSTEM_START, 0x40U)) {
        return false;
    }
    elapsed_ms = 0U;
    while (!vl53l4cd_data_ready()) {
        delay_ms(5);
        elapsed_ms += 5U;
        if (elapsed_ms >= VL53L4CD_DATA_TIMEOUT_MS) {
            return false;
        }
    }

    if (!vl53l4cd_clear_interrupt() ||
        !vl53l4cd_stop_ranging() ||
        !vl53l4cd_write_reg8(VL53L4CD_REG_VHV_TIMEOUT_MACROP_LOOP_BOUND, 0x09U) ||
        !vl53l4cd_write_reg8(0x000BU, 0x00U) ||
        !vl53l4cd_write_reg16(0x0024U, 0x0500U)) {
        return false;
    }

    /* 默认 50ms 时序预算，连续模式 */
    return vl53l4cd_set_range_timing(50U, 0U);
}

bool vl53l4cd_start_ranging(void)
{
    /* 本驱动固定使用连续模式 */
    return vl53l4cd_write_reg8(VL53L4CD_REG_SYSTEM_START, 0x21U);
}

bool vl53l4cd_stop_ranging(void)
{
    return vl53l4cd_write_reg8(VL53L4CD_REG_SYSTEM_START, 0x80U);
}