import threading

NAME = "6-7"


def run(motion, tts):
    # Threaded so the TTS call doesn't block the arm motion below.
    threading.Thread(target=tts.say, args=("six seven",), daemon=True).start()

    motion.setStiffnesses(["RArm", "LArm"], 1.0)

    # Lock both arms up in front of the body, hands held out like a scale.
    # ElbowYaw is rotated ~90° off zero here - that's what points the elbow's
    # bend axis so flexing tips the forearm forward/down instead of curling
    # it in across the chest. These joints don't move again until the return.
    hold_names = [
        "RShoulderPitch", "RShoulderRoll", "RElbowYaw", "RWristYaw", "RHand",
        "LShoulderPitch", "LShoulderRoll", "LElbowYaw", "LWristYaw", "LHand"
    ]
    hold_angles = [
        0.2, -0.2, 1.4, 1.57, 0.6,
        0.2, 0.2, -1.4, -1.57, 0.6
    ]
    motion.angleInterpolationWithSpeed(hold_names, hold_angles, 0.25)

    # Only the elbows seesaw now - a small tip back and forth, one arm
    # rising while the other falls, like weighing something in each hand
    # ("six... seven..."). Small amplitude so it stays a wobble, not a fold.
    elbow_names = ["RElbowRoll", "LElbowRoll"]
    r_low, r_high = 0.6, 1.0
    l_low, l_high = -0.6, -1.0

    elbow_angles = [
        [r_low, r_high, r_low, r_high, r_low, r_high],
        [l_high, l_low, l_high, l_low, l_high, l_low],
    ]
    times = [[0.35, 0.7, 1.05, 1.4, 1.75, 2.1]] * 2
    motion.angleInterpolation(elbow_names, elbow_angles, times, True)

    # Return everything to neutral
    neutral_names = hold_names + elbow_names
    neutral = [1.5, -0.15, 1.2, 0.0, 0.0,
               1.5, 0.15, -1.2, 0.0, 0.0,
               0.5, -0.5]
    motion.angleInterpolationWithSpeed(neutral_names, neutral, 0.3)
    # Relaxed stiffness after completion
    motion.setStiffnesses(["RArm", "LArm"], 0.3)