#!/usr/bin/env bash
# 一键：roscore（若还没有）→ MAVROS（串口连 PX4）→ flight_intent 伴飞桥（前台，直到 Ctrl+C）
#
# 适合「不想自己敲 roslaunch」时直接跑；用法与 run_px4_offboard_one_terminal.sh 前两参一致。
#
# 在 voice_drone_assistant 根目录：
#   bash scripts/run_flight_bridge_with_mavros.sh
#   bash scripts/run_flight_bridge_with_mavros.sh /dev/ttyACM0 921600
#
# 飞控连上后，另开终端发 JSON（std_msgs/String 必须用 YAML 字段 data，见 docs/FLIGHT_BRIDGE_ROS1.md）：
#   source /opt/ros/noetic/setup.bash
#   rostopic pub -1 /input std_msgs/String \
#     "data: '{\"is_flight_intent\":true,\"version\":1,\"actions\":[{\"type\":\"land\",\"args\":{}}],\"summary\":\"降\"}'"
#
# 环境变量（可选）：
#   ROS_MASTER_URI     默认 http://127.0.0.1:11311
#   ROS_HOSTNAME       默认 127.0.0.1
#   BRIDGE_PYTHON      若不设则 python3（与 mavros 同机即可，不必 conda）
#   OFFBOARD_PYTHON    未设 BRIDGE_PYTHON 时也试 yanshi 环境（与 offboard 脚本习惯一致）
#
# Ctrl+C：结束伴飞桥，并停止本脚本拉起的 MAVROS；若 roscore 由本脚本启动则一并结束。

set -euo pipefail

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

if [[ ! -f /opt/ros/noetic/setup.bash ]]; then
  echo "未找到 /opt/ros/noetic/setup.bash（伴飞桥当前仅支持 ROS1 Noetic）" >&2
  exit 2
fi

# shellcheck source=/dev/null
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}"
FCU_URL="${DEV}:${BAUD}"

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 "${BRIDGE_PYTHON:-}" ]] && [[ -x "$BRIDGE_PYTHON" ]]; then
    echo "$BRIDGE_PYTHON"
    return
  fi
  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
}

echo "===== flight_intent 伴飞桥（含 MAVROS）fcu_url=${FCU_URL} ====="

if ! master_ok; then
  echo "[1/3] 启动 roscore …"
  roscore > /tmp/roscore_flight_bridge.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_flight_bridge.log" >&2
    kill_children
    exit 1
  fi
  echo "roscore 已就绪 (pid=$ROSCORE_PID)"
else
  echo "[1/3] 已有 ROS master，跳过 roscore"
fi

echo "[2/3] 启动 MAVROS …"
stop_mavros
sleep 1
roslaunch mavros px4.launch fcu_url:="$FCU_URL" > /tmp/mavros_flight_bridge.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_flight_bridge.log" >&2
  echo "可重试: bash $0 ${DEV} 57600" >&2
  kill_children
  exit 1
fi
echo "MAVROS 已连接飞控"

PY="$(resolve_python)"
export PYTHONPATH="${PYTHONPATH:-}:${ROOT}"

echo "[3/3] 启动伴飞桥（Python: $PY）…"
echo "  Ctrl+C 退出并清理本脚本启动的 MAVROS$([[ "$WE_STARTED_ROSCORE" -eq 1 ]] && echo +roscore)。"
echo "  发意图: 另开终端 source noetic 后 rostopic pub ... 见脚本头注释。"
set +e
"$PY" -m voice_drone.flight_bridge.ros1_node
BRIDGE_EXIT=$?
set -e

kill_children
exit "$BRIDGE_EXIT"