#include "app_tasks.h"
#include "FreeRTOS.h"
#include "cmsis_os.h"
#include "main.h"
#include "snc_can_app.h"
#include "robot_params.h"  /* 统一参数配置 */
#include <stdio.h>
#include <string.h>

/* 引入各个传感器和黑板的头文件 */
#include "Contract/robot_blackboard.h"
#include "Contract/robot_odom.h"
#include "laser/laser_manager.h"
#include "IMU/hwt101.h"
#include "vl53_board.h"
#include "Contract/robot_cmd_slot.h"
#include "preproc/corridor_preproc.h"
#include "est/corridor_filter.h"
#include "nav/corridor_ctrl.h"
#include "nav/segment_fsm.h"
#include "nav/nav_script.h"
#include "nav/global_nav.h"
#include "nav/track_map.h"
extern osMutexId_t logMutexHandle;

/* 如果你的项目中没有引入 i2c.h，可以通过 extern 声明 I2C 句柄 */
extern I2C_HandleTypeDef hi2c1;
extern I2C_HandleTypeDef hi2c2;

static float g_test_vx = 0.0f;
static float g_test_wz = 0.0f;

static uint32_t AppTasks_MsToTicks(uint32_t ms)
{
  uint32_t ticks = pdMS_TO_TICKS(ms);
  return (ticks == 0U) ? 1U : ticks;
}

static void App_UpdateTestCommand(void)
{
  g_test_vx = 0.0f;
  g_test_wz = 0.0f;
}

static void App_PrintStatus(void)
{
  // 【新增】静态计数器，由于是 static 修饰，它的值在函数退出后不会丢失
  static uint8_t print_divider = 0;

  // 假设外部任务是 100ms 调用一次该函数：
  // 如果设定 >= 5，就是 5 * 100ms = 500ms 打印一次
  // 如果设定 >= 10，就是 10 * 100ms = 1000ms (1秒) 打印一次
  if (++print_divider < 5) {
    return; // 还没到设定的次数，直接 return，不执行后续获取快照和打印的动作
  }
  print_divider = 0; // 计数器清零，重新开始下一轮计数

  // 1. 获取全局黑板的快照
  RobotBlackboard_t snap;
  Blackboard_GetSnapshot(&snap);

  // 2. 申请串口互斥锁，防止多线程打印打架导致乱码
  if (logMutexHandle != NULL) {
    (void)osMutexAcquire(logMutexHandle, osWaitForever);
  }

  // 3. 打印表头和当前系统时间
  printf("\r\n========== Robot Status [%lu ms] ==========\r\n", HAL_GetTick());

  // 4. 打印 IMU 姿态
  if (snap.imu_yaw.is_valid) {
    printf("[IMU]     Yaw: %7.2f deg | Cont: %7.2f deg | Wz: %7.2f\r\n",
           snap.imu_yaw.value, snap.imu_yaw_continuous.value, snap.imu_wz.value);
  } else {
    printf("[IMU]     --- OFFLINE or INVALID ---\r\n");
  }

  // 5. 打印侧向阵列雷达 (强制转为 int，去掉多余的小数点)
  printf("[%s] Left-F : %4d mm | Left-R : %4d mm\r\n",
         VL53_BOARD_DRIVER_NAME,
         (int)snap.dist_left_f.value, (int)snap.dist_left_r.value);
  printf("          Right-F: %4d mm | Right-R: %4d mm\r\n",
         (int)snap.dist_right_f.value, (int)snap.dist_right_r.value);

  // 6. 打印前后激光测距雷达 (四个探头独立显示)
  printf("[LASER]   F-STP: %4d mm | F-ATK: %4d mm\r\n",
         (int)snap.dist_front_stp.value, (int)snap.dist_front_atk.value);
  printf("          R-STP: %4d mm | R-ATK: %4d mm\r\n",
         (int)snap.dist_rear_stp.value, (int)snap.dist_rear_atk.value);
  printf("==================================================\r\n");

  // 7. 释放锁
  if (logMutexHandle != NULL) {
    (void)osMutexRelease(logMutexHandle);
  }
}

