#!/bin/bash
# ============================================================
# Robot AGV 全量启动脚本 v2.2
# 完整流程：
#   清理旧进程（不杀 daemon） -> 启动 bringup ->
#   启动激光时间戳修正节点 -> 启动 Nav2 ->
#   设置导航精度参数 -> 启动 Flask
# ============================================================
set -e

AGV_APP_DIR="/home/elephant/work/agv_app"
AGV_ROS2_DIR="/home/elephant/agv_pro_ros2"
ROS_DOMAIN_ID_VAL=1

echo "=========================================="
echo "  Robot AGV 全量启动 v2.2"
echo "=========================================="
echo ""

# ---------- 1. 清理旧进程（不杀 ros2-daemon） ----------
echo "[1/7] 清理旧进程..."
pkill -f "ros2 launch agv_pro_bringup" 2>/dev/null || true
pkill -f "ros2 launch agv_pro_navigation2" 2>/dev/null || true
pkill -f "agv_pro_node" 2>/dev/null || true
pkill -f "lslidar_driver_node" 2>/dev/null || true
pkill -f "scan_timestamp_fixer" 2>/dev/null || true
pkill -f "python.*app.py" 2>/dev/null || true
sleep 4
echo "   清理完成"

# ---------- 2. 重启 ros2 daemon（仅杀 daemon进程本身，不杀整个环境） ----------
echo "[2/7] 重启 ros2 daemon..."
pkill -f "ros2-daemon" 2>/dev/null || true
sleep 2
nohup bash -c "source /opt/ros/humble/setup.bash && ros2 daemon start" >/dev/null 2>&1 &
sleep 5
echo "   ros2 daemon 已就绪"

# ---------- 3. 启动 bringup (含激光雷达) ----------
echo "[3/7] 启动 AGV Bringup..."
source /opt/ros/humble/setup.bash
cd "$AGV_ROS2_DIR"
source install/setup.bash
nohup ros2 launch agv_pro_bringup agv_pro_bringup.launch.py \
    port_name:=/dev/agvpro_controller > /tmp/ros2_bringup.log 2>&1 &
BRINGUP_PID=$!
echo "   bringup PID: $BRINGUP_PID"

echo "   等待 bringup 就绪..."
for i in $(seq 1 20); do
    if ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 topic list 2>/dev/null | grep -q '/odom'; then
        echo "   ✅ bringup 已就绪"
        break
    fi
    sleep 2
done

# ---------- 4. 启动激光时间戳修正节点（单例，不重复启动） ----------
echo "[4/7] 启动激光时间戳修正节点..."
# 确保只有1个 fixer 进程在运行
pkill -f "scan_timestamp_fixer" 2>/dev/null || true
sleep 2

for i in $(seq 1 10); do
    if ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 topic list 2>/dev/null | grep -q '/scan'; then
        echo "   /scan 话题已上线"
        break
    fi
    sleep 2
done

nohup bash -c "source /opt/ros/humble/setup.bash && \
    ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL python3 /home/elephant/work/scan_fixer/fix_scan_timestamp.py" \
    > /tmp/scan_fixer.log 2>&1 &
FIXER_PID=$!
echo "   scan_timestamp_fixer PID: $FIXER_PID"
sleep 5

# 验证只有1个 fixer 进程
FIXER_COUNT=$(ps aux | grep -c "[f]ix_scan_timestamp" 2>/dev/null || echo 0)
if [ "$FIXER_COUNT" -gt 1 ]; then
    echo "   ⚠️ 发现 $FIXER_COUNT 个 fixer 进程，杀掉多余的..."
    pkill -f "scan_timestamp_fixer" 2>/dev/null || true
    sleep 2
    nohup bash -c "source /opt/ros/humble/setup.bash && \
        ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL python3 /home/elephant/work/scan_fixer/fix_scan_timestamp.py" \
        > /tmp/scan_fixer.log 2>&1 &
    sleep 3
fi

if ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 topic list 2>/dev/null | grep -q '/scan_corrected'; then
    echo "   ✅ /scan_corrected 已上线"
else
    echo "   ⚠️ /scan_corrected 未上线，检查日志："
    cat /tmp/scan_fixer.log
fi

# ---------- 5. 启动 Nav2 ----------
echo "[5/7] 启动 Nav2 导航..."
source /opt/ros/humble/setup.bash
cd "$AGV_ROS2_DIR"
source install/setup.bash
nohup ros2 launch agv_pro_navigation2 navigation2_active.launch.py \
    autostart:=True > /tmp/ros2_nav2.log 2>&1 &
NAV2_PID=$!
echo "   Nav2 PID: $NAV2_PID"
sleep 12

echo "   等待 Nav2 节点就绪..."
for i in $(seq 1 15); do
    NODES=$(ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 node list 2>/dev/null | \
        grep -c "lifecycle_manager_navigation\|bt_navigator\|controller_server" 2>/dev/null || echo 0)
    if [ "$NODES" -ge 3 ]; then
        echo "   ✅ Nav2 节点已就绪 ($NODES 个)"
        break
    fi
    sleep 3
done

# ---------- 6. 设置精度参数 ----------
echo "[6/7] 设置导航精度参数 (xy_goal_tolerance=0.05m)..."
source /opt/ros/humble/setup.bash
cd "$AGV_ROS2_DIR"
source install/setup.bash

for NODE in /controller_server /bt_navigator /planner_server; do
    ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 param set $NODE general_goal_checker.xy_goal_tolerance 0.05 2>/dev/null || true
    ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 param set $NODE general_goal_checker.yaw_goal_tolerance 0.05 2>/dev/null || true
done
ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 param set /controller_server FollowPath.xy_goal_tolerance 0.05 2>/dev/null || true
ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 param set /controller_server general_goal_checker.stateful True 2>/dev/null || true
ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 param set /controller_server FollowPath.stateful True 2>/dev/null || true
echo "   精度参数已设置"

# ---------- 7. 启动 Flask ----------
echo "[7/7] 启动 Flask API..."
cd "$AGV_APP_DIR"
nohup python3 app.py > /tmp/agv_flask.log 2>&1 &
FLASK_PID=$!
echo "   Flask PID: $FLASK_PID"
sleep 4

# ---------- 完成 ----------
echo ""
echo "=========================================="
echo "  ✅ 启动完成"
echo "=========================================="
echo ""
echo "  进程状态:"
for PROC in "bringup:$BRINGUP_PID" "Nav2:$NAV2_PID" "fixer:$FIXER_PID" "Flask:$FLASK_PID"; do
    NAME="${PROC%%:*}"
    PID="${PROC##*:}"
    echo "    $NAME : $(ps aux | grep -w "$PID" | grep -v grep | awk '{print $2}' || echo '已退出')"
done
echo ""
echo "  日志文件:"
echo "    bringup  : /tmp/ros2_bringup.log"
echo "    Nav2     : /tmp/ros2_nav2.log"
echo "    fixer    : /tmp/scan_fixer.log"
echo "    Flask    : /tmp/agv_flask.log"
echo ""
echo "  关键验证命令:"
echo "    curl http://localhost:5000/api/navigate/status"
echo "    ROS_DOMAIN_ID=1 ros2 topic echo /scan_corrected --once"
echo "    ROS_DOMAIN_ID=1 ros2 topic echo /amcl_pose --once"