Demo: Handshake

"""
Demo: Handshake

Purpose:
Use a simple RealSense-based presence check to trigger a handshake pose.

Important:
This is a vision-assisted classroom demo. It does not claim perfect hand
detection or human-level understanding.
"""

from time import sleep, time
from swayform.motion import MotionClient
from swayform.vision import RealSenseInput


DETECTION_TIMEOUT_SECONDS = 10
HANDSHAKE_HOLD_SECONDS = 2.0

HANDSHAKE_POSE = {
    "shoulder_pitch": 38,
    "shoulder_roll": 10,
    "elbow_pitch": 82,
    "wrist_yaw": 0,
}


def wait_for_user(camera: RealSenseInput, timeout: float) -> bool:
    """
    Wait until the camera reports a user inside the interaction zone.
    Returns True if a user is found, otherwise False.
    """
    start_time = time()

    while time() - start_time < timeout:
        if camera.user_in_interaction_zone():
            return True

        sleep(0.1)

    return False


def run_handshake(motion: MotionClient) -> None:
    """
    Move into a conservative handshake pose, wait briefly,
    then return to idle.
    """
    motion.move_joint_group("right_arm", HANDSHAKE_POSE)
    sleep(HANDSHAKE_HOLD_SECONDS)
    motion.safe_pose("idle")


def main() -> None:
    motion = MotionClient()
    camera = RealSenseInput()

    motion.safe_pose("idle")
    print("Waiting for user...")

    user_detected = wait_for_user(camera, DETECTION_TIMEOUT_SECONDS)

    if not user_detected:
        print("No user detected. Returning to idle.")
        motion.safe_pose("idle")
        return

    try:
        motion.lock_behavior("handshake_demo")
        run_handshake(motion)
    finally:
        motion.unlock_behavior("handshake_demo")
        motion.safe_pose("idle")


if __name__ == "__main__":
    main()