#!/usr/bin/env bash
# 克隆 Flexiv 官方 ROS 2 栈（Rizon 用，RDK v1.x → humble-v1 分支）。
set -euo pipefail

ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
SRC="${ROOT}/src"
BRANCH="${FLEXIV_ROS2_BRANCH:-humble-v1}"

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

# 无论如何都要保证官方 URDF 包存在（mesh + xacro）
if [[ ! -d flexiv_description ]]; then
  echo "Fetching flexiv_description (${BRANCH})..."
  if ! git clone --depth 1 -b "${BRANCH}" https://github.com/flexivrobotics/flexiv_description.git; then
    echo "git clone slow/failed — downloading zip..."
    python3 - <<PY
import urllib.request, socket, zipfile, io, shutil, os
url = "https://codeload.github.com/flexivrobotics/flexiv_description/zip/refs/heads/${BRANCH}"
old = socket.getaddrinfo
socket.getaddrinfo = lambda h, p, f=0, t=0, pr=0, fl=0: [ai for ai in old(h, p, socket.AF_INET, t, pr, fl)]
data = urllib.request.urlopen(urllib.request.Request(url, headers={"User-Agent": "Mozilla/5.0"}), timeout=300).read()
dest = "${SRC}/flexiv_description"
with zipfile.ZipFile(io.BytesIO(data)) as z:
    z.extractall("/tmp")
shutil.rmtree(dest, ignore_errors=True)
shutil.move("/tmp/flexiv_description-${BRANCH}", dest)
print("flexiv_description ready at", dest)
PY
  fi
else
  echo "flexiv_description already present"
fi

if [[ ! -d flexiv_ros2 ]]; then
  echo "Cloning flexiv_ros2 (${BRANCH})..."
  git clone https://github.com/flexivrobotics/flexiv_ros2.git -b "${BRANCH}"
else
  echo "flexiv_ros2 already present — pull if needed: git -C flexiv_ros2 pull"
fi

REPOS_FILE=""
for candidate in flexiv_ros2/flexiv.humble.repos flexiv_ros2/flexiv.repos; do
  if [[ -f "${candidate}" ]]; then
    REPOS_FILE="${candidate}"
    break
  fi
done

if [[ -z "${REPOS_FILE}" ]]; then
  echo "ERROR: no .repos file found under flexiv_ros2"
  exit 1
fi

echo "Importing dependencies from ${REPOS_FILE}..."
if ! command -v vcs >/dev/null 2>&1; then
  echo "Please install: sudo apt install python3-vcstool"
  exit 1
fi

# 优先用本地 Flexiv_Rizon4 里的 RDK（有本地副本时就不去 GitHub 克隆）
LOCAL_FLEXIV="${LOCAL_FLEXIV:-$HOME/文档/Flexiv_Rizon4}"
if [[ -d "${LOCAL_FLEXIV}/flexiv_rdk" ]]; then
  echo "Using local RDK: ${LOCAL_FLEXIV}/flexiv_rdk"
  bash "${ROOT}/scripts/use_local_flexiv.sh"
elif [[ -d flexiv_rdk ]]; then
  touch flexiv_rdk/COLCON_IGNORE
else
  echo "Importing flexiv_rdk from ${REPOS_FILE}..."
  vcs import "${SRC}" < "${REPOS_FILE}" --recursive --skip-existing
  if [[ -d flexiv_rdk ]]; then
    touch flexiv_rdk/COLCON_IGNORE
  fi
fi

# 其余依赖一律从 .repos 导入（description 已单独处理；skip-existing 跳过已有）
vcs import "${SRC}" < "${REPOS_FILE}" --recursive --skip-existing || true

cat <<EOF

Done. Next steps (Ubuntu 22.04 / Humble + Rizon):

  Local RDK wired via: scripts/use_local_flexiv.sh
    src/flexiv_rdk     -> ${LOCAL_FLEXIV}/flexiv_rdk   (your copy, often RDK 2.x)
    src/flexiv_rdk_v1  -> RDK 1.9.1 for flexiv_ros2 humble-v1 (if present)

  1) Install ROS deps:
       sudo apt install -y python3-colcon-common-extensions python3-vcstool \\
         ros-humble-ros2-control ros-humble-ros2-controllers \\
         ros-humble-moveit ros-humble-xacro ros-humble-joint-state-publisher-gui

  2) Build & install RDK **v1.9.1** into ~/flexiv_install (for flexiv_hardware):
       RDK_SRC=${SRC}/flexiv_rdk_v1
       # fallback if only one tree: RDK_SRC=${SRC}/flexiv_rdk
       cd \${RDK_SRC}/thirdparty
       source /opt/ros/humble/setup.bash   # or: conda activate ros_flexiv
       bash build_and_install_dependencies_not_in_ros2.sh ~/flexiv_install
       cd \${RDK_SRC} && rm -rf build && mkdir build && cd build
       cmake .. -DCMAKE_INSTALL_PREFIX=~/flexiv_install -DCMAKE_PREFIX_PATH=~/flexiv_install
       cmake --build . --target install --config Release

  3) Build this workspace:
       cd ${ROOT}
       source /opt/ros/humble/setup.bash
       colcon build --symlink-install --cmake-args -DCMAKE_PREFIX_PATH=~/flexiv_install
       source install/setup.bash

  4) Real robot (site SN, no gripper yet):
       ros2 launch arm_bringup real.launch.py
       # or explicitly:
       ros2 launch arm_bringup real.launch.py robot_sn:=Rizon4-063989 load_gripper:=false

  Official fake hardware:
       ros2 launch flexiv_bringup rizon.launch.py robot_sn:=Rizon4-063989 use_fake_hardware:=true load_gripper:=false

EOF