#!/usr/bin/env bash
set -euo pipefail

script_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
repo_root="$(cd "${script_dir}/.." && pwd)"
port="${OPEN_SOURCE_DASHBOARD_PORT:-8090}"
map_dir="${GO2_OS_MAP_DIR:-${HOME}/go2_maps}"

# Source ROS2 环境（setup.bash 内部可能引用未绑定变量）
set +u
source /opt/ros/humble/setup.bash
if [[ -f "${HOME}/unitree_ros2/cyclonedds_ws/install/setup.bash" ]]; then
  source "${HOME}/unitree_ros2/cyclonedds_ws/install/setup.bash"
fi
if [[ -f "${repo_root}/install/setup.bash" ]]; then
  source "${repo_root}/install/setup.bash"
fi
set -u

# CycloneDDS 配置（必须和 Nav2 一致才能发现 /map /tf 等 topic）
ros_iface="${GO2_WIRED_IFACE:-ens160}"
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export CYCLONEDDS_URI="<CycloneDDS><Domain><General><Interfaces><NetworkInterface name=\"${ros_iface}\" priority=\"default\" multicast=\"default\" /></Interfaces></General></Domain></CycloneDDS>"

# 加载 dashboard 环境
dashboard_env="${repo_root}/go2_dashboard/config/dashboard.env"
if [[ -f "${dashboard_env}" ]]; then
  set -a; source "${dashboard_env}"; set +a
fi
runtime_env="${repo_root}/scripts/go2_runtime.env"
if [[ -f "${runtime_env}" ]]; then source "${runtime_env}"; fi

# 找最新地图
latest_map="$(ls -t "${map_dir}"/*.yaml 2>/dev/null | head -1 || true)"
if [[ -z "${latest_map}" ]]; then
  printf '[go2_restart_dashboard] ERROR: no map found in %s\n' "${map_dir}" >&2
  exit 1
fi
printf '[go2_restart_dashboard] latest map: %s\n' "${latest_map}"

# 停止旧进程
current_pid="$(pgrep -f 'go2_dashboard/server.py' | head -1 || true)"
if [[ -n "${current_pid}" ]]; then
  printf '[go2_restart_dashboard] stopping old dashboard (pid=%s)\n' "${current_pid}"
  kill "${current_pid}" 2>/dev/null || true
  sleep 1
  kill -0 "${current_pid}" 2>/dev/null && kill -9 "${current_pid}" 2>/dev/null || true
fi

# 启动
printf '[go2_restart_dashboard] starting dashboard with map: %s\n' "${latest_map}"
cd "${repo_root}"
mkdir -p "${repo_root}/log"

nohup python3 go2_dashboard/server.py \
  --host 0.0.0.0 \
  --port "${port}" \
  --repo-root "${repo_root}" \
  --map-yaml "${latest_map}" \
  --camera-iface "${GO2_CAMERA_IFACE:-ens160}" \
  --nav-log-dir "${repo_root}/log/go2_nav" \
  --pose-config "${repo_root}/go2_dashboard/config/pose_pipeline.json" \
  --waypoint-action-config "${repo_root}/go2_dashboard/config/waypoint_action.json" \
  > "${repo_root}/log/dashboard.log" 2>&1 &

new_pid=$!
sleep 2
if kill -0 "${new_pid}" 2>/dev/null; then
  dashboard_ip="$(hostname -I 2>/dev/null | awk '{print $1}')"
  printf '[go2_restart_dashboard] started (pid=%s)\n' "${new_pid}"
  printf '[go2_restart_dashboard] url: http://%s:%s\n' "${dashboard_ip}" "${port}"
else
  printf '[go2_restart_dashboard] ERROR: failed to start, check %s/log/dashboard.log\n' "${repo_root}" >&2
  tail -20 "${repo_root}/log/dashboard.log" >&2
  exit 1
fi