#!/usr/bin/env bash
# 单终端一键：按需 roscore → MAVROS 串口(MAVLink) → px4_ctrl_offboard_demo.py
#
# 用法（在仓库根目录）：
#   bash scripts/run_px4_offboard_one_terminal.sh
#   bash scripts/run_px4_offboard_one_terminal.sh /dev/ttyACM0 921600
#   bash scripts/run_px4_offboard_one_terminal.sh /dev/ttyACM0 921600 120
#     第三参 DEMO_MAX_SEC：仅限制「px4_ctrl_offboard_demo.py」运行时长（秒），到时发 SIGTERM 结束
#     demo，随后脚本清理 MAVROS / 可选 roscore 并退出；0 或不写表示不限制。
#     也可用环境变量 OFFBOARD_DEMO_MAX_SEC（第三参优先）。
#
# 环境变量（可选）：
#   ROS_MASTER_URI   默认 http://127.0.0.1:11311
#   ROS_HOSTNAME     默认 127.0.0.1
#   OFFBOARD_PYTHON  默认优先用 conda env「yanshi」里的 python，否则 python3
#   OFFBOARD_DEMO_MAX_SEC  同第三参；整条脚本从头限时请用: timeout 600 bash 本脚本 ...
#   ROCKET_OFFBOARD_DEMO_PY  显式指定 px4_ctrl_offboard_demo.py 路径（缺省时先 $ROOT/src/ 再 $ROOT/../src/）
#
# 退出：Ctrl+C 会结束 demo，并停止本脚本拉起的 MAVROS；若 roscore 由本脚本启动，会一并结束。

set -euo pipefail

SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
ROOT="$(cd "$SCRIPT_DIR/.." && pwd)"
cd "$ROOT"

resolve_demo_py() {
  if [[ -n "${ROCKET_OFFBOARD_DEMO_PY:-}" ]] && [[ -f "${ROCKET_OFFBOARD_DEMO_PY}" ]]; then
    echo "$(cd "$(dirname "${ROCKET_OFFBOARD_DEMO_PY}")" && pwd)/$(basename "${ROCKET_OFFBOARD_DEMO_PY}")"
    return
  fi
  local c1="$ROOT/src/px4_ctrl_offboard_demo.py"
  if [[ -f "$c1" ]]; then
    echo "$c1"
    return
  fi
  local c2="$ROOT/../src/px4_ctrl_offboard_demo.py"
  if [[ -f "$c2" ]]; then
    echo "$(cd "$(dirname "$c2")" && pwd)/px4_ctrl_offboard_demo.py"
    return
  fi
  echo "错误: 找不到 px4_ctrl_offboard_demo.py。已尝试:" >&2
  echo "  ROCKET_OFFBOARD_DEMO_PY=${ROCKET_OFFBOARD_DEMO_PY:-（未设置）}" >&2
  echo "  $c1" >&2
  echo "  $c2" >&2
  exit 3
}

DEMO_PY="$(resolve_demo_py)"

source /opt/ros/noetic/setup.bash
export ROS_MASTER_URI="${ROS_MASTER_URI:-http://127.0.0.1:11311}"
export ROS_HOSTNAME="${ROS_HOSTNAME:-127.0.0.1}"

DEV="${1:-/dev/ttyACM0}"
BAUD="${2:-921600}"
DEMO_MAX_SEC="${3:-${OFFBOARD_DEMO_MAX_SEC:-0}}"
FCU_URL="${DEV}:${BAUD}"

if ! [[ "$DEMO_MAX_SEC" =~ ^[0-9]+$ ]]; then
  echo "错误: 第三参（demo 最长运行秒数）须为非负整数，当前=${DEMO_MAX_SEC}"
  exit 2
fi

ROSCORE_PID=""
MAVROS_PID=""
WE_STARTED_ROSCORE=0

master_ok() {
  timeout 3 rosnode list &>/dev/null
}

stop_mavros() {
  pkill -f '/opt/ros/noetic/lib/mavros/mavros_node' 2>/dev/null || true
}

