#!/bin/bash
# Launch GR00T-WBC on real G1 robot via GB10
# Prerequisites:
#   1. Robot powered on and connected via Ethernet (192.168.123.0/24 subnet)
#   2. GB10 has 192.168.123.100/24 on enP7s7: sudo ip addr add 192.168.123.100/24 dev enP7s7
#   3. UFW disabled or allows traffic from 192.168.123.0/24
#   4. Robot in debug mode (L2+R2 while in damping state on harness) for motor control

set -e
cd ~/GR00T-WholeBodyControl
source .venv/bin/activate

echo "=== GR00T-WBC Real Robot Launch ==="
echo ""
echo "[1/2] Checking network..."
if ping -c 1 -W 1 192.168.123.161 > /dev/null 2>&1; then
    echo "  Locomotion computer (192.168.123.161): OK"
else
    echo "  ERROR: Cannot reach 192.168.123.161"
    echo "  Fix: sudo ip addr add 192.168.123.100/24 dev enP7s7"
    exit 1
fi

echo "[2/2] Launching GR00T-WBC (real mode)..."
echo "  --interface real (auto-detects enP7s7 via 192.168.123.x)"
echo "  Controls: ] = enable walk, w/s = fwd/back, a/d = strafe, q/e = rotate, z = zero"
echo "  WARNING: Robot must be on harness + debug mode (L2+R2) before enabling walk!"
echo ""

export LD_LIBRARY_PATH=/opt/ros/jazzy/lib:$LD_LIBRARY_PATH

python3 gr00t_wbc/control/main/teleop/run_g1_control_loop.py \
    --interface real \
    "$@"