#!/usr/bin/env bash
# =============================================================================
# 创建 / 修复 Gazebo 仿真用的独立 conda 环境。
#
# 为什么单独一个环境，而不是往 ros_flexiv 里装：
#     ros_flexiv 是**能跑真机**的那一套。仿真需要往环境里加 Gazebo 及其一大堆
#     依赖（protobuf / ignition / 渲染栈），和真机环境混装随时可能把它弄坏。
#     真机环境坏了，代价远大于仿真带来的便利。所以两者物理隔离。
#
# 用法:
#     bash scripts/install_sim_env.sh          # 创建或补齐（幂等，可重复跑）
#     bash scripts/install_sim_env.sh --verify # 只验证现有环境，不安装
#
# 装完用 scripts/sim_env.sh 激活，用 scripts/run_sim_gz.sh 启动。
# =============================================================================
set -euo pipefail

ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
ENV_NAME="${SIM_ENV_NAME:-ros_gz_sim}"
CONDA_SH="${HOME}/anaconda3/etc/profile.d/conda.sh"

# 频道组合照抄现有 ros_flexiv 环境的安装记录，那是已经证明可用的搭配。
CHANNELS=(-c robostack-staging -c conda-forge)

ok()   { printf '  \033[32m✓\033[0m %s\n' "$1"; }
info() { printf '  \033[36m·\033[0m %s\n' "$1"; }
die()  { printf '  \033[31m✗ %s\033[0m\n' "$1" >&2; exit 1; }

if [[ ! -f "${CONDA_SH}" ]]; then
  die "找不到 conda: ${CONDA_SH}（本脚本假定 anaconda 装在 ~/anaconda3）"
fi
# shellcheck disable=SC1090
source "${CONDA_SH}"

# --- 验证 -------------------------------------------------------------------
verify() {
  info "激活 ${ENV_NAME} 并自检…"
  conda activate "${ENV_NAME}" 2>/dev/null || die "激活失败，环境可能没建好"

  echo
  echo "1) 仿真器本体"
  # 注意：Humble 配的是 Gazebo Fortress，命令行是 `ign gazebo`，不是 `gz sim`。
  # 现代包名 gz-sim 在 conda-forge 上要 libprotobuf>=6.31，而 RoboStack 的
  # Humble 把 protobuf 钉在 5.29.3，混装会解不开依赖。Fortress 是官方配对版本。
  ign gazebo --version 2>&1 | head -1 | sed 's/^/     /'

  echo
  echo "2) ROS 侧桥接与力控"
  local missing=0 p
  for p in ros_gz_sim ros_gz_bridge gz_ros2_control controller_manager ros2_control xacro; do
    if ros2 pkg prefix "$p" >/dev/null 2>&1; then
      ok "$p"
    else
      printf '     \033[31m✗ %s 缺失\033[0m\n' "$p"; missing=1
    fi
  done

  echo
  echo "3) 构建工具链（Humble 的 rosidl 需要 empy 3.x）"
  local empy_ver
  empy_ver="$(python -c 'import em; print(em.__version__)' 2>/dev/null || echo '?')"
  if [[ "${empy_ver}" == 3.* ]]; then
    ok "empy ${empy_ver}"
  else
    printf '     \033[31m✗ empy %s —— 必须是 3.x，否则 rosidl 生成接口时会报\n' "${empy_ver}"
    printf '       "AttributeError: NoneType object has no attribute shutdown"\033[0m\n'
    missing=1
  fi

  echo
  echo "4) 仿真能真跑起来吗（无头空跑 30 步）"
  local world
  world="$(find "${CONDA_PREFIX}/share" -name empty.sdf -path '*gazebo*' 2>/dev/null | head -1)"
  if [[ -n "${world}" ]] && timeout 60 ign gazebo -s -r --iterations 30 "${world}" >/dev/null 2>&1; then
    ok "仿真引擎正常启动"
  else
    printf '     \033[31m✗ 仿真引擎启动失败\033[0m\n'; missing=1
  fi

  echo
  if [[ "${missing}" -eq 0 ]]; then
    echo "环境就绪。下一步:"
    echo "    source scripts/sim_env.sh"
    echo "    bash scripts/build_sim.sh"
    echo "    bash scripts/run_sim_gz.sh"
  else
    echo "环境不完整，重跑 bash scripts/install_sim_env.sh 补齐。"
    return 1
  fi
}

if [[ "${1:-}" == "--verify" ]]; then
  verify
  exit $?
fi

# --- 安装 -------------------------------------------------------------------
echo "=== 1/4 创建环境 ${ENV_NAME} ==="
if conda env list | awk '{print $1}' | grep -qx "${ENV_NAME}"; then
  ok "已存在，跳过创建"
else
  conda create -y -n "${ENV_NAME}" python=3.11 pip || die "创建环境失败"
  ok "已创建"
fi

echo
echo "=== 2/4 安装 ROS 2 Humble 仿真栈 ==="
info "ros-humble-ros-gz 会一并带入 Gazebo Fortress（ignition-gazebo6）"
conda install -y -n "${ENV_NAME}" "${CHANNELS[@]}" \
  ros-humble-ros-base \
  ros-humble-ros-gz \
  ros-humble-gz-ros2-control \
  ros-humble-ros2-control \
  ros-humble-ros2-controllers \
  ros-humble-controller-manager \
  ros-humble-robot-state-publisher \
  ros-humble-joint-state-publisher \
  ros-humble-joint-state-publisher-gui \
  ros-humble-xacro \
  ros-humble-tf2-ros \
  ros-humble-camera-info-manager \
  ros-humble-image-transport-plugins \
  compilers cmake make pkg-config catkin_pkg lark-parser numpy \
  colcon-common-extensions || die "ROS 仿真栈安装失败"

echo
echo "=== 3/4 钉住 empy 3.x（Humble 的硬性要求）==="
# 坑：conda 默认会给 empy 4.x，而 Humble 的 rosidl_adapter 用 empy 3.x 的 API。
# 症状是构建自定义消息包时报 '_interpreter.shutdown()' 的 AttributeError，
# 报错点在 rosidl_adapter 内部，看起来完全不像 empy 的问题。
conda install -y -n "${ENV_NAME}" "${CHANNELS[@]}" "empy=3.3.4" || die "empy 降级失败"
ok "empy 3.3.4"

echo
echo "=== 4/4 自检 ==="
verify