#!/usr/bin/env bash
# Assemble the build context (pull unitree_sdk2 + CycloneDDS from this robot) and
# build the self-contained image. Run ON the robot (aarch64).
#   ./docker/build_image.sh
set -euo pipefail

HERE="$(cd "$(dirname "$0")" && pwd)"     # .../DFX_inspire_service/docker
REPO="$(dirname "$HERE")"                 # .../DFX_inspire_service
CTX="$HERE/.ctx"
IMG="${IMG:-inspire-hand:latest}"

# The C++ SDK can come from three places, best first: an explicit override, the robot's
# own checkout, or deps/ shipped with this project — so a robot that has never seen the
# SDK can still build the image. Same idea for CycloneDDS below.
SDK="${UNITREE_SDK2_DIR:-}"
if [ -z "$SDK" ]; then
  for c in "$HOME/unitree_sdk2" "$REPO/deps/unitree_sdk2"; do
    [ -f "$c/lib/aarch64/libunitree_sdk2.a" ] && { SDK="$c"; break; }
  done
fi
[ -n "$SDK" ] && [ -f "$SDK/lib/aarch64/libunitree_sdk2.a" ] || {
  echo "ERROR: no unitree_sdk2 with lib/aarch64/libunitree_sdk2.a"
  echo "  looked in: \$UNITREE_SDK2_DIR, $HOME/unitree_sdk2, $REPO/deps/unitree_sdk2"
  echo "  fix: git clone --depth 1 https://github.com/unitreerobotics/unitree_sdk2 ~/unitree_sdk2"
  exit 1; }
echo ">> unitree_sdk2: $SDK"

echo ">> assembling build context in $CTX"
rm -rf "$CTX"
mkdir -p "$CTX/deps/usr_local_include" "$CTX/deps/usr_local_lib" "$CTX/deps/unitree_sdk2" "$CTX/app"

# unitree_sdk2 headers + static lib
cp -r "$SDK/include" "$CTX/deps/unitree_sdk2/"
cp -r "$SDK/lib"     "$CTX/deps/unitree_sdk2/"

# CycloneDDS headers: C (dds, ddsc), C++ (ddscxx) and iceoryx. /usr/local first (how the
# robot has it), then a copy staged in deps/ for a machine without a system install.
CDDS_INC=""
for c in /usr/local/include "$REPO/deps/cyclonedds/install/include"; do
  [ -d "$c/ddscxx" ] && { CDDS_INC="$c"; break; }
done
[ -n "$CDDS_INC" ] || {
  echo "ERROR: no CycloneDDS headers (need ddscxx)"
  echo "  looked in: /usr/local/include, $REPO/deps/cyclonedds/install/include"
  echo "  fix: build CycloneDDS 0.10.x with -DCMAKE_INSTALL_PREFIX=/usr/local,"
  echo "       or stage its install/ prefix at $REPO/deps/cyclonedds"
  exit 1; }
echo ">> CycloneDDS headers: $CDDS_INC"
for d in dds ddsc ddscxx iceoryx; do cp -r "$CDDS_INC/$d" "$CTX/deps/usr_local_include/"; done

# The IDL headers (idl/, idlc/) are needed ONLY by the python binding: pip building
# cyclonedds compiles a _idlpy extension against idl/retcode.h, and `make install` of the
# C++ side does not put them in /usr/local/include. Without them the image build dies at
#   idlpy/src/context.h:17: fatal error: idl/retcode.h: No such file or directory
# CYCLONEDDS_SRC points at the checkout whose install/ prefix has them (0.10.5 here).
CDDS_SRC="${CYCLONEDDS_SRC:-$HOME/cyclonedds}"
# Search every layout these headers turn up in. A robot that got CycloneDDS through a ROS2
# colcon workspace has them under install/cyclonedds/include, while /usr/local carries the
# C and C++ headers but not idl/ — the two prefixes complement each other, so take idl from
# wherever it actually is. CYCLONEDDS_IDL_INCLUDE overrides the search.
for d in idl idlc; do
  for c in "${CYCLONEDDS_IDL_INCLUDE:-}/$d" "/usr/local/include/$d" \
           "$CDDS_SRC/install/include/$d" "$REPO/deps/cyclonedds/install/include/$d" \
           "$HOME/cyclonedds_ws/install/cyclonedds/include/$d"; do
    [ -n "${c#/}" ] && [ -d "$c" ] && {
      cp -r "$c" "$CTX/deps/usr_local_include/"
      [ "$d" = idl ] && echo ">> idl headers: $c"
      break; }
  done
