#!/bin/bash
# ============================================================
# Robot AGV 全量启动脚本 v5.0
# 修复：
#   - v5.0: 使用公共库重构，减少代码重复
#   - v4.0: 彻底杀死 ros2 daemon 进程 + 启动前进程数量检查
#   - v3.0: 彻底清理 FastRTPS 共享内存文件（永久修复 DDS 通信问题）
#   - v2.7: 添加 ROS_DOMAIN_ID 环境变量传递
#   - v2.6: 清理 scan_fixer lock 文件防残留
# ============================================================
set -euo pipefail

# 加载公共库
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
source "$SCRIPT_DIR/ros-common.sh"

# ============================================================================
# 主流程
# ============================================================================

section "Robot AGV 全量启动 v5.0"

# ============================================================================
# 1. 清理旧环境
# ============================================================================
step "1/8" "清理旧进程和共享内存"

kill_all_soft
kill_all_hard
stop_ros2_daemon
cleanup_fastrtps

# 验证进程已停止
echo "   验证进程停止..."
PROC_COUNT=$(count_residual_processes)
echo "   残留进程数: $PROC_COUNT"
if [ "$PROC_COUNT" -gt 0 ]; then
    echo "   [WARN] 仍有进程残留，再次强制终止..."
    kill_all_hard
    PROC_COUNT=$(count_residual_processes)
    echo "   清理后残留: $PROC_COUNT"
fi
info "ok" "清理完成"

# ============================================================================
# 2. 启动 ros2 daemon
# ============================================================================
step "2/8" "启动 ros2 daemon"
source "$ROS_SETUP" 2>/dev/null || true
rm -rf "$FASTRTPS_SHM_DIR"/fastrtps_* 2>/dev/null || true
start_ros2_daemon || true

# ============================================================================
# 3. 启动 bringup (含激光雷达)
# ============================================================================
step "3/8" "启动 AGV Bringup"
cd "$AGV_ROS2_DIR"
source "$ROS_WORKSPACE_SETUP"

rm -rf "$FASTRTPS_SHM_DIR"/fastrtps_* 2>/dev/null || true

nohup bash -c "export ROS_DOMAIN_ID=$ROS_DOMAIN_ID && \
    ros2 launch agv_pro_bringup agv_pro_bringup.launch.py port_name:=$AGV_CONTROLLER_DEVICE" \
    > "$BRINGUP_LOG" 2>&1 &
BRINGUP_PID=$!
echo "   bringup PID: $BRINGUP_PID"

# 等待 /odom 话题
wait_for_topic /odom 40 || show_log_tail "$BRINGUP_LOG"

# ============================================================================
# 4. 启动系统时钟发布器
# ============================================================================
step "4/8" "启动系统时钟发布器 (clock_publisher)"

nohup bash -c "source \"$ROS_SETUP\" && \
    ROS_DOMAIN_ID=$ROS_DOMAIN_ID python3 \"$SCAN_FIXER_DIR/clock_publisher.py\"" \
    > "$CLOCK_LOG" 2>&1 &
CLOCK_PID=$!
echo "   clock_publisher PID: $CLOCK_PID"
sleep 2

wait_for_topic /clock 10 || show_log_tail "$CLOCK_LOG"

# ============================================================================
# 5. 启动激光时间戳修正节点
# ============================================================================
step "5/8" "启动激光时间戳修正节点"

# 先等待 /scan 话题
if ! wait_for_topic /scan 20; then
    echo "   [WARN] /scan 未上线，检查 bringup 日志"
fi

nohup bash -c "source \"$ROS_SETUP\" && \
    ROS_DOMAIN_ID=$ROS_DOMAIN_ID python3 \"$SCAN_FIXER_DIR/fix_scan_timestamp_v6.py\"" \
    > "$SCAN_FIXER_LOG" 2>&1 &
FIXER_PID=$!
echo "   fix_scan_timestamp PID: $FIXER_PID"
sleep 5

# 检查是否有多个 fixer 进程
FIXER_COUNT=$(count_matching_processes "fix_scan_timestamp")
if [ "$FIXER_COUNT" -gt 1 ]; then
    echo "   [WARN] 发现 $FIXER_COUNT 个 fixer 进程，重启..."
    pkill -f "fix_scan_timestamp" 2>/dev/null || true
    pkill -f "clock_publisher" 2>/dev/null || true
    sleep 2
    rm -f /tmp/scan_fixer.lock
    nohup bash -c "source \"$ROS_SETUP\" && \
        ROS_DOMAIN_ID=$ROS_DOMAIN_ID python3 \"$SCAN_FIXER_DIR/fix_scan_timestamp_v6.py\"" \
        > "$SCAN_FIXER_LOG" 2>&1 &
    FIXER_PID=$!
    sleep 3
fi

wait_for_topic /scan_corrected 15 || show_log_tail "$SCAN_FIXER_LOG"

# ============================================================================
# 6. 启动 Nav2
# ============================================================================
step "6/8" "启动 Nav2 导航"

