#!/bin/bash
# ============================================================
# Robot AGV 全量启动脚本
# 完整流程：清理旧进程 -> 启动 bringup -> 启动 Nav2 -> 激活 Lifecycle -> 启动 Flask
# ============================================================
set -e

LOG_DIR="/home/elephant/work/agv_app"
cd "$LOG_DIR"

echo "=========================================="
echo "  Robot AGV 全量启动"
echo "=========================================="
echo ""

# ---------- 1. 清理旧进程 ----------
echo "[1/6] 清理旧进程..."
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 "python.*app.py" 2>/dev/null || true
pkill -f "ros2-daemon" 2>/dev/null || true
sleep 4
echo "   清理完成"

# ---------- 2. 重启 ros2 daemon ----------
echo "[2/6] 重启 ros2 daemon..."
source /opt/ros/humble/setup.bash
ros2 daemon stop 2>/dev/null || true
sleep 2
nohup bash -c "source /opt/ros/humble/setup.bash && ros2 daemon start" &>/dev/null &
sleep 5

# 检查 daemon 是否正常
if ! bash -c "source /opt/ros/humble/setup.bash && ros2 node list &>/dev/null"; then
    echo "   ⚠️ 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 &
    sleep 5
fi
echo "   ros2 daemon 已就绪"

# ---------- 3. 启动 bringup ----------
echo "[3/6] 启动 AGV Bringup (agv_pro + 激光雷达)..."
source /opt/ros/humble/setup.bash
cd /home/elephant/agv_pro_ros2
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"

# 等待 bringup 就绪（检查 /odom 话题出现）
echo "   等待 bringup 就绪..."
for i in $(seq 1 15); do
    if bash -c "source /opt/ros/humble/setup.bash && source install/setup.bash && ros2 topic list 2>/dev/null | grep -q '/odom'"; then
        echo "   ✅ bringup 已就绪 (/odom 话题已上线)"
        break
    fi
    sleep 2
done

# ---------- 4. 启动 Nav2 ----------
echo "[4/6] 启动 Nav2 导航..."
source /opt/ros/humble/setup.bash
cd /home/elephant/agv_pro_ros2
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

# ---------- 5. 等待并激活 Nav2 Lifecycle ----------
echo "[5/6] 等待 Nav2 Lifecycle Manager 就绪..."
source /opt/ros/humble/setup.bash
cd /home/elephant/agv_pro_ros2
source install/setup.bash

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

# 检查是否已 Active（autostart=True 应该自动激活）
echo "   检查 Lifecycle 状态..."
LIFECYCLE_STATE=$(ros2 lifecycle list /lifecycle_manager_navigation 2>/dev/null | grep "Active\|Inactive" | head -1 || echo "")
if echo "$LIFECYCLE_STATE" | grep -q "Active"; then
    echo "   ✅ Nav2 Lifecycle 已激活 (autostart=True 生效)"
else
    echo "   ⚠️ Lifecycle 未激活，手动 configure + activate..."
    ros2 lifecycle set /lifecycle_manager_navigation configure 2>/dev/null || true
    sleep 3
    ros2 lifecycle set /lifecycle_manager_navigation activate 2>/dev/null || true
    sleep 3
    LIFECYCLE_STATE=$(ros2 lifecycle list /lifecycle_manager_navigation 2>/dev/null | grep "Active\|Inactive" | head -1 || echo "")
    if echo "$LIFECYCLE_STATE" | grep -q "Active"; then
        echo "   ✅ Lifecycle 手动激活成功"
    else
        echo "   ⚠️ Lifecycle 仍无法激活，继续（Nav2 action 可能仍可用）"
    fi
fi

# ---------- 6. 启动 Flask ----------
echo "[6/6] 启动 Flask API 服务..."
cd /home/elephant/work/agv_app
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 "  进程状态:"
echo "    bringup : $(ps aux | grep -w "$BRINGUP_PID" | grep -v grep | awk '{print $2}' || echo '已退出')"
echo "    Nav2    : $(ps aux | grep -w "$NAV2_PID" | grep -v grep | awk '{print $2}' || echo '已退出')"
echo "    Flask   : $(ps aux | grep -w "$FLASK_PID" | grep -v grep | awk '{print $2}' || echo '已退出')"
echo ""
echo "  日志文件:"
echo "    bringup : /tmp/ros2_bringup.log"
echo "    Nav2    : /tmp/ros2_nav2.log"
echo "    Flask   : /tmp/agv_flask.log"
echo ""
echo "  API 地址: http://localhost:5000"
echo ""
echo "  常用检查命令:"
echo "    curl http://localhost:5000/api/status"
echo "    curl http://localhost:5000/api/navigate/status"
echo "    curl -X POST http://localhost:5000/api/mission/init_pose"
echo "    curl -X POST http://localhost:5000/api/device/connect -H 'Content-Type: application/json' -d '{\"device\":\"agv\"}'"
echo "    curl -X POST http://localhost:5000/api/navigate/to -H 'Content-Type: application/json' -d '{\"x\":-0.249,\"y\":-0.957}'"
echo ""