#!/usr/bin/env python3
"""Robot antenna-clap test for the Morse Code app (Lite / local daemon).

Drives the antennas to clap on the app's impulse wire-code schedule (same code
as lib/robot-tapper.js), records the laptop mic, runs the app's detector, and
decodes — confirming the clap is audible + detectable and letting us tune the
contact angle / hold / latency lead. Mirrors the proven, safe collision recipe
from marionette/tests/test_antenna_collision.py (low-PID antennas stall at
contact; gentle).

MOVES THE ROBOT and MAKES NOISE. Run only when ready.

    /Users/remi/.virtualenvs/mini/bin/python tools/robot_clap_test.py --once
    /Users/remi/.virtualenvs/mini/bin/python tools/robot_clap_test.py --phrase "SOS" --unit 150
"""
from __future__ import annotations

import argparse
import math
import sys
import time
from pathlib import Path

import numpy as np
import requests
import sounddevice as sd

TOOLS = Path(__file__).resolve().parent
sys.path.insert(0, str(TOOLS))
from calibrate_audio import (  # noqa: E402
    timing, text_to_onsets, onsets_to_text, detect_transient_onsets, SR,
)

DEG = math.pi / 180.0

# Defaults mirror lib/robot-tapper.js DEFAULT_TAP_PROFILE.
RIGHT_REST_DEG = -39.0
LEFT_REST_DEG = 0.0
COLLISION_DEG = 40.0
APPROACH_MS = 80.0
HOLD_MS = 50.0
RETURN_MS = 80.0
RATE_HZ = 50.0
LEAD_MS = 0.0


def left_angle_at(t_ms, onsets, collision, approach, hold, ret, rest):
    for T in onsets:
        a0, h_end, r_end = T - approach, T + hold, T + hold + ret
        if t_ms < a0 or t_ms > r_end:
            continue
        if t_ms <= T:
            f = (t_ms - a0) / approach if approach > 0 else 1.0
            f = min(1.0, max(0.0, f))
            return rest + (collision - rest) * (f * f)
        if t_ms <= h_end:
            return collision
        f = (t_ms - h_end) / ret
        f = min(1.0, max(0.0, f))
        return collision + (rest - collision) * (1 - (1 - f) * (1 - f))
    return rest


def stop_current_app():
    try:
        requests.post("http://127.0.0.1:8000/api/apps/stop-current-app", timeout=4)
    except Exception:
        pass


def run(args):
    from reachy_mini import ReachyMini
    from reachy_mini.utils import create_head_pose

    t = timing(args.unit)
    onsets = [0.0] if args.once else text_to_onsets(args.phrase, t)
    onsets = [o - args.lead for o in onsets]
    collision = args.collision
    end_ms = (onsets[-1] if onsets else 0) + HOLD_MS + RETURN_MS + 300
    rec_secs = (args.leadin_ms + end_ms + args.settle_ms + 1200) / 1000.0

    print(f"Phrase {args.phrase!r}  unit={args.unit}ms  contact={collision}°  "
          f"hold={args.hold}ms  lead={args.lead}ms")
    print(f"Recording mic for {rec_secs:.1f}s; robot will clap {len(onsets)} time(s).")

    stop_current_app()
    time.sleep(0.5)

    r = ReachyMini(media_backend="no_media")
    try:
        r.goto_target(create_head_pose(),
                      antennas=[LEFT_REST_DEG * DEG, RIGHT_REST_DEG * DEG], duration=1.0)
        time.sleep(1.2)

        rec = sd.rec(int(rec_secs * SR), samplerate=SR, channels=1, dtype="float32")
        leadin = args.leadin_ms / 1000.0
        time.sleep(leadin)  # silence head so the first onset has a quiet window

        period = max(0.01, 1.0 / RATE_HZ)
        t0 = time.monotonic()
        while True:
            tm = (time.monotonic() - t0) * 1000.0
            if tm > end_ms:
                break
            left = left_angle_at(tm, onsets, collision, APPROACH_MS, args.hold, RETURN_MS, LEFT_REST_DEG)
            r.set_target(head=np.eye(4), body_yaw=0.0,
                         antennas=np.array([left * DEG, RIGHT_REST_DEG * DEG]))
            nxt = t0 + (math.floor((tm / 1000.0) / period) + 1) * period
            dt = nxt - time.monotonic()
            if dt > 0:
                time.sleep(dt)

        # Let the final clap fully settle BEFORE any return motion — otherwise
        # the return goto jerks the antenna and rings out extra clicks right
        # after the last real onset (corrupting the last symbol).
        time.sleep(args.settle_ms / 1000.0)
        r.goto_target(create_head_pose(),
                      antennas=[LEFT_REST_DEG * DEG, RIGHT_REST_DEG * DEG], duration=0.8)
        sd.wait()
    finally:
        try:
            r.goto_target(create_head_pose(), antennas=[0.0, 0.0], duration=0.8)
            time.sleep(1.0)
        except Exception:
            pass

    audio = rec.flatten()
    if args.save:
        audio.astype(np.float32).tofile(args.save)
        print(f"Saved raw float32 mono @{SR}Hz to {args.save} ({audio.size} samples)")
    detected = detect_transient_onsets(audio, SR, highpass_freq=args.highpass,
                                       threshold_db=20 * math.log10(args.ratio))
    detected_ms = [d * 1000.0 for d in detected]
    peak = float(np.max(np.abs(audio))) if audio.size else 0.0

    print(f"\nMic peak amplitude: {peak:.3f}  (want clearly > the room floor)")
    print(f"Detected {len(detected_ms)} click(s); expected {len(onsets)}.")
    if not args.once:
        decoded = onsets_to_text(detected_ms, t)
        ok = decoded == args.phrase.upper()
        print(f"Decoded: {decoded!r}  [{'OK' if ok else 'MISMATCH'}]")
        return 0 if ok else 2
    print("Single-clap check: a click should be clearly visible above the floor.")
    return 0 if len(detected_ms) >= 1 else 2


def main():
    ap = argparse.ArgumentParser(description="Robot antenna-clap test")
    ap.add_argument("--once", action="store_true", help="one gentle clap (safety check)")
    ap.add_argument("--phrase", default="SOS")
    ap.add_argument("--unit", type=int, default=150)
    ap.add_argument("--collision", type=float, default=COLLISION_DEG)
    ap.add_argument("--hold", type=float, default=HOLD_MS)
    ap.add_argument("--lead", type=float, default=LEAD_MS, help="advance timeline (ms)")
    ap.add_argument("--leadin-ms", type=float, default=600.0)
    ap.add_argument("--settle-ms", type=float, default=600.0,
                    help="quiet settle after the last clap before returning to rest")
    ap.add_argument("--highpass", type=float, default=2000.0)
    ap.add_argument("--ratio", type=float, default=0.1)
    ap.add_argument("--save", default=None, help="save raw float32 mono recording to this path")
    return run(ap.parse_args())


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