#!/usr/bin/env bash
# =============================================================================
# 激活**仿真**环境（Gazebo Fortress + 本工作区的仿真构建产物）。
#
# 用法: source scripts/sim_env.sh
#
# 与 scripts/env.sh（真机环境）的关系：
#     两者互斥，各自用一套 conda 环境 + 一套构建产物。
#       真机: ros_flexiv  → build/       install/
#       仿真: ros_gz_sim  → build_sim/   install_sim/
#     切换时**最好开新的 shell**。同一 shell 里来回切会残留 PATH/PYTHONPATH，
#     症状详见 scripts/lib/conda_switch.sh 顶部的说明（实测踩过）。
#     这里会先退到 base 再切以尽量规避，但开新 shell 才是稳妥做法。
#
# 为什么仿真用独立的 build/install 目录：
#     工作区只有一份源码。如果仿真也往 install/ 里写，就会把真机环境的构建
#     产物覆盖掉（两边的依赖路径不同）。分目录后互不干扰。
# =============================================================================
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"

SIM_ENV_NAME="${SIM_ENV_NAME:-ros_gz_sim}"
REAL_ENV_NAME="${REAL_ENV_NAME:-ros_flexiv}"
CONDA_SH="${HOME}/anaconda3/etc/profile.d/conda.sh"

# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/conda_switch.sh"

if [[ ! -f "${CONDA_SH}" ]]; then
  echo "找不到 conda: ${CONDA_SH}" >&2
  return 1 2>/dev/null || exit 1
fi

# shellcheck disable=SC1090
source "${CONDA_SH}"

# 关掉 `set -u`：ROS 的 activate.d 脚本会引用未绑定的 CONDA_BUILD，
# 调用方常带 set -u，那样会直接把 source 打断，表现为"脚本毫无输出就退出"。
_sim_had_u=0
case "$-" in *u*) _sim_had_u=1; set +u ;; esac

if ! conda_switch_clean "${SIM_ENV_NAME}" "${REAL_ENV_NAME}"; then
  [[ "${_sim_had_u}" -eq 1 ]] && set -u
  unset _sim_had_u
  echo "无法激活 conda 环境 ${SIM_ENV_NAME}。" >&2
  echo "先创建: bash ${ROOT}/scripts/install_sim_env.sh" >&2
  return 1 2>/dev/null || exit 1
fi

# 仿真构建产物（由 scripts/build_sim.sh 用 --install-base 生成，与真机隔离）
if [[ -f "${ROOT}/install_sim/setup.bash" ]]; then
  # shellcheck disable=SC1091
  source "${ROOT}/install_sim/setup.bash"
else
  echo "提示: install_sim/ 还不存在，先跑 bash scripts/build_sim.sh" >&2
fi

[[ "${_sim_had_u}" -eq 1 ]] && set -u
unset _sim_had_u

export YOLO_AUTOINSTALL=false
export ROS2_WS="${ROOT}"
# 仿真单独一个 DDS domain：避免与正在跑的真机节点互相抢话题/污染 TF。
# 真机用的是默认 domain，两者天然隔离。
export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-77}"

# =============================================================================
# Gazebo 的插件与资源搜索路径 —— 必须在这里设，不能只在 launch 里设
# =============================================================================
# ros_gz_sim 的 gz_sim.launch.py 用的是 `from os import environ`，即在**生成
# launch 描述时**读进程环境，然后把它算出来的值作为 additional_env 传给 gz。
# 所以用 launch 的 SetEnvironmentVariable 补是没用的：那个值会被 gz_sim.launch.py
# 自己算的那份字典覆盖掉。必须在 ros2 launch 之前就进到进程环境里。
#
# 1) 插件搜索路径
#    缺了它，Gazebo 会在加载 gz_ros2_control 时报：
#        [Err] [SystemLoader.cc:94] Failed to load system plugin
#        [libgz_ros2_control-system.so] : couldn't find shared library.
#    而库其实就在 $CONDA_PREFIX/lib 下（`ldd` 也全齐）。原因是 gz 的
#    SystemLoader 先在搜索路径里找文件，找不到就直接放弃，**不会**去
#    dlopen 一个裸文件名（所以 conda 二进制的 RPATH 兜不住）。
#    又因为 gz_sim.launch.py 只把 LD_LIBRARY_PATH 拼进搜索路径，而 conda 激活
#    并不往 LD_LIBRARY_PATH 里加 $CONDA_PREFIX/lib，于是这条路径永远缺失。
#
# 2) 资源搜索路径
#    gz 的 URDF 解析器会把 mesh 的 package:// 改写成 model://，
#    而 model://<名> 是去"资源路径下的子目录"里找的。因此这里放的必须是
#    **各包 share 目录的上一层**（该层的子目录名就是包名），不是 share 目录本身。
#    缺了它的表现是一堆非致命但很吵的报错（视觉网格加载不出来），
#    并且**碰撞网格同样加载不出来** —— 后者会让装配任务的接触判据失效。
if [[ -n "${CONDA_PREFIX:-}" ]]; then
  export GZ_SIM_SYSTEM_PLUGIN_PATH="${CONDA_PREFIX}/lib${GZ_SIM_SYSTEM_PLUGIN_PATH:+:${GZ_SIM_SYSTEM_PLUGIN_PATH}}"
  # IGN_* 是 Fortress（改名之前）用的名字，两个都设，兼容新旧插件查找逻辑
  export IGN_GAZEBO_SYSTEM_PLUGIN_PATH="${CONDA_PREFIX}/lib${IGN_GAZEBO_SYSTEM_PLUGIN_PATH:+:${IGN_GAZEBO_SYSTEM_PLUGIN_PATH}}"
fi

_res_paths=()
for _d in "${ROOT}"/install_sim/*/share; do
  [[ -d "${_d}" ]] && _res_paths+=("${_d}")
done
_res_paths+=("${ROOT}/devices/sim/gazebo/models")   # 以后往这里放自定义模型
_res_joined="$(IFS=:; echo "${_res_paths[*]}")"

export GZ_SIM_RESOURCE_PATH="${_res_joined}${GZ_SIM_RESOURCE_PATH:+:${GZ_SIM_RESOURCE_PATH}}"
export IGN_GAZEBO_RESOURCE_PATH="${_res_joined}${IGN_GAZEBO_RESOURCE_PATH:+:${IGN_GAZEBO_RESOURCE_PATH}}"
unset _res_paths _res_joined _d

# 仿真与真机共用同一份 site.yaml —— 这样"仿真里调好的参数搬到真机也生效"。
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh" 2>/dev/null || true

echo "[sim_env] CONDA=${CONDA_DEFAULT_ENV}  ROS_DISTRO=${ROS_DISTRO}  ROS_DOMAIN_ID=${ROS_DOMAIN_ID}"