done
[ -f "$CTX/deps/usr_local_include/idl/retcode.h" ] \
  || echo ">> WARNING: no idl/ headers — the in-image pip cyclonedds build will fail"

# CycloneDDS shared runtime libs (same fallback order as the headers)
CDDS_LIB="/usr/local/lib"
ls "$CDDS_LIB"/libddsc.so* >/dev/null 2>&1 || CDDS_LIB="$REPO/deps/cyclonedds/install/lib"
cp -P "$CDDS_LIB"/libddsc* "$CDDS_LIB"/libddscxx* \
      "$CDDS_LIB"/libcyclonedds* "$CDDS_LIB"/libdds_security_* \
      "$CTX/deps/usr_local_lib/" 2>/dev/null || true
ls "$CTX/deps/usr_local_lib"/libddsc.so* >/dev/null 2>&1 \
  || echo ">> WARNING: no libddsc.so — the in-image C++ build will not link"

# The app, with web/ split into its own context dir. The Dockerfile copies web/ AFTER the
# C++ build, so editing the dashboard rebuilds in seconds instead of recompiling every
# binary — and the dashboard is what changes most often.
rsync -a --exclude 'build/' --exclude 'docker/.ctx/' --exclude 'web/' "$REPO/" "$CTX/app/"
rsync -a "$REPO/web/" "$CTX/app_web/"
cp "$HERE/Dockerfile" "$CTX/Dockerfile"

# ---- recorder for the dashboard's Record/Replay panel ---------------------------------
# The panel runs g1_record_replay.py as a CHILD PROCESS, so the script and a python that
# can import unitree_sdk2py both have to be inside the image. Both dirs are always created
# (empty is fine) because the Dockerfile COPYs them unconditionally — with an empty one the
# image still builds and the panel simply reports "recorder not found".
# Both live IN this project (recorder/ and vendor/), so a clean checkout builds a complete
# image with no environment set and nothing fetched from elsewhere on the machine. The env
# vars remain as overrides for anyone pointing at their own working copy.
REC="${RECORDER_SRC:-$REPO/recorder}"
SDKPY="${UNITREE_SDK2PY_DIR:-$REPO/vendor/unitree_sdk2_python}"
mkdir -p "$CTX/deps/recorder/seed" "$CTX/deps/unitree_sdk2_python"
if [ -f "$REC/g1_record_replay.py" ] && [ -f "$SDKPY/setup.py" ]; then
  cp "$REC/g1_record_replay.py" "$CTX/deps/recorder/"
  # The home pose is seeded rather than shipped in DataG1/, because run.sh bind-mounts
  # DataG1 over the top and would hide anything baked there.
  for h in "$REC/arm_home.jsonl" "$REC/DataG1/arm_home.jsonl"; do
    [ -f "$h" ] && { cp "$h" "$CTX/deps/recorder/seed/"; break; }
  done
  rsync -a --exclude '.git/' --exclude '*.egg-info/' "$SDKPY/" "$CTX/deps/unitree_sdk2_python/"
  echo ">> recorder included: $REC  +  unitree_sdk2py: $SDKPY"
else
  echo ">> NOTE: record/replay panel will be DISABLED in this image."
  [ -f "$REC/g1_record_replay.py" ] || echo "   missing $REC/g1_record_replay.py   (set RECORDER_SRC)"
  [ -f "$SDKPY/setup.py" ]          || echo "   missing $SDKPY/setup.py            (set UNITREE_SDK2PY_DIR)"
fi

echo ">> docker build -t $IMG (this compiles the binaries in-image; takes a few min)"
cd "$CTX"
docker build -t "$IMG" .
rm -rf "$CTX"
echo ">> done: $IMG"
echo "   run with: $REPO/docker/run.sh"