#!/usr/bin/env bash
set -euo pipefail

H2_ROOT=${H2_ROOT:-$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/.." && pwd)}
H2_IF=${H2_IF:-enp4s0f0}
H2_PY=${H2_PY:-python3}
cd "$H2_ROOT"

set +u
# shellcheck disable=SC1090
source scripts/source_unitree_env.sh "$H2_IF"
# shellcheck disable=SC1091
source install/setup.bash
set -u
export ROS2CLI_NO_DAEMON=1

echo "[1/4] H2BR HEALTHY + DISARMED"
"$H2_PY" scripts/inspect_shm_state.py --once \
  --require-healthy --require-disarmed --expected-interface "$H2_IF"

echo "[2/4] BRIDGE LOWSTATE UPDATE RATE"
"$H2_PY" - <<'PY'
import fcntl
import time
from runtime.h2_shm_bridge import H2SharedMemoryBridge

bridge = H2SharedMemoryBridge()
try:
    with bridge._file_lock(fcntl.LOCK_SH):
        first = int(bridge._mem.state_seq)
    start = time.monotonic()
    time.sleep(3.0)
    elapsed = time.monotonic() - start
    with bridge._file_lock(fcntl.LOCK_SH):
        last = int(bridge._mem.state_seq)
finally:
    bridge.close()
updates = (last - first) / 2.0
rate = updates / elapsed
print(f"state_updates={updates:.0f} elapsed_s={elapsed:.6f} update_rate_hz={rate:.1f}")
if not 900.0 <= rate <= 1150.0:
    raise SystemExit(f"[FAIL] bridge LowState update rate {rate:.1f} Hz outside [900,1150]")
print("[PASS] bridge receives approximately 1000 LowState updates/s")
PY

echo "[3/4] UNIQUE ROS LOWCMD ENDPOINT"
LOWCMD_INFO=$(mktemp)
trap 'rm -f "$LOWCMD_INFO"' EXIT
ros2 topic info /lowcmd --verbose --no-daemon --spin-time 5 >"$LOWCMD_INFO"
cat "$LOWCMD_INFO"
grep -Eq '^Publisher count: +1$' "$LOWCMD_INFO"
grep -Fq 'h2_lowcmd_shm_bridge' "$LOWCMD_INFO"

echo "[4/4] NO LOWCMD SAMPLE WITHOUT RUNNER"
set +e
timeout 3s ros2 topic echo /lowcmd unitree_hg/msg/LowCmd \
  --once --no-daemon --spin-time 2 >/tmp/h2_no_runner_lowcmd_echo.txt 2>&1
LOWCMD_ECHO_RC=$?
set -e
cat /tmp/h2_no_runner_lowcmd_echo.txt
test "$LOWCMD_ECHO_RC" -eq 124

echo "[PASS] default 1000Hz bridge is healthy/disarmed and publishes no command without runner"