#!/usr/bin/env bash
# =============================================================================
# manage.sh — test & manage the Inspire RH56 hands + G1 arm gestures (on PC2)
#
#   ./manage.sh <command> [args]
#
# Service / status:
#   status            show service, devices, adapters (default if no command)
#   devices           list /dev/inspire_* + ttyUSB + CH340 adapters
#   build             (re)build all binaries via build.sh
#   start             start ONE inspire_g1 (detached); refuses if already running
#   stop              stop inspire_g1
#   restart           stop + start
#   logs              tail the inspire_g1 log
#
# Hand only (needs the service; auto-starts it):
#   example           Unitree open/close test (hand_example)
#   menu              interactive finger menu (hand_gestures)
#   open|close|box    a finger pose (hand_gestures <name>)
#   pose <6 nums>     one-shot right-hand pose (hand_pose), e.g. pose 1 1 1 1 1 1
#   record            sculpt & SAVE named hand shapes (hand_record; save/load live)
#   poses             list saved shapes
#   play <name>       command a saved shape onto the hands
#   dashboard         start the web slider dashboard (http://<robot-ip>:8088)
#   dashboard-stop    stop the dashboard
#
# Arm + hand fused (ARM MOVES — clear space, E-stop ready; needs the service):
#   like              thumbs-up "like" held during the built-in handshake
#   handshake         reach -> grip -> thumbs-up -> release
#   thumbup           raise right hand -> thumbs-up -> release
#
# Arm only (built-in actions):
#   arm <id|name>     run a built-in action (e.g. arm "shake hand", arm 99)
#   armlist           list built-in actions
#
# Env: IFACE (default eth0)
# =============================================================================
set -uo pipefail

HERE="$(cd "$(dirname "$0")" && pwd)"
BUILD="$HERE/build"
IFACE="${IFACE:-eth0}"
LOG="/tmp/inspire_g1.log"
SVC="$BUILD/inspire_g1"

c_red=$'\e[31m'; c_grn=$'\e[32m'; c_yel=$'\e[33m'; c_rst=$'\e[0m'
ok(){   echo "${c_grn}$*${c_rst}"; }
warn(){ echo "${c_yel}$*${c_rst}"; }
err(){  echo "${c_red}$*${c_rst}" >&2; }

svc_count(){ local n; n="$(pgrep -xc inspire_g1 2>/dev/null)"; echo "${n:-0}"; }
svc_running(){ [ "$(svc_count)" -ge 1 ]; }
have_devices(){ [ -e /dev/ttyUSB0 ] && [ -e /dev/ttyUSB1 ]; }
writable(){ [ -w /dev/ttyUSB0 ] && [ -w /dev/ttyUSB1 ]; }
need_build(){ [ -x "$SVC" ] || { err "binaries missing — run: ./manage.sh build"; exit 1; }; }

show_devices(){
  echo "Hand serial (ttyUSB0=right, ttyUSB1=left):"
  if have_devices; then
    ls -l /dev/ttyUSB0 /dev/ttyUSB1 2>&1 | sed 's/^/  /'
    writable || warn "  NOT writable — run the udev rule (see SETUP_G1.md §2) or sudo chmod 666."
  else warn "  /dev/ttyUSB0+1 MISSING — plug in the two CH340 hand adapters."; fi
  echo "CH340 adapters (lsusb):"
  lsusb | grep -i "1a86" | sed 's/^/  /' || warn "  none detected"
}

cmd_status(){
  local n; n="$(svc_count)"
  if   [ "$n" -eq 0 ]; then warn "inspire_g1: NOT running"
  elif [ "$n" -eq 1 ]; then ok   "inspire_g1: running ($(pgrep -x inspire_g1))"
  else err "inspire_g1: $n INSTANCES running — duplicates corrupt the serial bus! run: ./manage.sh restart"; fi
  show_devices
}

cmd_wait(){
  echo "Waiting for hand adapters — plug in the two USB-RS485 dongles now (Ctrl+C to stop)..."
  while ! have_devices; do
    printf "\r  CH340 on bus: %s   ttyUSB0:%s ttyUSB1:%s   " \
      "$(lsusb | grep -ci 1a86)" \
      "$([ -e /dev/ttyUSB0 ] && echo ✓ || echo ✗)" \
      "$([ -e /dev/ttyUSB1 ] && echo ✓ || echo ✗)"
    sleep 1
  done
  echo; ok "Adapters detected!"; show_devices
}

