#include "robot_params.h"

#if PARAM_VL53_USE_L1X

#include "vl53_board.h"
#include "FreeRTOS.h"
#include "task.h"
#include "vl53_calibration_config.h"

static void vl53_ema_init(Vl53EMA_t *ema, float alpha)
{
    ema->x = 0.0f;
    ema->alpha = alpha;
    ema->initialized = 0u;
}

static float vl53_ema_update(Vl53EMA_t *ema, float measurement)
{
    if (ema->initialized == 0u) {
        ema->x = measurement;
        ema->initialized = 1u;
        return ema->x;
    }

    ema->x = ema->alpha * measurement + (1.0f - ema->alpha) * ema->x;
    return ema->x;
}

static const Vl53L1RuntimeCalibration_t *vl53_get_runtime_calibration(uint8_t id)
{
    switch (id) {
    case 0: return &k_vl53l1_left_calibration[0];
    case 1: return &k_vl53l1_left_calibration[1];
    case 2: return &k_vl53l1_right_calibration[0];
    case 3: return &k_vl53l1_right_calibration[1];
    default: return NULL;
    }
}

static VL53L1_Error vl53_apply_runtime_calibration(VL53L1_DEV dev, uint8_t id)
{
    const Vl53L1RuntimeCalibration_t *cal = vl53_get_runtime_calibration(id);

    if ((cal == NULL) || (cal->calibrated == 0u)) {
        return VL53L1_ERROR_NONE;
    }

    return VL53L1_SetCalibrationData(dev, (VL53L1_CalibrationData_t *)&cal->data);
}

static uint32_t vl53_sanitize_timing_budget_us(uint32_t timing_budget_us)
{
    if (timing_budget_us < 20000u) {
        return 20000u;
    }
    if (timing_budget_us > 1000000u) {
        return 1000000u;
    }
    return timing_budget_us;
}

static uint32_t vl53_compute_inter_measurement_ms(uint32_t timing_budget_us)
{
    uint32_t timing_budget_ms = (timing_budget_us + 999u) / 1000u;

    /* UM2356: inter-measurement period shorter than timing budget时会立即开始下一帧。 */
    if (timing_budget_ms < 25u) {
        timing_budget_ms = 25u;
    }
    return timing_budget_ms;
}

static VL53L1_Error vl53_configure_ranging_profile(VL53L1_DEV dev, uint32_t timing_budget_us)
{
    VL53L1_Error status;
    const uint32_t inter_measurement_ms = vl53_compute_inter_measurement_ms(timing_budget_us);

    status = VL53L1_SetPresetMode(dev, VL53L1_PRESETMODE_LITE_RANGING);
    if (status != VL53L1_ERROR_NONE) return status;

    status = VL53L1_SetDistanceMode(dev, VL53L1_DISTANCEMODE_LONG);
    if (status != VL53L1_ERROR_NONE) return status;

    status = VL53L1_SetMeasurementTimingBudgetMicroSeconds(dev, timing_budget_us);
    if (status != VL53L1_ERROR_NONE) return status;

    status = VL53L1_SetInterMeasurementPeriodMilliSeconds(dev, inter_measurement_ms);
    if (status != VL53L1_ERROR_NONE) return status;

    status = VL53L1_SetLimitCheckEnable(dev, VL53L1_CHECKENABLE_SIGMA_FINAL_RANGE, 1u);
    if (status != VL53L1_ERROR_NONE) return status;

    status = VL53L1_SetLimitCheckEnable(dev, VL53L1_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1u);
    if (status != VL53L1_ERROR_NONE) return status;

    return VL53L1_ERROR_NONE;
}

static VL53L1_Error vl53_do_static_init(VL53L1_DEV dev, uint32_t timing_budget_us, uint8_t id)
{
    VL53L1_Error status;

    status = VL53L1_StaticInit(dev);
    if (status != VL53L1_ERROR_NONE) return status;

    status = vl53_apply_runtime_calibration(dev, id);
    if (status != VL53L1_ERROR_NONE) return status;

    return vl53_configure_ranging_profile(dev, timing_budget_us);
}

