#!/bin/bash
# 在 Mac 上直连 MCU 跑控制 (需要先停 Pi 上的抢占源)
#
# 用法:
#   bash tools/onboard/run_on_mac.sh example   # 跑 example_position_pro.py
#   bash tools/onboard/run_on_mac.sh sin       # 跑 test_sin_leg.py
#   bash tools/onboard/run_on_mac.sh monitor   # 只读监听
#   bash tools/onboard/run_on_mac.sh stop      # 只停 Pi 上的抢占源
#   bash tools/onboard/run_on_mac.sh start     # 恢复 sportMode

PI_HOST="${PI_HOST:-192.168.123.161}"
PI_USER="${PI_USER:-pi}"
TEST="${1:-example}"

stop_competitors() {
    echo "=== 停 Pi 上的 MCU:8007 抢占源 ==="
    ssh ${PI_USER}@${PI_HOST} bash <<'EOF' || true
set +e
if sudo pkill -9 -f keep_sport_alive 2>/dev/null; then echo '  ✅ keep_sport_alive.sh'; else echo '  (无看门狗)'; fi
sleep 0.3
if sudo pkill -9 -f Legged_sport 2>/dev/null; then echo '  ✅ Legged_sport'; else echo '  (无)'; fi
if sudo pkill -9 -f appTransit 2>/dev/null; then echo '  ✅ appTransit'; else echo '  (无)'; fi
sleep 0.5
echo '--- 验证残留 ---'
if pgrep -fl 'Legged_sport|appTransit|keep_sport_alive' 2>/dev/null; then
    echo '  ⚠️  仍有残留'
else
    echo '  ✅ 全部清除'
fi
EOF
}

restart_sport() {
    echo "=== 恢复 sportMode (Pi 端 watchdog 会拉起 Legged_sport) ==="
    ssh ${PI_USER}@${PI_HOST} \
        "sudo bash -c 'nohup /home/pi/Unitree/autostart/sportMode/keep_sport_alive.sh > /dev/null 2>&1 &' 2>/dev/null || \
         sudo systemctl start sportMode 2>/dev/null || \
         echo '⚠️  请手动重启或重启狗'"
    sleep 2
    ssh ${PI_USER}@${PI_HOST} "pgrep -fl Legged_sport || echo '⚠️  Legged_sport 未运行'"
}

case "$TEST" in
    stop)
        stop_competitors
        ;;
    start)
        restart_sport
        ;;
    example)
        stop_competitors
        echo ""
        echo "=== Mac 直连跑 example_position_pro.py ==="
        conda run -n free_dog_sdk python tools/onboard/example_position_pro.py \
            --state data/lowlevel_captures/blowfish_state.bin \
            --max-steps 5000
        ;;
    sin)
        stop_competitors
        echo ""
        echo "=== Mac 直连跑 test_sin_leg.py ==="
        conda run -n free_dog_sdk python tools/onboard/test_sin_leg.py \
            --amplitude 0.3 --freq 0.5 --duration 10
        ;;
    monitor)
        stop_competitors
        echo ""
        echo "=== Mac 直连只读监听 30 秒 ==="
        conda run -n free_dog_sdk python tools/onboard/monitor_state.py \
            --state data/lowlevel_captures/blowfish_state.bin \
            --duration 30 --verbose
        ;;
    *)
        echo "用法: bash run_on_mac.sh [example|sin|monitor|stop|start]"
        exit 1
        ;;
esac

echo ""
echo "============================================"
echo "测试结束. 恢复 sportMode:"
echo "  bash tools/onboard/run_on_mac.sh start"
echo "============================================"