#!/usr/bin/env python3
"""Quick check: is the robot reachable?

Usage:
    python tests/check_robot.py

Exits 0 if the robot is connected, 1 otherwise.
"""

import sys
import threading


def main() -> int:
    result = [None, None]  # [reachy, error]

    def _connect():
        try:
            from reachy_mini import ReachyMini
            reachy = ReachyMini(media_backend="no_media")
            pose = reachy.get_current_head_pose()
            result[0] = pose
        except Exception as exc:
            result[1] = str(exc)

    t = threading.Thread(target=_connect, daemon=True)
    t.start()
    t.join(timeout=10)

    if t.is_alive():
        print("FAIL: connection timed out after 10s")
        return 1
    if result[1]:
        print(f"FAIL: {result[1]}")
        return 1

    print(f"OK: robot connected, head pose shape = {result[0].shape}")
    return 0


if __name__ == "__main__":
    sys.exit(main())