void AppTasks_RunCanTxTask(void *argument)
{
  (void)argument;
  uint32_t wake_tick = osKernelGetTickCount();
  const uint32_t period_ticks = AppTasks_MsToTicks(20U);
  // 用于存放从指令槽里取出来的控制指令
  RobotTargetCmd_t current_cmd;

  for (;;) {
    // 维持心跳发送
    SNC_CAN_20msTask();

    // 从指令槽中获取最新指令，设置 100ms 超时容忍度
    if (CmdSlot_Pop(&current_cmd, 100)) {
      // 如果算法层在 100ms 内有更新指令，正常下发给底盘
      (void)SNC_CAN_SendCmdVel(current_cmd.target_vx, current_cmd.target_wz, current_cmd.ctrl_flags);
    } else {
      // 🚨 触发安全保护：如果超过 100ms 没收到上层新指令（可能算法死机或网络断开）
      // 强制发送 0 速度，让底盘立刻停车，防止飞车撞墙
      (void)SNC_CAN_SendCmdVel(0.0f, 0.0f, 0U);
    }

    wake_tick += period_ticks;
    (void)osDelayUntil(wake_tick);
  }
}

void AppTasks_RunMonitorTask(void *argument)
{
  (void)argument;
  uint32_t wake_tick = osKernelGetTickCount();
  const uint32_t period_ticks = AppTasks_MsToTicks(100U);

  for (;;) {
    uint32_t now_ms = HAL_GetTick();

    // 1. 执行 CAN 侧的周期性监控 (心跳/超时判断)
    SNC_CAN_100msTask();
    SNC_CAN_PollOnlineState(now_ms);

    // 2. 获取 CAN 数据上下文
    const SNC_CAN_AppContext_t *can_ctx = SNC_CAN_GetContext();

    // 3. 将底盘的核心状态写入全局黑板
    Blackboard_UpdateChassisState(can_ctx->status.state,
                                  can_ctx->status.diag_bits,
                                  can_ctx->status.online);

    // 4. 里程计更新已迁移到 navTask(20ms)，此处不再消费
    //    原因：odom 实时性对 EKF 预测至关重要，需要与导航闭环同频

    /* 指令下发已由 navTask 接管，此处不再发送测试指令 */
    // 调用纯净版播报员 (内部自带分频逻辑)
    // App_PrintStatus();

    wake_tick += period_ticks;
    (void)osDelayUntil(wake_tick);
  }
}
/* =========================================================
 * 1. 前后激光雷达任务 (Laser Manager)
 * ========================================================= */
void AppTasks_RunLaserTestTask_Impl(void *argument)
{
  (void)argument;

  // 初始化前后防撞雷达
  LASER_SIMPLE_Init();

  for (;;)
  {
    // 底层高频巡检
    LASER_SIMPLE_Poll(HAL_GetTick());

    // 获取无撕裂快照
    const laser_simple_snapshot_t *snap = LASER_SIMPLE_GetSnapshot();

    // 写入全局数据黑板
    Blackboard_UpdateLaser(snap);

    // 维持 50ms 巡检频率 (避障数据建议稍快一些)
    osDelay(50);
  }
}

/* =========================================================
 * 2. IMU 姿态传感器任务 (HWT101)
 * ========================================================= */
void AppTasks_RunImuTask_Impl(void *argument)
{
  (void)argument;

  // 初始化 IMU (开启 DMA 接收)，使用 UART7
  HWT101_Init(&huart7);
  osDelay(100);
  HWT101_ZeroYaw();
  for (;;) {
    // 高频解析 DMA 数据
    HWT101_Process();

    // 安全拷贝 IMU 数据并写入黑板
    HWT101_Data_t imu_data;
    HWT101_GetData(&imu_data);
    Blackboard_UpdateIMU(&imu_data);

    osDelay(10); // 10ms 解析与更新周期
  }
}

/* =========================================================
 * 3. 侧向阵列雷达任务 (VL53 双总线)
 * ========================================================= */
