#!/usr/bin/env bash
# Wire local Flexiv trees under ~/文档/Flexiv_Rizon4 into this workspace.
# Avoids re-cloning RDK / sim plugin from GitHub.
set -euo pipefail

ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
SRC="${ROOT}/src"
TP="${ROOT}/thirdparty"
LOCAL_FLEXIV="${LOCAL_FLEXIV:-$HOME/文档/Flexiv_Rizon4}"

if [[ ! -d "${LOCAL_FLEXIV}/flexiv_rdk" ]]; then
  echo "ERROR: not found: ${LOCAL_FLEXIV}/flexiv_rdk"
  echo "Set LOCAL_FLEXIV to your Flexiv_Rizon4 directory."
  exit 1
fi

mkdir -p "${SRC}" "${TP}"

VER_LINE=$(grep -E '^project\(flexiv_rdk' "${LOCAL_FLEXIV}/flexiv_rdk/CMakeLists.txt" | head -1 || true)
echo "Linking RDK (${VER_LINE:-unknown version})..."
ln -sfn "${LOCAL_FLEXIV}/flexiv_rdk" "${SRC}/flexiv_rdk"
touch "${LOCAL_FLEXIV}/flexiv_rdk/COLCON_IGNORE"

if [[ -d "${LOCAL_FLEXIV}/flexiv_sim_plugin" ]]; then
  if [[ -d "${TP}/flexiv_sim_plugin" && ! -L "${TP}/flexiv_sim_plugin" ]]; then
    mv "${TP}/flexiv_sim_plugin" "${TP}/flexiv_sim_plugin.cloned.bak.$(date +%s)"
  fi
  ln -sfn "${LOCAL_FLEXIV}/flexiv_sim_plugin" "${TP}/flexiv_sim_plugin"
  touch "${LOCAL_FLEXIV}/flexiv_sim_plugin/COLCON_IGNORE"
  echo "Linked thirdparty/flexiv_sim_plugin -> ${LOCAL_FLEXIV}/flexiv_sim_plugin"
fi

# humble-v1 ROS hardware needs RDK 1.9.x (kept separately if present)
if [[ -d "${SRC}/flexiv_rdk.bak.1787646725" ]]; then
  ln -sfn "${SRC}/flexiv_rdk.bak.1787646725" "${SRC}/flexiv_rdk_v1"
  touch "${SRC}/flexiv_rdk.bak.1787646725/COLCON_IGNORE"
  echo "ROS stack RDK v1.9.1 available at src/flexiv_rdk_v1"
fi

cat <<EOF

Done.
  src/flexiv_rdk          -> ${LOCAL_FLEXIV}/flexiv_rdk   (your tree, currently v2.1)
  thirdparty/flexiv_sim_plugin -> local sim plugin

Note: flexiv_ros2 humble-v1 expects RDK **v1.9.1**.
  Build/install src/flexiv_rdk_v1 into ~/flexiv_install for flexiv_hardware.
  Use your v2.1 tree for RDK C++/Python examples and Isaac-side tooling.

EOF