#!/bin/bash
###############################################################################
#   ros2_nodes.sh
#   一键启动 / 停止 / 重启 / 状态查询 13 个 ROS 2 节点（或仅核心 4 节点）
#   用法：
#     ./ros2_nodes.sh start           # 默认启动全部 13 节点
#     ./ros2_nodes.sh start all       # 同上
#     ./ros2_nodes.sh start core      # 仅启动 4 个核心节点
#     ./ros2_nodes.sh stop            # 停止
#     ./ros2_nodes.sh restart [core]  # 重启（可选 core / all）
#     ./ros2_nodes.sh status          # 查看状态
#     ./ros2_nodes.sh rotate          # 手动轮换日志
###############################################################################

##== 基本路径与环境 ===========================================================
WORKSPACE_PATH="/home/nvidia/sweeper_e800"
source "$WORKSPACE_PATH/install/setup.bash"

##== 日志目录与轮换配置 =======================================================
LOG_DIR="/var/log/ros2"
MAX_LOG_SIZE="10M"   # 单个日志大小
MAX_LOG_FILES=5      # 保留文件数

##== 节点命令列表 ==============================================================

# 1) 全量 13 节点（键名 = 节点标识 , 值 = 启动命令）
declare -A NODES_ALL=(
  # ---- 核心 4 节点 ----
  # ["mc"]="ros2 run mc mc_node"
  ["radio_ctrl"]="ros2 run radio_ctrl radio_ctrl_node"
  ["ctrl_arbiter"]="ros2 run ctrl_arbiter ctrl_arbiter_node"
  ["mqtt_report"]="ros2 run mqtt_report mqtt_report_node"

  # ---- 新增 9 节点 ----
  ["rslidar_sdk"]="ros2 launch rslidar_sdk start.py"
  ["rtk"]="ros2 run rtk rtk_node"
  ["route"]="ros2 run route route_node"
  ["sub"]="ros2 run sub sub_node"
  ["task_manager"]="ros2 run task_manager task_manager_node"
  ["detect_line"]="ros2 run detect_line detect_line_node"
  ["pl"]="ros2 run pl pl_node"
  ["fu"]="ros2 run fu fu_node"
  ["pub_gps"]="ros2 run pub_gps pub_gps_node"
)

# 2) 核心节点名称数组（便于 core 模式筛选）
CORE_NAMES=(mc radio_ctrl ctrl_arbiter mqtt_report)

##== 函数 =====================================================================

# 预启动环境准备：CAN、USB 权限、日志目录与 logrotate
setup_environment() {
  echo "[INFO] 设置 CAN0 波特率为 500K..."
  sudo ip link set can0 down 2>/dev/null
  sudo ip link set can0 type can bitrate 500000
  sudo ip link set can0 up
  echo "[INFO] CAN0 配置完成。"

  echo "[INFO] 设置 /dev/ttyUSB0 权限为 777..."
  sudo chmod 777 /dev/ttyUSB0
  echo "[INFO] ttyUSB0 权限设置完成。"

  echo "[INFO] 设置 /dev/ttyTHS1 权限为 777..."
  sudo chmod 777 /dev/ttyTHS1
  echo "[INFO] ttyTHS1 权限设置完成。"

  # 日志目录
  echo "[INFO] 创建日志目录: $LOG_DIR"
  sudo mkdir -p "$LOG_DIR"
  sudo chown -R nvidia:nvidia "$LOG_DIR"

  # logrotate 配置
  local LOGROTATE_CONF="/etc/logrotate.d/ros2_nodes"
  echo "[INFO] 配置 logrotate: $LOGROTATE_CONF"
  sudo bash -c "cat > $LOGROTATE_CONF" <<EOF
$LOG_DIR/*.log {
    su nvidia nvidia
    daily
    maxsize $MAX_LOG_SIZE
    rotate $MAX_LOG_FILES
    missingok
    notifempty
    compress
    delaycompress
    sharedscripts
    copytruncate
}
EOF
}

# 启动节点：start_nodes [core|all]
start_nodes() {
  local mode=${1:-all}            # 默认 all
  setup_environment

  # 选择节点集合
  local names=()
  if [[ "$mode" == "core" ]]; then
    names=("${CORE_NAMES[@]}")
  else
    names=("${!NODES_ALL[@]}")
  fi

  echo "[INFO] 启动模式: $mode"
  echo "[INFO] 节点列表: ${names[*]}"
  echo "[INFO] 每个节点启动间隔 0.1s"

  for name in "${names[@]}"; do
    echo "[START] $name"
    local log_file="${LOG_DIR}/${name}.log"

    #=====================================================================
    ${NODES_ALL[$name]} >> "$log_file" 2>&1 &
    # gnome-terminal --title="$name" \
    #   -- bash -c "source $WORKSPACE_PATH/install/setup.bash && ${NODES_ALL[$name]}; exec bash" &
    #=====================================================================

    local pid=$!
    echo $pid > "/tmp/$name.pid"
    echo "[INFO] $name PID: $pid | 日志: $log_file"

    sleep 0.1    # 每个节点之间暂停 0.1 秒
  done

  echo "[INFO] 所有节点已启动 (${mode})."
}

# 停止全部正在运行的节点
stop_nodes() {
  echo "[INFO] 停止 ROS 2 节点..."
  for pidfile in /tmp/*.pid; do
    [ -e "$pidfile" ] || continue
    local name=$(basename "$pidfile" .pid)
    local pid=$(cat "$pidfile")
    if ps -p "$pid" > /dev/null 2>&1; then
      kill -SIGINT "$pid" && echo "[STOP] $name (PID $pid) - 发送 SIGINT"
      local timeout=5
      while kill -0 "$pid" 2>/dev/null && [ $timeout -gt 0 ]; do
        sleep 0.5; ((timeout--))
      done
      if kill -0 "$pid" 2>/dev/null; then
        kill -9 "$pid" && echo "[STOP] $name (PID $pid) - 强制终止"
      fi
    fi
    rm -f "$pidfile"
  done
  echo "[INFO] 所有节点已停止。"
}

# 手动触发日志轮换
rotate_logs() {
  echo "[INFO] 手动轮换日志..."
  sudo logrotate -f /etc/logrotate.d/ros2_nodes
  echo "[INFO] 日志轮换完成。"
}

# 查看节点运行状态
show_status() {
  echo "[INFO] 节点状态:"
  for name in "${!NODES_ALL[@]}"; do
    if [ -f "/tmp/$name.pid" ]; then
      local pid=$(cat "/tmp/$name.pid")
      if ps -p "$pid" > /dev/null 2>&1; then
        echo "[RUNNING] $name (PID $pid)"
      else
        echo "[DEAD]    $name (PID $pid)"
      fi
    else
      echo "[STOPPED] $name"
    fi
  done
}

##== 主入口 ===================================================================

case "$1" in
  start)
    # 用法: ./ros2_nodes.sh start [core|all]
    start_nodes "${2:-all}"
    ;;
  stop)
    stop_nodes
    ;;
  restart)
    stop_nodes
    sleep 1
    start_nodes "${2:-all}"
    ;;
  rotate)
    rotate_logs
    ;;
  status)
    show_status
    ;;
  *)
    echo "Usage: $0 {start|stop|restart|rotate|status} [core|all]"
    echo "  core : 仅启动 4 个核心节点 (mc radio_ctrl ctrl_arbiter mqtt_report)"
    echo "  all  : 启动全部 13 个节点 (默认)"
    ;;
esac