void AppTasks_RunVl53Task_Impl(void *argument)
{
    (void)argument;
    static Vl53Board_t left_board;
    static Vl53Board_t right_board;
  /* --- 配置左侧雷达 (I2C2) --- */
  Vl53BoardHwCfg_t left_cfgs[2] = {
    {&hi2c2, GPIOB, GPIO_PIN_1, 0x62, "LF", 0}, // 左前 (PB1)
    {&hi2c2, GPIOC, GPIO_PIN_5, 0x64, "LR", 1}  // 左后 (PC5)
  };

  // 【修改 1】：测距预算从 robot_params.h 读取
  Vl53Board_Init(&left_board, left_cfgs, 2, PARAM_VL53_TIMING_BUDGET);
  Vl53Board_StartContinuous(&left_board);

  /* --- 配置右侧雷达 (I2C1) --- */
  Vl53BoardHwCfg_t right_cfgs[2] = {
    {&hi2c1, GPIOD, GPIO_PIN_13, 0x56, "RF", 2}, // 右前 (PD13)
    {&hi2c1, GPIOD, GPIO_PIN_14, 0x58, "RR", 3}  // 右后 (PD14)
  };

  // 【修改 2】：同样从 robot_params.h 读取
  Vl53Board_Init(&right_board, right_cfgs, 2, PARAM_VL53_TIMING_BUDGET);
  Vl53Board_StartContinuous(&right_board);

    Vl53BoardSnapshot_t left_snap, right_snap, combined_snap;

    for(;;) {
        // 读取左右两侧的雷达快照
        Vl53Board_ReadAll(&left_board, &left_snap);
        Vl53Board_ReadAll(&right_board, &right_snap);

        // 合并到一个统一的 Snapshot 中送给黑板
        memset(&combined_snap, 0, sizeof(combined_snap));
        combined_snap.tick_ms = HAL_GetTick();

        // 映射约定：[0]左前，[1]左后，[2]右前，[3]右后
        for(int i = 0; i < 2; i++) {
            // 左侧复制到 [0], [1]
            combined_snap.range_mm[i]          = left_snap.range_mm[i];
            combined_snap.range_mm_filtered[i] = left_snap.range_mm_filtered[i];
            combined_snap.range_status[i]      = left_snap.range_status[i];

            // 右侧偏移 2 个身位，复制到 [2], [3]
            combined_snap.range_mm[i+2]          = right_snap.range_mm[i];
            combined_snap.range_mm_filtered[i+2] = right_snap.range_mm_filtered[i];
            combined_snap.range_status[i+2]      = right_snap.range_status[i];
        }

        // 合并掩码 (右侧掩码左移 2 位)
        combined_snap.valid_mask = (left_snap.valid_mask & 0x03) | ((right_snap.valid_mask & 0x03) << 2);

        // 统一推送到黑板
        Blackboard_UpdateVl53(&combined_snap);

        // 与 VL53 约 33ms 的连续测量节奏匹配，避免高频空轮询旧数据。
        osDelay(33);
    }
}

/* =========================================================
 * 4. 走廊导航控制任务 (Nav Pipeline)
 *    周期：20ms (50Hz)，与 CAN 发送同频
 *    流水线：Obs → Filter → Ctrl → Script → FSM → CmdSlot_Push
 * ========================================================= */