kill_children() {
  if [[ -n "${MAVROS_PID:-}" ]] && kill -0 "$MAVROS_PID" 2>/dev/null; then
    kill "$MAVROS_PID" 2>/dev/null || true
    wait "$MAVROS_PID" 2>/dev/null || true
  fi
  if [[ "${WE_STARTED_ROSCORE}" -eq 1 ]] && [[ -n "${ROSCORE_PID:-}" ]] && kill -0 "$ROSCORE_PID" 2>/dev/null; then
    kill "$ROSCORE_PID" 2>/dev/null || true
    wait "$ROSCORE_PID" 2>/dev/null || true
  fi
}

trap 'kill_children; exit 130' INT
trap 'kill_children; exit 143' TERM

resolve_python() {
  if [[ -n "${OFFBOARD_PYTHON:-}" ]] && [[ -x "$OFFBOARD_PYTHON" ]]; then
    echo "$OFFBOARD_PYTHON"
    return
  fi
  for cand in \
    "${HOME}/miniconda3/envs/yanshi/bin/python" \
    "${HOME}/anaconda3/envs/yanshi/bin/python" \
    "${HOME}/mambaforge/envs/yanshi/bin/python"; do
    if [[ -x "$cand" ]]; then
      echo "$cand"
      return
    fi
  done
  command -v python3
}

if ! master_ok; then
  echo "[1/3] 启动 roscore ..."
  roscore > /tmp/roscore_offboard_one_terminal.log 2>&1 &
  ROSCORE_PID=$!
  WE_STARTED_ROSCORE=1
  for _ in $(seq 1 50); do
    master_ok && break
    sleep 0.2
  done
  if ! master_ok; then
    echo "roscore 未起来，请查看: tail -40 /tmp/roscore_offboard_one_terminal.log"
    kill_children
    exit 1
  fi
  echo "roscore 已就绪 (pid=$ROSCORE_PID)"
else
  echo "[1/3] 已有 ROS master，跳过 roscore"
fi

echo "[2/3] 启动 MAVROS  fcu_url=${FCU_URL}"
stop_mavros
sleep 1
roslaunch mavros px4.launch fcu_url:="$FCU_URL" > /tmp/mavros_offboard_one_terminal.log 2>&1 &
MAVROS_PID=$!

echo "等待飞控连接 (connected=true)，超时 60s ..."
connected=0
for _ in $(seq 1 60); do
  if timeout 4 rostopic echo /mavros/state -n 1 2>/dev/null | grep -qE 'connected: [Tt]rue'; then
    connected=1
    break
  fi
  sleep 1
done
if [[ "$connected" -ne 1 ]]; then
  echo "仍未连上飞控。请检查串口与波特率，日志: tail -60 /tmp/mavros_offboard_one_terminal.log"
  echo "可重试: bash $0 ${DEV} 57600"
  kill_children
  exit 1
fi
echo "MAVROS 已连接飞控"

PY="$(resolve_python)"
echo "[3/3] 使用 Python: $PY"
if [[ "$DEMO_MAX_SEC" -gt 0 ]]; then
  echo "运行 $DEMO_PY，${DEMO_MAX_SEC}s 后自动结束 demo 并退出本脚本（随后清理子进程）"
else
  echo "运行 $DEMO_PY （Ctrl+C 结束并将停止本脚本启动的 MAVROS）"
fi
set +e
if [[ "$DEMO_MAX_SEC" -gt 0 ]]; then
  timeout --signal=TERM --kill-after=8 "$DEMO_MAX_SEC" \
    "$PY" "$DEMO_PY"
  DEMO_EXIT=$?
  if [[ "$DEMO_EXIT" -eq 124 ]]; then
    echo "[timeout] 已到 ${DEMO_MAX_SEC}s，已终止 px4_ctrl_offboard_demo.py（退出码 124）"
  fi
else
  "$PY" "$DEMO_PY"
  DEMO_EXIT=$?
fi
set -e

kill_children
exit "$DEMO_EXIT"