#include "robot_blackboard.h"
#include "FreeRTOS.h"
#include "task.h"
#include <string.h>

/* 全局唯一的黑板实例 */
static RobotBlackboard_t g_blackboard = {0};

/* =========================================================
 * 生产者 API 实现
 * ========================================================= */

void Blackboard_UpdateIMU(const HWT101_Data_t *imu_data) {
    if (imu_data == NULL) return;
    
    taskENTER_CRITICAL();
    // 更新原始航向角 ([-180, +180) 度)
    g_blackboard.imu_yaw.value = imu_data->yaw;
    g_blackboard.imu_yaw.timestamp_ms = imu_data->last_update;
    g_blackboard.imu_yaw.is_valid = (imu_data->online != 0);

    // 更新 unwrap 后的连续航向角 (无跳变, 度)
    g_blackboard.imu_yaw_continuous.value = imu_data->yaw_continuous;
    g_blackboard.imu_yaw_continuous.timestamp_ms = imu_data->last_update;
    g_blackboard.imu_yaw_continuous.is_valid = (imu_data->online != 0);

    // 更新角速度 (deg/s)
    g_blackboard.imu_wz.value = imu_data->wz;
    g_blackboard.imu_wz.timestamp_ms = imu_data->last_update;
    g_blackboard.imu_wz.is_valid = (imu_data->online != 0);
    taskEXIT_CRITICAL();
}

void Blackboard_UpdateVl53(const Vl53BoardSnapshot_t *vl_data) {
    if (vl_data == NULL) return;
    
    taskENTER_CRITICAL();
    // 约定: [0]左前, [1]左后, [2]右前, [3]右后
    g_blackboard.dist_left_f.value        = vl_data->range_mm_filtered[0];
    g_blackboard.dist_left_f.is_valid     = (vl_data->valid_mask & (1 << 0)) != 0;
    g_blackboard.dist_left_f.timestamp_ms = vl_data->tick_ms;

    g_blackboard.dist_left_r.value        = vl_data->range_mm_filtered[1];
    g_blackboard.dist_left_r.is_valid     = (vl_data->valid_mask & (1 << 1)) != 0;
    g_blackboard.dist_left_r.timestamp_ms = vl_data->tick_ms;

    g_blackboard.dist_right_f.value        = vl_data->range_mm_filtered[2];
    g_blackboard.dist_right_f.is_valid     = (vl_data->valid_mask & (1 << 2)) != 0;
    g_blackboard.dist_right_f.timestamp_ms = vl_data->tick_ms;

    g_blackboard.dist_right_r.value        = vl_data->range_mm_filtered[3];
    g_blackboard.dist_right_r.is_valid     = (vl_data->valid_mask & (1 << 3)) != 0;
    g_blackboard.dist_right_r.timestamp_ms = vl_data->tick_ms;
    taskEXIT_CRITICAL();
}

void Blackboard_UpdateLaser(const laser_simple_snapshot_t *ls_data) {
    if (ls_data == NULL) return;

    taskENTER_CRITICAL();
    
    // 1. 前方 STP
    g_blackboard.dist_front_stp.value = ls_data->ch[LASER_CH_FRONT_STP].distance_mm;
    g_blackboard.dist_front_stp.is_valid = ls_data->ch[LASER_CH_FRONT_STP].valid;
    g_blackboard.dist_front_stp.timestamp_ms = ls_data->ch[LASER_CH_FRONT_STP].update_tick_ms;

    // 2. 前方 ATK
    g_blackboard.dist_front_atk.value = ls_data->ch[LASER_CH_FRONT_ATK].distance_mm;
    g_blackboard.dist_front_atk.is_valid = ls_data->ch[LASER_CH_FRONT_ATK].valid;
    g_blackboard.dist_front_atk.timestamp_ms = ls_data->ch[LASER_CH_FRONT_ATK].update_tick_ms;

    // 3. 后方 STP
    g_blackboard.dist_rear_stp.value = ls_data->ch[LASER_CH_REAR_STP].distance_mm;
    g_blackboard.dist_rear_stp.is_valid = ls_data->ch[LASER_CH_REAR_STP].valid;
    g_blackboard.dist_rear_stp.timestamp_ms = ls_data->ch[LASER_CH_REAR_STP].update_tick_ms;

    // 4. 后方 ATK
    g_blackboard.dist_rear_atk.value = ls_data->ch[LASER_CH_REAR_ATK].distance_mm;
    g_blackboard.dist_rear_atk.is_valid = ls_data->ch[LASER_CH_REAR_ATK].valid;
    g_blackboard.dist_rear_atk.timestamp_ms = ls_data->ch[LASER_CH_REAR_ATK].update_tick_ms;

    taskEXIT_CRITICAL();
}

void Blackboard_UpdateOdom(float vx, float wz) {
    taskENTER_CRITICAL();
    g_blackboard.odom_vx = vx;
    g_blackboard.odom_wz = wz;
    taskEXIT_CRITICAL();
}

void Blackboard_UpdateChassisState(uint8_t state, uint32_t diag, bool online) {
    taskENTER_CRITICAL();
    g_blackboard.chassis_state = state;
    g_blackboard.chassis_diag = diag;
    g_blackboard.chassis_online = online;
    taskEXIT_CRITICAL();
}

/* =========================================================
 * 消费者 API 实现
 * ========================================================= */

void Blackboard_GetSnapshot(RobotBlackboard_t *out_snapshot) {
    if (out_snapshot == NULL) return;
    
    // 关中断拷贝整个结构体，保证上层算法拿到的数据处于同一个时间切片
    taskENTER_CRITICAL();
    *out_snapshot = g_blackboard;
    taskEXIT_CRITICAL();
}