void AppTasks_RunNavTask_Impl(void *argument)
{
  (void)argument;
  uint32_t wake_tick = osKernelGetTickCount();
  const uint32_t period_ticks = AppTasks_MsToTicks(20U);
  uint32_t last_ms = HAL_GetTick();

  /* 等传感器全部就绪再启动 (避免刚上电全是脏数据)。
   * 实车上 IMU 清零和多路测距任务稳定需要数秒，500ms 不够。 */
  osDelay(PARAM_NAV_STARTUP_DELAY_MS);

#if USE_GLOBAL_NAV
  GlobalNav_Start();   /* 开始赛道级导航 */
#else
  NavScript_Start();   /* 单沟测试模式 */
#endif

  for (;;) {
    uint32_t now_ms = HAL_GetTick();
    float dt_s = (float)(now_ms - last_ms) / 1000.0f;
    if (dt_s <= 0.0f || dt_s > 0.5f) {
      dt_s = 0.02f; /* 容错：防止首拍或溢出 */
    }
    last_ms = now_ms;

    /* --- Step 0: 里程计更新（从 monitorTask 迁移至此，20ms 实时消费）--- */
    {
      int16_t  odom_fl, odom_rl, odom_fr, odom_rr;
      uint32_t odom_span_ms;
      uint8_t  odom_frames = SNC_CAN_ConsumeOdomDelta(
                               &odom_fl, &odom_rl,
                               &odom_fr, &odom_rr,
                               &odom_span_ms);

      if (odom_frames > 0U) {
        /* 有新的增量帧需要积分 */
        Odom_Update(now_ms,
                    odom_fl, odom_rl,
                    odom_fr, odom_rr,
                    odom_span_ms);
      } else {
        /* odom 断流保护：超过协议允许时间后主动清零黑板里的速度 */
        Odom_HandleTimeout(now_ms, SNC_ODOM_TIMEOUT_MS);
      }
    }

    /* --- Step 1: 拍摄黑板快照 --- */
    RobotBlackboard_t board;
    Blackboard_GetSnapshot(&board);

    /* --- Step 2: 预处理 → 清洗观测 --- */
    CorridorObs_t obs;
    CorridorPreproc_ExtractObs(&board, now_ms, &obs);

    /* --- Step 3: EKF → 走廊状态估计 --- */
    /* 注意: HWT101 输出 wz 单位是 °/s，EKF 需要 rad/s，必须转换！ */
    float imu_wz_raw = board.imu_wz.is_valid ? board.imu_wz.value : 0.0f;
    float imu_wz  = PARAM_DEG2RAD(imu_wz_raw);
    float odom_vx = board.odom_vx;

    /* IMU 连续 yaw → rad，作为 EKF 额外航向观测 */
    float imu_yaw_cont_rad = board.imu_yaw_continuous.is_valid
                             ? PARAM_DEG2RAD(board.imu_yaw_continuous.value) : 0.0f;
    bool  imu_yaw_ok       = board.imu_yaw_continuous.is_valid;

    CorridorState_t corridor_state;
    CorridorFilter_Update(&obs, imu_wz, odom_vx, dt_s,
                          imu_yaw_cont_rad, imu_yaw_ok, &corridor_state);

#if USE_GLOBAL_NAV
    /* ========== 赛道级导航模式 (6沟 S 型遍历) ========== */

    /* --- Step 4: 赛道级导航 --- */
    GlobalNavOutput_t nav_out;
    GlobalNav_Update(&obs, &corridor_state, &board, now_ms, &nav_out);

    /* --- Step 5: 控制律 --- */
    RawCmd_t raw_cmd;
    memset(&raw_cmd, 0, sizeof(raw_cmd));
    raw_cmd.t_ms = now_ms;

    if (nav_out.request_corridor) {
      /* 沟内闭环：使用走廊控制器 */
      CorridorCtrl_Compute(&corridor_state, &obs, imu_wz, &raw_cmd);
    } else if (nav_out.use_override) {
      /* 赛道级覆盖：直接用导航输出 */
      raw_cmd.v = nav_out.override_v;
      raw_cmd.w = nav_out.override_w;
    }
    /* else: raw_cmd 已是零速 */

    /* --- Step 6: 安全仲裁 (带动作模式感知) --- */
    SegFsmOutput_t fsm_out;
    SegFsm_Update(&raw_cmd, &obs, &corridor_state, nav_out.safety_mode, &fsm_out);

#else
    /* ========== 单沟测试模式 (原 nav_script) ========== */

    /* --- Step 4: 段脚本执行器 --- */
    NavScriptOutput_t script_out;
    float imu_yaw_cont_deg = board.imu_yaw_continuous.is_valid
                             ? board.imu_yaw_continuous.value : 0.0f;
    NavScript_Update(&obs, &corridor_state, imu_yaw_cont_deg, &script_out);

    /* --- Step 5: 控制律 --- */
    RawCmd_t raw_cmd;
    if (script_out.use_override) {
      raw_cmd.t_ms = now_ms;
      raw_cmd.v = script_out.override_v;
      raw_cmd.w = script_out.override_w;
      raw_cmd.flags = 0U;
    } else if (script_out.request_corridor) {
      CorridorCtrl_Compute(&corridor_state, &obs, imu_wz, &raw_cmd);
    } else {
      raw_cmd.t_ms = now_ms;
      raw_cmd.v = 0.0f;
      raw_cmd.w = 0.0f;
      raw_cmd.flags = 0U;
    }

    /* --- Step 6: 安全仲裁 (按脚本阶段切换模式) --- */
    SegFsmOutput_t fsm_out;
    SafetyMode_t safety_mode = SAFETY_MODE_CORRIDOR;
    if (!script_out.active) {
      safety_mode = SAFETY_MODE_IDLE;
    } else if (script_out.stage == SCRIPT_STAGE_TURN_AT_END) {
      safety_mode = SAFETY_MODE_TURN;
    }
    SegFsm_Update(&raw_cmd, &obs, &corridor_state, safety_mode, &fsm_out);

#endif

    /* --- Step 7: 将安全后的指令喂给 CAN 发送层 --- */
    CmdSlot_Push(fsm_out.safe_v, fsm_out.safe_w, 0U);

    wake_tick += period_ticks;
    (void)osDelayUntil(wake_tick);
  }
}

