#include "corridor_preproc.h"

/**
 * @brief 内部辅助函数：处理单个侧向 VL53L0X 测距
 * @return true 表示数据合法且在量程内，false 表示异常或超量程飞点
 */
static bool process_side_laser(const SensorItem_t *item, float *out_m)
{
    // 驱动层报异常，直接抛弃
    if (!item->is_valid) {
        return false;
    }
    
    // 毫米转米
    float dist_m = item->value / 1000.0f;
    
    // 剔除硬件量程外的死区或异常噪点
    if (dist_m > PREPROC_MAX_SIDE_RANGE_M || dist_m < PREPROC_MIN_SIDE_RANGE_M) {
        return false;
    }

    /* 靠墙过近退化：
     * 侧向 VL53 在最小量程附近（例如实际 3cm、模组最小可信 4cm）时，
     * 直接判无效会让系统退化成单边观测，反而更容易贴墙。
     * 这里改成“钳位到最小可信距离”，既保留该侧存在感，又避免把不可信的超近值直接送上去。 */
    if (dist_m < PREPROC_SAT_NEAR_SIDE_RANGE_M) {
        dist_m = PREPROC_SAT_NEAR_SIDE_RANGE_M;
    }
    
    *out_m = dist_m;
    return true;
}

/**
 * @brief 内部辅助函数：处理前后向互补雷达 (STP 远距离 + ATK 近盲区)
 * @note  完美解决 STP-23L 的 7cm 物理盲区问题，防止“幽灵撞墙”
 */
static bool process_complementary_laser(const SensorItem_t *stp, const SensorItem_t *atk, float *out_m)
{
    bool stp_ok = stp->is_valid && (stp->value > 0.0f);
    bool atk_ok = atk->is_valid && (atk->value > 0.0f);

    float stp_m = stp->value / 1000.0f;
    float atk_m = atk->value / 1000.0f;

    /* 1. 近距离补盲优先：如果 ATK 存活，且测距进入 8cm 内 (7cm盲区+1cm裕量) */
    /* 此时 STP 的数据大概率是垃圾数据（卡死在7cm或报错），无条件信任 ATK */
    if (atk_ok && atk_m <= PREPROC_BLIND_ZONE_M) {
        *out_m = atk_m;
        return true;
    }

    /* 2. 正常中远距离：两者都存活，取保守值防撞 */
    if (stp_ok && atk_ok) {
        // 如果 STP 读数等于 7cm 极限值，它可能已经瞎了，直接用 ATK 的值
        if (stp_m <= 0.07f) {
            *out_m = atk_m;
        } else {
            *out_m = (stp_m < atk_m) ? stp_m : atk_m; // 正常情况取两者中更近的
        }
        return true;
    }

    /* 3. 硬件单体降级：只剩下一个雷达活着 */
    if (stp_ok) {
        // 只有 STP 活着，但读数在盲区内。这数据极其危险（骗人的），宁可报失效触发急停！
        if (stp_m <= 0.07f) {
            return false; 
        }
        *out_m = stp_m;
        return true;
    }
    
    if (atk_ok) {
        *out_m = atk_m;
        return true;
    }

    /* 全挂了，通知上层失明 */
    return false;
}

void CorridorPreproc_ExtractObs(const RobotBlackboard_t *board, uint32_t now_ms, CorridorObs_t *out_obs)
{
    // 初始化输出结构体，清空掩码
    out_obs->t_ms = now_ms;
    out_obs->valid_mask = 0U;
    
    out_obs->d_lf = 0.0f;
    out_obs->d_lr = 0.0f;
    out_obs->d_rf = 0.0f;
    out_obs->d_rr = 0.0f;
    out_obs->d_front = 0.0f;
    out_obs->d_back = 0.0f;

    /* --- 1. 独立清洗侧向 4 个 VL53L0X 数据 --- */
    if (process_side_laser(&board->dist_left_f, &out_obs->d_lf)) {
        out_obs->valid_mask |= CORRIDOR_OBS_MASK_LF;
    }
    if (process_side_laser(&board->dist_left_r, &out_obs->d_lr)) {
        out_obs->valid_mask |= CORRIDOR_OBS_MASK_LR;
    }
    if (process_side_laser(&board->dist_right_f, &out_obs->d_rf)) {
        out_obs->valid_mask |= CORRIDOR_OBS_MASK_RF;
    }
    if (process_side_laser(&board->dist_right_r, &out_obs->d_rr)) {
        out_obs->valid_mask |= CORRIDOR_OBS_MASK_RR;
    }

    /* --- 2. 前向雷达：远近互补融合 --- */
    if (process_complementary_laser(&board->dist_front_stp, &board->dist_front_atk, &out_obs->d_front)) {
        /* 补偿传感器内缩：将"传感器到墙"转换为"车体前端到墙"
         * d_body = d_sensor - front_laser_offset
         * 当 offset > 0 时表示传感器在车体内部，车体前端比传感器更接近墙 */
        out_obs->d_front -= PARAM_FRONT_LASER_OFFSET;
        if (out_obs->d_front < 0.0f) out_obs->d_front = 0.0f;  /* 安全下限 */
        out_obs->valid_mask |= CORRIDOR_OBS_MASK_FRONT;
    }

    /* --- 3. 后向雷达：远近互补融合 --- */
    if (process_complementary_laser(&board->dist_rear_stp, &board->dist_rear_atk, &out_obs->d_back)) {
        /* 补偿传感器内缩：同前向逻辑 */
        out_obs->d_back -= PARAM_REAR_LASER_OFFSET;
        if (out_obs->d_back < 0.0f) out_obs->d_back = 0.0f;
        out_obs->valid_mask |= CORRIDOR_OBS_MASK_BACK;
    }
}