VL53L1_Error Vl53Board_Init(Vl53Board_t *board, const Vl53BoardHwCfg_t *hw_cfgs, uint8_t count, uint32_t timing_budget_us)
{
    if ((board == NULL) || (hw_cfgs == NULL) || (count == 0u) || (count > VL53_MAX_DEVS_PER_BOARD)) {
        return VL53L1_ERROR_INVALID_PARAMS;
    }

    memset(board, 0, sizeof(Vl53Board_t));
    board->dev_count = count;
    board->timing_budget_us = vl53_sanitize_timing_budget_us((timing_budget_us == 0u) ? 33000u : timing_budget_us);

    for (uint8_t i = 0; i < count; i++) {
        board->dev[i].name = hw_cfgs[i].name;
        board->dev[i].id = hw_cfgs[i].id;
        board->dev[i].comms_type = VL53L1_I2C;
        board->dev[i].comms_speed_khz = 400u;
        board->dev[i].is_present = 0u;

        VL53L1_PlatformAttachBus(&board->dev[i], hw_cfgs[i].hi2c, VL53L1_DEFAULT_ADDR_8BIT, 100u, NULL);
        VL53L1_PlatformAttachPins(&board->dev[i], hw_cfgs[i].xshut_port, hw_cfgs[i].xshut_pin, NULL, 0u);
        (void)VL53L1_PlatformSetXShut(&board->dev[i], GPIO_PIN_RESET);

        vl53_ema_init(&board->ema[i], PARAM_VL53_EMA_ALPHA);
    }

    vTaskDelay(pdMS_TO_TICKS(10u));

    for (uint8_t i = 0; i < count; i++) {
        if (VL53L1_PlatformSetXShut(&board->dev[i], GPIO_PIN_SET) != VL53L1_ERROR_NONE) continue;
        vTaskDelay(pdMS_TO_TICKS(20u));

        board->dev[i].i2c_slave_address = VL53L1_DEFAULT_ADDR_8BIT;
        if (VL53L1_CommsInitialise(&board->dev[i], VL53L1_I2C, board->dev[i].comms_speed_khz) != VL53L1_ERROR_NONE) continue;
        if (VL53L1_WaitDeviceBooted(&board->dev[i]) != VL53L1_ERROR_NONE) continue;
        if (VL53L1_PlatformChangeAddress(&board->dev[i], hw_cfgs[i].runtime_addr_8bit) != VL53L1_ERROR_NONE) continue;
        if (VL53L1_DataInit(&board->dev[i]) != VL53L1_ERROR_NONE) continue;
        if (vl53_do_static_init(&board->dev[i], board->timing_budget_us, hw_cfgs[i].id) != VL53L1_ERROR_NONE) continue;

        board->init_mask |= (uint8_t)(1u << i);
        board->dev[i].is_present = 1u;
    }

    return VL53L1_ERROR_NONE;
}

VL53L1_Error Vl53Board_StartContinuous(Vl53Board_t *board)
{
    if (board == NULL) return VL53L1_ERROR_INVALID_PARAMS;

    for (uint8_t i = 0; i < board->dev_count; i++) {
        if (board->init_mask & (1u << i)) {
            (void)VL53L1_StartMeasurement(&board->dev[i]);
        }
    }

    return VL53L1_ERROR_NONE;
}

VL53L1_Error Vl53Board_StopContinuous(Vl53Board_t *board)
{
    if (board == NULL) return VL53L1_ERROR_INVALID_PARAMS;

    for (uint8_t i = 0; i < board->dev_count; i++) {
        if (board->init_mask & (1u << i)) {
            (void)VL53L1_StopMeasurement(&board->dev[i]);
        }
    }

    return VL53L1_ERROR_NONE;
}

VL53L1_Error Vl53Board_ReadAll(Vl53Board_t *board, Vl53BoardSnapshot_t *snapshot)
{
    if ((board == NULL) || (snapshot == NULL)) return VL53L1_ERROR_INVALID_PARAMS;

    memset(snapshot, 0, sizeof(Vl53BoardSnapshot_t));
    snapshot->tick_ms = xTaskGetTickCount() * portTICK_PERIOD_MS;

    for (uint8_t i = 0; i < board->dev_count; i++) {
        if ((board->init_mask & (1u << i)) == 0u) {
            snapshot->range_status[i] = 255u;
            continue;
        }

        uint8_t ready = 0u;
        if (VL53L1_GetMeasurementDataReady(&board->dev[i], &ready) != VL53L1_ERROR_NONE) continue;

        if (ready != 0u) {
            VL53L1_RangingMeasurementData_t data;
            memset(&data, 0, sizeof(data));

            if (VL53L1_GetRangingMeasurementData(&board->dev[i], &data) == VL53L1_ERROR_NONE) {
                snapshot->range_mm[i] = (data.RangeMilliMeter < 0) ? 0u : (uint16_t)data.RangeMilliMeter;
                snapshot->range_status[i] = data.RangeStatus;

                if (data.RangeStatus == 0u) {
                    snapshot->valid_mask |= (uint8_t)(1u << i);
#if PARAM_VL53_USE_EMA_FILTER
                    snapshot->range_mm_filtered[i] = vl53_ema_update(&board->ema[i], (float)snapshot->range_mm[i]);
#else
                    snapshot->range_mm_filtered[i] = (float)snapshot->range_mm[i];
                    board->ema[i].x = (float)snapshot->range_mm[i];
                    board->ema[i].initialized = 1u;
#endif
                } else {
                    snapshot->range_mm_filtered[i] = board->ema[i].x;
                }

                (void)VL53L1_ClearInterruptAndStartMeasurement(&board->dev[i]);
            }
        } else {
            snapshot->range_mm_filtered[i] = board->ema[i].x;
        }
    }

    return VL53L1_ERROR_NONE;
}

#endif