#!/usr/bin/env bash
# 通过常驻守护进程注入动作（只写状态文件，不自己开连接）
#
# 前提：robot-daemon.py 正在运行，保持唯一长连接。
#
# 用法：
#   bash rc.sh go -40 800        # 后退 40% 速度 0.8 秒
#   bash rc.sh go 40 800         # 前进
#   bash rc.sh strafe 40 800     # 右转（负值左转）
#   bash rc.sh joy 30 -40 800    # 任意摇杆组合
#   bash rc.sh jump              # 跳一下
#   bash rc.sh stop              # 立即归零
#   bash rc.sh status            # 看当前注入状态 + 守护进程是否活着
#   bash rc.sh hold              # 持续注入零帧（确保静止，默认 5 秒）

set -u
D="$(cd "$(dirname "$0")" && pwd)"
STATE="${RC_STATE:-$D/daemon-state.json}"
PIDF="$D/daemon.pid"

die() { echo "[rc] $*" >&2; exit 1; }

alive() {
  [ -f "$PIDF" ] || return 1
  local pid; pid="$(cat "$PIDF" 2>/dev/null || true)"
  [ -n "$pid" ] || return 1
  kill -0 "$pid" 2>/dev/null || return 1
  return 0
}

write_state() {  # joy_x joy_y dir seconds
  local jx="$1" jy="$2" dr="$3" secs="$4"
  local until
  if [ "${secs:-0}" = "0" ]; then
    until=0
  else
    until="$(python3 -c "import time;print(f'{time.time()+float($secs):.3f}')")"
  fi
  python3 - "$STATE" "$jx" "$jy" "$dr" "$until" <<'PY'
import json, sys, os, tempfile
path, jx, jy, dr, until = sys.argv[1], int(sys.argv[2]), int(sys.argv[3]), sys.argv[4], float(sys.argv[5])
data = {"joy_x": jx, "joy_y": jy, "dir": dr, "until": until}
d = os.path.dirname(os.path.abspath(path))
fd, tmp = tempfile.mkstemp(dir=d)
with os.fdopen(fd, "w") as f:
    json.dump(data, f)
os.replace(tmp, path)
PY
}

CMD="${1:-}"; shift || true
alive || die "守护进程未运行。先启动：setsid nohup python3 $D/robot-daemon.py > $D/daemon.log 2>&1 &"

# 平衡开关硬锁：拒绝关闭
case "$CMD" in
  stable|balance)
    die "拒绝：主动保持平衡不允许关闭（用户明确指令）。"
    ;;
esac

case "$CMD" in
  go)
    Y="${1:-50}"; MS="${2:-800}"
    write_state 0 "$Y" stop "$(python3 -c "print(float($MS)/1000)")"
    echo "[rc] 摇杆 y=$Y 持续 ${MS}ms" ;;
  strafe)
    X="${1:-50}"; MS="${2:-800}"
    write_state "$X" 0 stop "$(python3 -c "print(float($MS)/1000)")"
    echo "[rc] 摇杆 x=$X 持续 ${MS}ms" ;;
  joy)
    X="${1:-0}"; Y="${2:-0}"; MS="${3:-800}"
    write_state "$X" "$Y" stop "$(python3 -c "print(float($MS)/1000)")"
    echo "[rc] 摇杆 x=$X y=$Y 持续 ${MS}ms" ;;
  jump)
    MS="${1:-300}"
    write_state 0 0 jump "$(python3 -c "print(float($MS)/1000)")"
    echo "[rc] 跳 ${MS}ms" ;;
  stop)
    write_state 0 0 stop 0
    echo "[rc] 已归零" ;;
  hold)
    MS="${1:-5000}"
    write_state 0 0 stop "$(python3 -c "print(float($MS)/1000)")"
    echo "[rc] 保持零帧 ${MS}ms" ;;
  status)
    echo "[rc] 守护进程 PID: $(cat "$PIDF")"
    echo "[rc] 状态文件: $STATE"
    cat "$STATE" 2>/dev/null || echo "(无)"
    echo
    echo "[rc] 最近日志:"; tail -6 "$D/daemon.log" 2>/dev/null ;;
  *)
    sed -n '2,16p' "$0" | sed 's/^# \{0,1\}//'; exit 1 ;;
esac