#!/bin/bash
# ============================================================
# Robot AGV 全量启动脚本 v2.6
# 修复：
#   - 清理 scan_fixer lock 文件防残留
#   - Nav2 节点检测 grep -c 改为单行输出
#   - nohup 启动 Nav2 用 bash -c 包裹（确保 source 环境）
# ============================================================
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.6"
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 "fix_scan_timestamp" 2>/dev/null || true
pkill -f "python.*app.py" 2>/dev/null || true
sleep 4

# 清理 scan_fixer 锁文件（防残留 PID 导致启动失败）
rm -f /tmp/scan_fixer.lock

echo "   清理完成"

# ---------- 2. 重启 ros2 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] 启动激光时间戳修正节点..."
pkill -f "fix_scan_timestamp" 2>/dev/null || true
sleep 2

# 确保 /scan 存在
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 "   fix_scan_timestamp PID: $FIXER_PID"
sleep 5

# 验证 fixer 进程和 scan_corrected
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 "fix_scan_timestamp" 2>/dev/null || true
    sleep 2
    rm -f /tmp/scan_fixer.lock
    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=$!
    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 未上线，检查日志："
    tail -5 /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
# 使用 bash -c 确保 source 环境变量传递到 nohup
nohup bash -c "source /opt/ros/humble/setup.bash && \
    source /home/elephant/agv_pro_ros2/install/setup.bash && \
    export ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL && \
    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 20); do
    NODES=$(ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 node list 2>/dev/null | \
        grep -cE 'lifecycle_manager_navigation|bt_navigator|controller_server' 2>/dev/null || echo 0)
    # 去除可能的换行符，确保是单个数字
    NODES=$(echo "$NODES" | tr -d '\n' | awk '{print $1}')
    if [ "$NODES" -ge 3 ] 2>/dev/null; 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 timeout 1 ros2 param set $NODE general_goal_checker.xy_goal_tolerance 0.05 2>/dev/null || true
    ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL timeout 1 ros2 param set $NODE general_goal_checker.yaw_goal_tolerance 0.05 2>/dev/null || true
done
ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL timeout 1 ros2 param set /controller_server FollowPath.xy_goal_tolerance 0.05 2>/dev/null || true
ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL timeout 1 ros2 param set /controller_server general_goal_checker.stateful True 2>/dev/null || true
ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL timeout 1 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"