cmd_stop(){ pkill -x inspire_g1 2>/dev/null; sleep 1; pkill -9 -x inspire_g1 2>/dev/null; ok "stopped"; }

cmd_start(){
  need_build
  if svc_running; then warn "already running ($(svc_count) instance(s)); use restart to reset"; return 0; fi
  have_devices || warn "WARNING: /dev/inspire_* missing — fingers won't move until adapters are plugged in."
  ( cd "$BUILD" && setsid nohup "$SVC" >"$LOG" 2>&1 </dev/null & )
  sleep 2
  if svc_running; then ok "started (pid $(pgrep -x inspire_g1))"; sed 's/^/  /' "$LOG"
  else err "FAILED to start — log:"; cat "$LOG"; return 1; fi
}

cmd_restart(){ cmd_stop; cmd_start; }

ensure_service(){ svc_running || { echo "(starting inspire_g1...)"; cmd_start || exit 1; }
                  [ "$(svc_count)" -gt 1 ] && { warn "multiple instances; resetting"; cmd_restart; }; return 0; }

hand_gesture(){ need_build; ensure_service; "$BUILD/hand_gestures" "$@"; }
fused(){        need_build; ensure_service; warn "ARM WILL MOVE — clear space, E-stop ready."; "$BUILD/$1" "${@:2}" "$IFACE"; }

cmd="${1:-status}"; shift || true
case "$cmd" in
  status|st)       cmd_status ;;
  devices|dev)     show_devices ;;
  wait)            cmd_wait ;;
  build)           "$HERE/build.sh" ;;
  start)           cmd_start ;;
  stop)            cmd_stop ;;
  restart)         cmd_restart ;;
  logs)            tail -n 40 -f "$LOG" ;;
  example)         need_build; ensure_service; "$BUILD/hand_example" "$IFACE" ;;
  menu)            hand_gesture ;;
  open|close|box|combo|demo) hand_gesture "$cmd" ;;
  pose)            need_build; ensure_service; "$BUILD/hand_pose" "$IFACE" ;;
  record)          need_build; ensure_service; "$BUILD/hand_record" "$IFACE" ;;
  poses)           need_build; "$BUILD/hand_record" list ;;
  play)            need_build; ensure_service; "$BUILD/hand_record" play "$@" "$IFACE" ;;
  dashboard)       need_build; ensure_service
                   pkill -x hand_bridge 2>/dev/null; pkill -f "hand_web.py" 2>/dev/null; sleep 1
                   setsid "$BUILD/hand_bridge" "$IFACE" 7799 >/tmp/hand_bridge.log 2>&1 </dev/null &
                   setsid python3 "$HERE/web/hand_web.py"   >/tmp/hand_web.log    2>&1 </dev/null &
                   sleep 2
                   ip=$(hostname -I | awk "{print \$1}")
                   pgrep -x hand_bridge >/dev/null && echo "  bridge: up" || { echo "  bridge FAILED:"; sed "s/^/    /" /tmp/hand_bridge.log; }
                   grep -q "Running on" /tmp/hand_web.log && ok ">> Dashboard: http://$ip:8088   (also the wifi IP:8088)" || { echo "  web FAILED:"; sed "s/^/    /" /tmp/hand_web.log | tail -4; }
                   exit 0 ;;
  dashboard-stop)  pkill -x hand_bridge 2>/dev/null; pkill -f "hand_web.py" 2>/dev/null; ok "dashboard stopped" ;;
  like)            fused like "$@" ;;
  handshake)       fused handshake ;;
  thumbup)         fused thumbup ;;
  arm)             need_build; "$BUILD/arm_action" "$@" "$IFACE" ;;
  armlist)         need_build; "$BUILD/arm_action" list "$IFACE" ;;
  help|-h|--help)  sed -n '2,40p' "$0" | sed 's/^# \{0,1\}//' ;;
  *)               err "unknown command: $cmd"; echo "run: ./manage.sh help"; exit 1 ;;
esac