void AppTasks_Init(void)
{
  SNC_CAN_AppInit();
  Odom_Init();

  /* --- 初始化走廊滤波器 (EKF) --- */
  CorridorFilterConfig_t filter_cfg = {
    .sensor_base_length = PARAM_SENSOR_BASE_LENGTH,  /* 实测：同侧前后雷达间距 */
    .corridor_width     = PARAM_CORRIDOR_WIDTH,      /* 实测：走廊宽度 */
    .y_offset           = 0.0f,                       /* 0 = 绝对居中 */
    .side_sensor_inset  = PARAM_VL53_SIDE_INSET,     /* [兼容] 统一内缩距离 */
    .left_sensor_inset  = PARAM_VL53_LEFT_INSET,     /* [改进A] 左侧独立内缩 (实测后填入!) */
    .right_sensor_inset = PARAM_VL53_RIGHT_INSET,    /* [改进A] 右侧独立内缩 (实测后填入!) */
    .robot_width        = PARAM_ROBOT_WIDTH,         /* 实测：车体宽度 */
    .alpha_theta        = 0.98f,   /* 保留兼容，EKF 内部使用 Q/R */
    .alpha_y            = 0.7f,
  };
  CorridorFilter_Init(&filter_cfg);

  /* --- 初始化走廊控制器 --- */
  CorridorCtrlConfig_t ctrl_cfg = {
    .kp_theta          = PARAM_CTRL_KP_THETA,        /* 调优：航向比例增益 */
    .kd_theta          = PARAM_CTRL_KD_THETA,        /* 调优：航向微分增益 */
    .kp_y              = PARAM_CTRL_KP_Y,            /* 调优：横向比例增益 */
    .v_cruise          = PARAM_CTRL_V_CRUISE,        /* 调优：巡航速度 */
    .w_max             = PARAM_CTRL_W_MAX,           /* 角速度限幅 */
    .v_max             = PARAM_CTRL_V_MAX,           /* 线速度限幅 */
    .speed_reduction_k = PARAM_CTRL_SPEED_REDUCTION, /* 调优：弯道减速系数 */
    .exit_front_dist   = PARAM_CTRL_EXIT_FRONT_DIST, /* 调优：出沟检测距离 */
    .wall_escape_dist  = PARAM_CTRL_WALL_ESCAPE_DIST,
    .wall_escape_kp    = PARAM_CTRL_WALL_ESCAPE_KP,
    .wall_escape_w_max = PARAM_CTRL_WALL_ESCAPE_WMAX,
  };
  CorridorCtrl_Init(&ctrl_cfg);

  /* --- 初始化段状态机 --- */
  SegFsmConfig_t fsm_cfg = {
    .d_front_stop       = PARAM_SAFE_D_FRONT_STOP,      /* 安全：前向停车距离 */
    .d_front_approach   = PARAM_SAFE_D_FRONT_APPROACH,  /* 安全：前向减速距离 */
    .approach_min_v     = PARAM_SAFE_APPROACH_MIN_V,    /* 安全：减速区最低速度 */
    .conf_estop_thresh  = PARAM_SAFE_CONF_ESTOP,        /* 安全：E-Stop 置信度阈值 */
  };
  SegFsm_Init(&fsm_cfg);
  SegFsm_Start();  /* P0 修复: 必须显式启动安全状态机，否则 IDLE 状态直接输出零速 */

#if USE_GLOBAL_NAV
  /* --- 初始化赛道地图 --- */
  TrackMap_Init();

  /* --- 初始化赛道级导航状态机 --- */
  GlobalNavConfig_t gnav_cfg = {
    .entry_v                = PARAM_GNAV_ENTRY_V,
    .entry_distance         = PARAM_GNAV_ENTRY_DISTANCE,
    .entry_timeout_ms       = PARAM_GNAV_ENTRY_TIMEOUT,
    .turn_omega             = PARAM_GNAV_TURN_OMEGA,
    .turn_tolerance_rad     = PARAM_GNAV_TURN_TOLERANCE,
    .turn_decel_zone_rad    = PARAM_GNAV_TURN_DECEL_ZONE,
    .turn_min_omega         = PARAM_GNAV_TURN_MIN_OMEGA,
    .turn_timeout_ms        = PARAM_GNAV_TURN_TIMEOUT,
    .reacquire_v            = PARAM_GNAV_REACQUIRE_V,
    .reacquire_conf_thresh  = PARAM_GNAV_REACQUIRE_CONF,
    .reacquire_width_tol    = PARAM_GNAV_REACQUIRE_WIDTH_TOL,
    .reacquire_min_odom     = PARAM_GNAV_REACQUIRE_MIN_ODOM,
    .reacquire_confirm_ticks = PARAM_GNAV_REACQUIRE_TICKS,
    .reacquire_timeout_ms   = PARAM_GNAV_REACQUIRE_TIMEOUT,
    .align_kp_th            = PARAM_GNAV_ALIGN_KP_TH,
    .align_kp_y             = PARAM_GNAV_ALIGN_KP_Y,
    .align_th_tol_rad       = PARAM_GNAV_ALIGN_TH_TOL,
    .align_y_tol_m          = PARAM_GNAV_ALIGN_Y_TOL,
    .align_confirm_ticks    = PARAM_GNAV_ALIGN_TICKS,
    .align_timeout_ms       = PARAM_GNAV_ALIGN_TIMEOUT,
    .reacquire_min_back_dist = PARAM_GNAV_REACQUIRE_MIN_BACK,
    .corridor_end_detect_dist = PARAM_GNAV_CORRIDOR_END_DIST,
    .corridor_length_max    = PARAM_GNAV_CORRIDOR_MAX_LEN,
    .link_v                 = PARAM_GNAV_LINK_V,
    .link_distance          = PARAM_GNAV_LINK_DISTANCE,
    .link_timeout_ms        = PARAM_GNAV_LINK_TIMEOUT,
    .link_gap_runout        = PARAM_GNAV_LINK_GAP_RUNOUT,
    .link_wall_target       = PARAM_GNAV_LINK_WALL_TARGET,
    .link_wall_kp           = PARAM_GNAV_LINK_WALL_KP,
    .link_wall_blend        = PARAM_GNAV_LINK_WALL_BLEND,
    .exit_v                 = PARAM_GNAV_EXIT_V,
    .exit_runout            = PARAM_GNAV_EXIT_RUNOUT,
    .exit_max_dist          = PARAM_GNAV_EXIT_MAX_DIST,
    .exit_timeout_ms        = PARAM_GNAV_EXIT_TIMEOUT,
    .dock_v                 = PARAM_GNAV_DOCK_V,
    .dock_distance          = PARAM_GNAV_DOCK_DISTANCE,
    .heading_kp             = PARAM_GNAV_HEADING_KP,
    .corridor_width         = PARAM_CORRIDOR_WIDTH,
  };
  GlobalNav_Init(&gnav_cfg);

#else
  /* --- 初始化段脚本执行器 (单沟测试模式) --- */
  NavScriptConfig_t script_cfg = {
    .turn_target_angle    = 3.14159265f,                 /* 固定：180度转向 */
    .turn_omega           = PARAM_SCRIPT_TURN_OMEGA,     /* 调优：转向角速度 */
    .corridor_length      = 3.0f,                        /* 备用：垄沟长度估计 */
    .entry_align_timeout  = PARAM_SCRIPT_ENTRY_TIMEOUT,  /* 调优：入口对准超时 */
    .d_entry_exit_front   = 0.12f,                       /* 调优：出入口距离阈值 */
    .entry_align_v        = PARAM_SCRIPT_ENTRY_V,        /* 调优：入口对准速度 */
    .exit_runout_m        = PARAM_SCRIPT_EXIT_RUNOUT,    /* 调优：退出后冲出距离 */
    .exit_v               = PARAM_SCRIPT_EXIT_V,         /* P1 修复：退出直线速度独立参数 */
  };
  NavScript_Init(&script_cfg);
#endif
}