nohup bash -c "source \"$ROS_SETUP\" && source \"$ROS_WORKSPACE_SETUP\" && \
    export ROS_DOMAIN_ID=$ROS_DOMAIN_ID && \
    ros2 launch agv_pro_navigation2 navigation2_active.launch.py autostart:=True" \
    > "$NAV2_LOG" 2>&1 &
NAV2_PID=$!
echo "   Nav2 PID: $NAV2_PID"
sleep 12

echo "   等待 Nav2 节点就绪..."
wait_for_nodes 'lifecycle_manager_navigation|bt_navigator|controller_server' 3 45 || true

# ============================================================================
# 7. 设置精度参数
# ============================================================================
step "7/8" "设置导航精度参数 (xy_goal_tolerance=0.05m)"

for NODE in /controller_server /bt_navigator /planner_server; do
    ros2_exec timeout 1 ros2 param set $NODE general_goal_checker.xy_goal_tolerance 0.05 2>/dev/null || true
    ros2_exec timeout 1 ros2 param set $NODE general_goal_checker.yaw_goal_tolerance 0.05 2>/dev/null || true
done
ros2_exec timeout 1 ros2 param set /controller_server FollowPath.xy_goal_tolerance 0.05 2>/dev/null || true
ros2_exec timeout 1 ros2 param set /controller_server general_goal_checker.stateful True 2>/dev/null || true
ros2_exec timeout 1 ros2 param set /controller_server FollowPath.stateful True 2>/dev/null || true

info "ok" "精度参数已设置"

# ============================================================================
# 8. 启动 Flask API
# ============================================================================
step "8/8" "启动 Flask API"

cd "$AGV_APP_DIR"
nohup uv run --locked python app.py > "$FLASK_LOG" 2>&1 &
FLASK_PID=$!
echo "   Flask PID: $FLASK_PID"
sleep 4

# ============================================================================
# 9. 最终验证
# ============================================================================
section "系统全面验证"

# 验证话题数量
echo ""
echo "验证 ros2 topic list..."
TOPIC_COUNT=$(ros2_topic_count 5)
echo "   话题数量: $TOPIC_COUNT"
if [ "$TOPIC_COUNT" -gt 10 ]; then
    info "ok" "ros2 daemon 正常 (${TOPIC_COUNT} 个话题)"
else
    info "err" "ros2 topic list 异常 (${TOPIC_COUNT} 个话题，可能 DDS 有问题)"
    echo "   手动执行: rm -rf \"$FASTRTPS_SHM_DIR\"/fastrtps_* && ros2 daemon stop && ros2 daemon start"
fi

# 验证关键话题
echo ""
echo "验证关键话题..."
for TOPIC in /odom /scan /cmd_vel /tf /clock /scan_corrected; do
    if topic_exists "$TOPIC"; then
        info "ok" "$TOPIC"
    else
        info "warn" "$TOPIC 未找到"
    fi
done

# 验证进程数量
echo ""
echo "验证进程数量..."
BRINGUP_PROCS=$(count_matching_processes 'agv_pro_node|lslidar_driver_node')
echo "   AGV 核心进程: $BRINGUP_PROCS (应为 2)"
if [ "$BRINGUP_PROCS" -eq 2 ]; then
    info "ok" "进程数量正常（无重复）"
elif [ "$BRINGUP_PROCS" -gt 2 ]; then
    info "warn" "发现 $BRINGUP_PROCS 个核心进程（可能有残留），建议重启"
else
    info "warn" "进程数量异常"
fi

# FastRTPS 共享内存状态
echo ""
echo "FastRTPS 共享内存状态:"
FASTRTPS_NEW=$(count_fastrtps_files)
echo "   当前文件数: $FASTRTPS_NEW (正常运行时会有一些)"

# Flask API 状态
echo ""
echo "验证 Flask API..."
if pgrep -f "app.py" >/dev/null 2>&1; then
    info "ok" "Flask 进程运行中"
else
    info "err" "Flask 未运行"
fi

# ============================================================================
# 完成
# ============================================================================
section "[OK] 启动完成"
echo ""
echo "  进程状态:"
for proc_info in "bringup:$BRINGUP_PID" "Nav2:$NAV2_PID" "fixer:$FIXER_PID" "Flask:$FLASK_PID"; do
    name="${proc_info%%:*}"
    pid="${proc_info##*:}"
    if ps aux | grep -w "$pid" | grep -v grep >/dev/null 2>&1; then
        echo "    $name : 运行中 (PID: $pid)"
    else
        echo "    $name : 已退出"
    fi
done
echo ""
echo "  日志文件:"
echo "    bringup  : $BRINGUP_LOG"
echo "    Nav2     : $NAV2_LOG"
echo "    fixer    : $SCAN_FIXER_LOG"
echo "    Flask    : $FLASK_LOG"
echo ""
echo "  如果仍有问题，请依次执行："
echo "    1. ./scripts/stop_all.sh"
echo "    2. rm -rf \"$FASTRTPS_SHM_DIR\"/fastrtps_*"
echo "    3. ./scripts/prod-backend.sh"
echo ""