tests/test_hardware.py

"""Tier 2 — Hardware integration tests for Marionette.

These tests require a physical Reachy Mini robot connected and powered on.
They exercise the full record/playback pipeline through the HTTP API with
real motor control.

All tests are decorated @pytest.mark.hardware and are skipped by default.
Run with:
    cd marionette
    pytest tests/test_hardware.py -m hardware

Or via the test runner:
    python tests/run_tests.py --hardware
"""

import json
import threading
import time
import wave
from pathlib import Path

import pytest
import uvicorn

from marionette.main import create_app, COUNTDOWN_SECONDS, MOTION_SAMPLE_RATE

# Skip all tests in this module unless -m hardware is specified
pytestmark = pytest.mark.hardware

HARDWARE_PORT = 18043
STARTUP_TIMEOUT = 30  # seconds to wait for robot startup animation
POLL_INTERVAL = 0.25

# Module-level flag: set when the server gets stuck (e.g. audio hang).
# Subsequent tests skip immediately instead of waiting 30s each.
_server_stuck = False


class _ServerThread:
    """Runs a uvicorn server in a background thread."""

    def __init__(self, app, port: int):
        self.config = uvicorn.Config(app, host="127.0.0.1", port=port, log_level="warning")
        self.server = uvicorn.Server(self.config)
        self.thread = threading.Thread(target=self.server.run, daemon=True)

    def start(self):
        self.thread.start()
        for _ in range(50):
            if self.server.started:
                break
            time.sleep(0.1)

    def stop(self):
        self.server.should_exit = True
        self.thread.join(timeout=5)


def _wait_for_mode(base_url: str, target_mode: str, timeout: float) -> dict:
    """Poll GET /api/state until mode matches target_mode or timeout."""
    import httpx

    deadline = time.time() + timeout
    last_state = {}
    while time.time() < deadline:
        try:
            resp = httpx.get(f"{base_url}/api/state", timeout=5)
            last_state = resp.json()
            if last_state.get("mode") == target_mode:
                return last_state
        except Exception:
            pass
        time.sleep(POLL_INTERVAL)
    raise TimeoutError(
        f"Timed out waiting for mode={target_mode!r} "
        f"(last mode={last_state.get('mode')!r})"
    )


def _ensure_idle(base_url: str, timeout: float = 30) -> dict:
    """Force the server back to idle, or skip the test if unrecoverable.

    When the server is stuck (e.g. audio recording hung), this sends stop
    commands and waits.  If the server cannot recover within *timeout*
    seconds, the current test is skipped (not failed) so that one stuck
    audio operation doesn't cascade-fail every subsequent test.
    """
    global _server_stuck
    import httpx

    # If a previous test already detected the server is stuck, skip fast.
    if _server_stuck:
        pytest.skip("Server stuck from a previous test (likely audio hang)")

    try:
        state = httpx.get(f"{base_url}/api/state", timeout=5).json()
    except Exception:
        _server_stuck = True
        pytest.skip("Cannot reach server")

    mode = state.get("mode")
    if mode == "idle":
        return state

    # Try to cancel whatever is running
    try:
        if mode in ("recording", "countdown", "queued"):
            httpx.post(f"{base_url}/api/record/stop", timeout=5)
        elif mode == "playing":
            httpx.post(f"{base_url}/api/play/stop", timeout=5)
    except Exception:
        pass

    try:
        return _wait_for_mode(base_url, "idle", timeout)
    except TimeoutError:
        _server_stuck = True
        pytest.skip(
            f"Server stuck in mode={mode!r} after stop — "
            f"likely audio/media hang on this platform"
        )


def _connect_robot():
    """Connect to ReachyMini with a timeout. Returns (reachy, error_msg)."""
    result = [None, None]  # [reachy, error]

    def _connect():
        try:
            from reachy_mini import ReachyMini
            result[0] = ReachyMini()
        except Exception as exc:
            result[1] = str(exc)

    t = threading.Thread(target=_connect, daemon=True)
    t.start()
    t.join(timeout=15)
    if t.is_alive():
        return None, "ReachyMini() connection timed out after 15s"
    if result[1]:
        return None, result[1]
    return result[0], None


@pytest.fixture(scope="session")
def hardware_server(tmp_path_factory):
    """Marionette server connected to a real robot.

    Starts the run() loop with a real ReachyMini and a uvicorn web server.
    Waits for the startup animation to complete (mode -> idle) before
    yielding the base URL and Marionette instance.
    """
    reachy, err = _connect_robot()
    if reachy is None:
        pytest.skip(f"Cannot connect to robot: {err}")

    tmp = tmp_path_factory.mktemp("hardware")
    app, marionette = create_app(
        registry_path=tmp / "registry.json",
        dataset_root=tmp / "datasets",
    )

    stop_event = threading.Event()

    # Start the run() loop — processes recordings/playbacks using the real robot
    run_thread = threading.Thread(
        target=marionette.run, args=(reachy, stop_event), daemon=True
    )
    run_thread.start()

    # Start the web server
    srv = _ServerThread(app, HARDWARE_PORT)
    srv.start()

    base_url = f"http://127.0.0.1:{HARDWARE_PORT}"

    # Wait for startup animation to finish
    try:
        _wait_for_mode(base_url, "idle", STARTUP_TIMEOUT)
    except TimeoutError:
        stop_event.set()
        srv.stop()
        pytest.skip("Robot startup animation did not finish in time")

    yield base_url, marionette, reachy

    stop_event.set()
    srv.stop()
    run_thread.join(timeout=5)


@pytest.fixture(scope="session")
def base_url(hardware_server):
    return hardware_server[0]


@pytest.fixture(scope="session")
def hw_marionette(hardware_server):
    return hardware_server[1]


@pytest.fixture(scope="session")
def hw_reachy(hardware_server):
    """The ReachyMini instance used by the Marionette server."""
    return hardware_server[2]


# ──────── Tests ────────────────────────────────────────────────────────
#
# Test ordering: silent operations first, audio operations last.
# If audio hangs (known Ubuntu/mic issue), silent tests still pass and
# audio tests are cleanly skipped via _ensure_idle.
# ──────────────────────────────────────────────────────────────────────


class TestHardwareStartup:
    def test_startup_reaches_idle(self, base_url: str):
        """After fixture setup, verify mode is 'idle'."""
        import httpx

        resp = httpx.get(f"{base_url}/api/state", timeout=5)
        assert resp.status_code == 200
        assert resp.json()["mode"] == "idle"


class TestHardwareRecording:
    def test_record_captures_motion(self, base_url: str):
        """Record 2s silent, verify move JSON has ~200 frames at ~100Hz."""
        import httpx

        _ensure_idle(base_url)
        resp = httpx.post(
            f"{base_url}/api/record",
            json={"duration": 2.0, "record_audio": False, "label": "hw-silent"},
            timeout=5,
        )
        assert resp.status_code == 200
        move_id = resp.json()["move_id"]

        _wait_for_mode(base_url, "idle", timeout=COUNTDOWN_SECONDS + 2 + 10)

        state = httpx.get(f"{base_url}/api/state", timeout=5).json()
        move = next((m for m in state["moves"] if m["id"] == move_id), None)
        assert move is not None, f"Move {move_id} not found in moves list"
        assert move["duration"] > 1.5, f"Duration too short: {move['duration']}"
        assert move["has_audio"] is False


class TestHardwarePlayback:
    def test_playback_silent_completes(self, base_url: str):
        """Play back a silent move, verify mode returns to idle."""
        import httpx

        _ensure_idle(base_url)
        state = httpx.get(f"{base_url}/api/state", timeout=5).json()
        move = next((m for m in state["moves"] if not m["has_audio"]), None)
        if move is None:
            pytest.skip("No silent move available for playback test")

        resp = httpx.post(
            f"{base_url}/api/play",
            json={"move_id": move["id"]},
            timeout=5,
        )
        assert resp.status_code == 200

        _wait_for_mode(base_url, "idle", timeout=move["duration"] + 15)

        state = httpx.get(f"{base_url}/api/state", timeout=5).json()
        assert state["mode"] == "idle"

    def test_record_and_delete(self, base_url: str):
        """Record a move, then delete it and verify file is removed."""
        import httpx

        _ensure_idle(base_url)
        resp = httpx.post(
            f"{base_url}/api/record",
            json={"duration": 1.5, "record_audio": False, "label": "hw-delete-me"},
            timeout=5,
        )
        assert resp.status_code == 200
        move_id = resp.json()["move_id"]

        _wait_for_mode(base_url, "idle", timeout=COUNTDOWN_SECONDS + 1.5 + 10)

        state = httpx.get(f"{base_url}/api/state", timeout=5).json()
        assert any(m["id"] == move_id for m in state["moves"])

        resp = httpx.delete(
            f"{base_url}/api/moves/{move_id}",
            timeout=5,
        )
        assert resp.status_code == 200

        state = httpx.get(f"{base_url}/api/state", timeout=5).json()
        assert not any(m["id"] == move_id for m in state["moves"])


class TestFullPipeline:
    """End-to-end pipeline tests: record → verify files → replay → delete.

    These tests exercise the full lifecycle and check that output files
    have the expected structure, durations, and frame counts.
    """

    def test_recording_produces_correct_json(self, base_url: str, hw_marionette):
        """Record 3s silent, verify JSON has timestamps, frames at ~100Hz."""
        import httpx

        _ensure_idle(base_url)
        duration = 3.0
        resp = httpx.post(
            f"{base_url}/api/record",
            json={"duration": duration, "record_audio": False, "label": "pipeline-json"},
            timeout=5,
        )
        assert resp.status_code == 200
        move_id = resp.json()["move_id"]

        _wait_for_mode(base_url, "idle", timeout=COUNTDOWN_SECONDS + duration + 10)

        # Read the actual JSON file from disk
        json_path = hw_marionette._dataset_dir / f"{move_id}.json"
        assert json_path.exists(), f"JSON file not found: {json_path}"

        data = json.loads(json_path.read_text())
        timestamps = data["time"]
        frames = data["set_target_data"]

        # Frame count should be close to duration * 100Hz
        expected_frames = int(duration * MOTION_SAMPLE_RATE)
        assert len(frames) > expected_frames * 0.8, (
            f"Too few frames: {len(frames)} (expected ~{expected_frames})"
        )
        assert len(timestamps) == len(frames)

        # Timestamps should span close to the requested duration
        actual_duration = timestamps[-1] - timestamps[0]
        assert actual_duration > duration * 0.8, (
            f"Recorded duration too short: {actual_duration:.2f}s (expected ~{duration}s)"
        )

        # Each frame must have head pose and antennas
        for frame in frames[:3]:  # check first few
            assert "head" in frame
            assert "antennas" in frame

        # Cleanup
        httpx.delete(f"{base_url}/api/moves/{move_id}", timeout=5)

    def test_record_replay_full_lifecycle(self, base_url: str, hw_marionette):
        """Record → verify → replay → verify playback completes → delete."""
        import httpx

        _ensure_idle(base_url)
        duration = 2.0

        # Step 1: Record
        resp = httpx.post(
            f"{base_url}/api/record",
            json={"duration": duration, "record_audio": False, "label": "lifecycle"},
            timeout=5,
        )
        assert resp.status_code == 200
        move_id = resp.json()["move_id"]

        state = _wait_for_mode(base_url, "idle", timeout=COUNTDOWN_SECONDS + duration + 10)
        assert any(m["id"] == move_id for m in state["moves"]), "Move not in list after recording"

        # Step 2: Replay
        resp = httpx.post(
            f"{base_url}/api/play",
            json={"move_id": move_id},
            timeout=5,
        )
        assert resp.status_code == 200

        state = _wait_for_mode(base_url, "idle", timeout=duration + 15)
        assert state["mode"] == "idle"

        # Step 3: Delete and verify cleanup
        json_path = hw_marionette._dataset_dir / f"{move_id}.json"
        assert json_path.exists()

        resp = httpx.delete(f"{base_url}/api/moves/{move_id}", timeout=5)
        assert resp.status_code == 200
        assert not json_path.exists(), "JSON file should be deleted"

        state = httpx.get(f"{base_url}/api/state", timeout=5).json()
        assert not any(m["id"] == move_id for m in state["moves"])

    def test_stop_cancels_queued_recording(self, base_url: str):
        """Submit a recording, immediately stop it — verify cancel works."""
        import httpx

        _ensure_idle(base_url)
        resp = httpx.post(
            f"{base_url}/api/record",
            json={"duration": 10.0, "record_audio": False, "label": "cancel-test"},
            timeout=5,
        )
        assert resp.status_code == 200

        # Immediately try to stop (may be queued or in countdown)
        time.sleep(0.1)
        resp = httpx.post(f"{base_url}/api/record/stop", timeout=5)
        assert resp.status_code == 200
        data = resp.json()
        assert data["stopped"] is True

        # Should return to idle
        _wait_for_mode(base_url, "idle", timeout=10)

    def test_recording_transitions_through_phases(self, base_url: str):
        """Verify the recording goes through queued → countdown → recording → idle."""
        import httpx

        _ensure_idle(base_url)
        duration = 3.0
        observed_modes = set()

        resp = httpx.post(
            f"{base_url}/api/record",
            json={"duration": duration, "record_audio": False, "label": "phases"},
            timeout=5,
        )
        assert resp.status_code == 200

        # Poll rapidly to observe phase transitions
        deadline = time.time() + COUNTDOWN_SECONDS + duration + 10
        while time.time() < deadline:
            try:
                state = httpx.get(f"{base_url}/api/state", timeout=2).json()
                observed_modes.add(state["mode"])

                # When in recording mode, verify timing fields are present
                if state["mode"] == "recording":
                    assert state["phase_start_at"] is not None, "recording phase_start_at is null"
                    assert state["phase_end_at"] is not None, "recording phase_end_at is null"

                # When in countdown, verify timing fields
                if state["mode"] == "countdown":
                    assert state["phase_start_at"] is not None, "countdown phase_start_at is null"
                    assert state["phase_end_at"] is not None, "countdown phase_end_at is null"

                if state["mode"] == "idle" and "phases" in (state.get("message") or ""):
                    break  # Recording completed
            except Exception:
                pass
            time.sleep(0.15)

        # We should have seen at least countdown and recording phases
        assert "countdown" in observed_modes, (
            f"Never saw countdown mode. Observed: {observed_modes}"
        )
        assert "recording" in observed_modes, (
            f"Never saw recording mode. Observed: {observed_modes}"
        )
        assert "idle" in observed_modes, (
            f"Never returned to idle. Observed: {observed_modes}"
        )

        # Cleanup
        state = httpx.get(f"{base_url}/api/state", timeout=5).json()
        move = next((m for m in state["moves"] if "phases" in m.get("label", "")), None)
        if move:
            httpx.delete(f"{base_url}/api/moves/{move['id']}", timeout=5)


def _create_synthetic_move(hw_marionette, label, duration, trajectory_fn):
    """Create a synthetic recording and inject it into Marionette's dataset.

    Args:
        hw_marionette: The Marionette instance (from fixture).
        label: Label/move_id for the recording.
        duration: Duration in seconds.
        trajectory_fn: Callable(t) -> (roll, pitch, yaw) in radians.
            Called at 100Hz for the full duration.

    Returns:
        move_id (str) — the ID of the injected move.
    """
    import numpy as np
    from scipy.spatial.transform import Rotation as R

    dt = 1.0 / MOTION_SAMPLE_RATE
    n = int(duration * MOTION_SAMPLE_RATE)
    timestamps = []
    frames = []
    for i in range(n):
        t = i * dt
        roll, pitch, yaw = trajectory_fn(t)
        rot = R.from_euler("xyz", [roll, pitch, yaw], degrees=False).as_matrix()
        pose = np.eye(4)
        pose[:3, :3] = rot
        timestamps.append(t)
        frames.append({
            "head": pose.tolist(),
            "antennas": [0.0, 0.0],
            "body_yaw": 0.0,
            "check_collision": False,
        })

    move_id = label
    data = {
        "description": f"Synthetic reference: {label}",
        "time": timestamps,
        "set_target_data": frames,
    }
    json_path = hw_marionette._dataset_dir / f"{move_id}.json"
    json_path.write_text(json.dumps(data, indent=2), encoding="utf-8")

    # Tell Marionette to pick up the new file
    hw_marionette._refresh_recordings()

    return move_id


def _observe_playback(base_url, hw_reachy, duration):
    """Observe head poses during playback at ~100Hz.

    Waits for playing mode, records poses until idle, returns
    (observed_times, observed_frames).
    """
    import httpx
    import numpy as np

    observed_times = []
    observed_frames = []
    playback_started = False
    t_start = time.time()
    t0 = None
    last_state_check = 0
    mode = "unknown"

    while time.time() - t_start < duration + 30:
        now = time.time()

        # Check API state every 0.5s (not every iteration — too slow)
        if now - last_state_check > 0.5:
            try:
                state = httpx.get(f"{base_url}/api/state", timeout=2).json()
                mode = state["mode"]
            except Exception:
                pass
            last_state_check = now

        if mode == "playing":
            if not playback_started:
                playback_started = True
                t0 = now
            pose = hw_reachy.get_current_head_pose()
            elapsed = now - t0
            observed_times.append(elapsed)
            observed_frames.append({
                "head": np.asarray(pose, dtype=float).tolist(),
                "antennas": [0.0, 0.0],
                "body_yaw": 0.0,
            })
        elif playback_started and mode == "idle":
            break

        time.sleep(0.01)

    return observed_times, observed_frames


class TestMotionAccuracy:
    """Play back synthetic reference recordings and compare observed poses.

    Creates known-trajectory JSON files, injects them into Marionette's
    dataset, plays them via the API, and observes actual robot poses
    during playback using get_current_head_pose().
    """

    def test_playback_reproduces_yaw_oscillation(
        self, base_url: str, hw_marionette, hw_reachy,
    ):
        """Inject sine-wave yaw reference, play back, observe, compare."""
        import httpx
        import numpy as np
        from scipy.spatial.transform import Rotation as R
        from pose_utils import compare_trajectories, frames_to_poses

        duration = 3.0
        freq, amplitude = 0.5, 0.4

        _ensure_idle(base_url)

        # Create synthetic reference with yaw oscillation
        move_id = _create_synthetic_move(
            hw_marionette,
            label="synth-yaw-osc",
            duration=duration,
            trajectory_fn=lambda t: (0.0, 0.0, amplitude * np.sin(2 * np.pi * freq * t)),
        )

        # Load reference for comparison
        ref_data = json.loads(
            (hw_marionette._dataset_dir / f"{move_id}.json").read_text()
        )
        ref_times = ref_data["time"]
        ref_frames = ref_data["set_target_data"]

        # Play it back
        resp = httpx.post(
            f"{base_url}/api/play",
            json={"move_id": move_id},
            timeout=5,
        )
        assert resp.status_code == 200

        # Observe poses during playback
        observed_times, observed_frames = _observe_playback(
            base_url, hw_reachy, duration,
        )

        assert len(observed_frames) > 50, (
            f"Too few observed frames: {len(observed_frames)}"
        )

        # Compare reference to observed
        metrics = compare_trajectories(
            ref_times, ref_frames, observed_times, observed_frames,
        )
        print(f"\nYaw oscillation playback ({len(observed_frames)} frames):")
        print(metrics.summary())

        assert metrics.magic_mean < 50, (
            f"Mean magic distance too high: {metrics.magic_mean:.1f}\n"
            f"{metrics.summary()}"
        )

        # Verify the yaw actually varied — robot moved
        rec_poses = frames_to_poses(observed_frames)
        yaws = [R.from_matrix(p[:3, :3]).as_euler("xyz")[2] for p in rec_poses]
        yaw_range = max(yaws) - min(yaws)
        assert yaw_range > 0.3, (
            f"Yaw range too small: {yaw_range:.2f} rad — robot may not have moved"
        )
        print(f"  Yaw range: {np.degrees(yaw_range):.1f} deg")

        # Cleanup
        httpx.delete(f"{base_url}/api/moves/{move_id}", timeout=5)

    def test_playback_reproduces_pitch_roll(
        self, base_url: str, hw_marionette, hw_reachy,
    ):
        """Inject combined pitch+roll reference, play back, observe, compare."""
        import httpx
        import numpy as np
        from pose_utils import compare_trajectories

        duration = 3.0
        _ensure_idle(base_url)

        move_id = _create_synthetic_move(
            hw_marionette,
            label="synth-pitch-roll",
            duration=duration,
            trajectory_fn=lambda t: (
                0.15 * np.sin(2 * np.pi * 0.4 * t),   # roll
                0.2 * np.sin(2 * np.pi * 0.3 * t),     # pitch
                0.0,                                     # yaw
            ),
        )

        ref_data = json.loads(
            (hw_marionette._dataset_dir / f"{move_id}.json").read_text()
        )
        ref_times = ref_data["time"]
        ref_frames = ref_data["set_target_data"]

        resp = httpx.post(
            f"{base_url}/api/play",
            json={"move_id": move_id},
            timeout=5,
        )
        assert resp.status_code == 200

        observed_times, observed_frames = _observe_playback(
            base_url, hw_reachy, duration,
        )

        assert len(observed_frames) > 50, (
            f"Too few observed frames: {len(observed_frames)}"
        )

        metrics = compare_trajectories(
            ref_times, ref_frames, observed_times, observed_frames,
        )
        print(f"\nPitch+roll playback ({len(observed_frames)} frames):")
        print(metrics.summary())

        assert metrics.magic_mean < 50, (
            f"Mean magic distance too high: {metrics.magic_mean:.1f}\n"
            f"{metrics.summary()}"
        )

        # Cleanup
        httpx.delete(f"{base_url}/api/moves/{move_id}", timeout=5)

    def test_playback_long_motion(
        self, base_url: str, hw_marionette, hw_reachy,
    ):
        """10s reference with slow oscillation — verify accuracy over time."""
        import httpx
        import numpy as np
        from pose_utils import compare_trajectories

        duration = 10.0
        _ensure_idle(base_url)

        move_id = _create_synthetic_move(
            hw_marionette,
            label="synth-long-yaw",
            duration=duration,
            trajectory_fn=lambda t: (0.0, 0.0, 0.3 * np.sin(2 * np.pi * 0.2 * t)),
        )

        ref_data = json.loads(
            (hw_marionette._dataset_dir / f"{move_id}.json").read_text()
        )
        ref_times = ref_data["time"]
        ref_frames = ref_data["set_target_data"]

        resp = httpx.post(
            f"{base_url}/api/play",
            json={"move_id": move_id},
            timeout=5,
        )
        assert resp.status_code == 200

        observed_times, observed_frames = _observe_playback(
            base_url, hw_reachy, duration,
        )

        assert len(observed_frames) > 200, (
            f"Too few observed frames for 10s playback: {len(observed_frames)}"
        )

        metrics = compare_trajectories(
            ref_times, ref_frames, observed_times, observed_frames,
        )
        print(f"\nLong playback ({len(observed_frames)} frames, {duration}s):")
        print(metrics.summary())

        assert metrics.magic_mean < 50, (
            f"Mean magic distance too high: {metrics.magic_mean:.1f}\n"
            f"{metrics.summary()}"
        )

        # Check first half vs second half — accuracy shouldn't degrade
        half = len(metrics.magic_distances) // 2
        first_half_mean = float(np.mean(metrics.magic_distances[:half]))
        second_half_mean = float(np.mean(metrics.magic_distances[half:]))
        print(f"  First half mean: {first_half_mean:.1f}")
        print(f"  Second half mean: {second_half_mean:.1f}")

        # Second half shouldn't be more than 2x worse than first half
        assert second_half_mean < first_half_mean * 2 + 10, (
            f"Accuracy degraded over time: first={first_half_mean:.1f}, "
            f"second={second_half_mean:.1f}"
        )

        # Cleanup
        httpx.delete(f"{base_url}/api/moves/{move_id}", timeout=5)


class TestMultiDuration:
    """Test recording and playback across different durations.

    Verifies frame counts, timing, and data integrity for short, medium,
    and long recordings.
    """

    @pytest.mark.parametrize("duration,label", [
        (1.0, "short-1s"),
        (3.0, "medium-3s"),
        (5.0, "standard-5s"),
        (10.0, "long-10s"),
    ])
    def test_record_duration(self, base_url: str, hw_marionette, duration: float, label: str):
        """Record at various durations, verify frame count and timing."""
        import httpx

        _ensure_idle(base_url)
        resp = httpx.post(
            f"{base_url}/api/record",
            json={"duration": duration, "record_audio": False, "label": label},
            timeout=5,
        )
        assert resp.status_code == 200
        move_id = resp.json()["move_id"]

        _wait_for_mode(base_url, "idle", timeout=COUNTDOWN_SECONDS + duration + 15)

        # Verify the recording
        json_path = hw_marionette._dataset_dir / f"{move_id}.json"
        assert json_path.exists()
        data = json.loads(json_path.read_text())

        timestamps = data["time"]
        frames = data["set_target_data"]
        expected_frames = int(duration * MOTION_SAMPLE_RATE)

        # Frame count within 20% of expected
        assert len(frames) > expected_frames * 0.8, (
            f"{label}: too few frames {len(frames)} (expected ~{expected_frames})"
        )
        assert len(frames) < expected_frames * 1.2, (
            f"{label}: too many frames {len(frames)} (expected ~{expected_frames})"
        )

        # Duration within 20% of expected
        actual_duration = timestamps[-1] - timestamps[0]
        assert actual_duration > duration * 0.8, (
            f"{label}: duration too short {actual_duration:.2f}s (expected ~{duration}s)"
        )

        # Frame rate should be close to 100Hz
        actual_fps = len(frames) / actual_duration if actual_duration > 0 else 0
        assert actual_fps > 80, f"{label}: frame rate too low {actual_fps:.1f}Hz (expected ~100Hz)"
        assert actual_fps < 120, f"{label}: frame rate too high {actual_fps:.1f}Hz (expected ~100Hz)"

        print(f"\n{label}: {len(frames)} frames in {actual_duration:.2f}s = {actual_fps:.1f}Hz")

        # Cleanup
        httpx.delete(f"{base_url}/api/moves/{move_id}", timeout=5)

    @pytest.mark.parametrize("duration,label", [
        (1.0, "play-short-1s"),
        (5.0, "play-standard-5s"),
        (10.0, "play-long-10s"),
    ])
    def test_playback_duration(self, base_url: str, hw_marionette, duration: float, label: str):
        """Record then play back at various durations, verify timing."""
        import httpx

        # First record a move
        _ensure_idle(base_url)
        resp = httpx.post(
            f"{base_url}/api/record",
            json={"duration": duration, "record_audio": False, "label": label},
            timeout=5,
        )
        assert resp.status_code == 200
        move_id = resp.json()["move_id"]
        _wait_for_mode(base_url, "idle", timeout=COUNTDOWN_SECONDS + duration + 15)

        # Now play it back and measure how long it takes
        _ensure_idle(base_url)
        t0 = time.time()
        resp = httpx.post(
            f"{base_url}/api/play",
            json={"move_id": move_id},
            timeout=5,
        )
        assert resp.status_code == 200
        _wait_for_mode(base_url, "idle", timeout=duration + 30)
        playback_time = time.time() - t0

        # Playback time should be close to the recording duration
        # (plus some overhead for goto-start-pose)
        assert playback_time > duration * 0.8, (
            f"{label}: playback too fast {playback_time:.2f}s (expected ~{duration}s)"
        )
        assert playback_time < duration + 10, (
            f"{label}: playback too slow {playback_time:.2f}s (expected ~{duration}s + overhead)"
        )

        print(f"\n{label}: playback took {playback_time:.2f}s (recording was {duration}s)")

        # Cleanup
        httpx.delete(f"{base_url}/api/moves/{move_id}", timeout=5)


class TestPerformance:
    """Timing and performance benchmarks."""

    def test_recording_start_latency(self, base_url: str):
        """Measure time from POST /api/record to countdown mode."""
        import httpx

        _ensure_idle(base_url)
        t0 = time.time()
        resp = httpx.post(
            f"{base_url}/api/record",
            json={"duration": 2.0, "record_audio": False, "label": "latency-test"},
            timeout=5,
        )
        assert resp.status_code == 200

        # Poll until we see countdown
        while time.time() - t0 < 5:
            state = httpx.get(f"{base_url}/api/state", timeout=2).json()
            if state["mode"] in ("countdown", "recording"):
                latency = time.time() - t0
                print(f"\nRecording start latency: {latency*1000:.0f}ms")
                assert latency < 5.0, f"Recording start too slow: {latency:.2f}s"
                break
            time.sleep(0.05)

        # Let it finish
        _wait_for_mode(base_url, "idle", timeout=COUNTDOWN_SECONDS + 2 + 10)

        # Cleanup
        state = httpx.get(f"{base_url}/api/state", timeout=5).json()
        for m in state["moves"]:
            if "latency" in m.get("label", ""):
                httpx.delete(f"{base_url}/api/moves/{m['id']}", timeout=5)

    def test_playback_start_latency(self, base_url: str):
        """Measure time from POST /api/play to playing mode."""
        import httpx

        # Need a move to play — record a quick one
        _ensure_idle(base_url)
        resp = httpx.post(
            f"{base_url}/api/record",
            json={"duration": 1.5, "record_audio": False, "label": "play-latency-src"},
            timeout=5,
        )
        assert resp.status_code == 200
        move_id = resp.json()["move_id"]
        _wait_for_mode(base_url, "idle", timeout=COUNTDOWN_SECONDS + 1.5 + 10)

        # Now measure playback start
        _ensure_idle(base_url)
        t0 = time.time()
        resp = httpx.post(
            f"{base_url}/api/play",
            json={"move_id": move_id},
            timeout=5,
        )
        assert resp.status_code == 200

        while time.time() - t0 < 10:
            state = httpx.get(f"{base_url}/api/state", timeout=2).json()
            if state["mode"] == "playing":
                latency = time.time() - t0
                print(f"\nPlayback start latency: {latency*1000:.0f}ms")
                assert latency < 5.0, f"Playback start too slow: {latency:.2f}s"
                break
            time.sleep(0.05)

        _wait_for_mode(base_url, "idle", timeout=15)

        # Cleanup
        httpx.delete(f"{base_url}/api/moves/{move_id}", timeout=5)

    def test_pose_read_rate(self, base_url: str):
        """Measure how fast we can read poses from the SDK."""
        from reachy_mini import ReachyMini

        _ensure_idle(base_url)
        with ReachyMini(media_backend="no_media") as reachy:
            n_reads = 500
            t0 = time.time()
            for _ in range(n_reads):
                reachy.get_current_head_pose()
            elapsed = time.time() - t0

        rate = n_reads / elapsed
        print(f"\nPose read rate: {rate:.0f} reads/s ({elapsed*1000/n_reads:.1f}ms per read)")
        assert rate > 50, f"Pose read rate too low: {rate:.0f}/s (need >50 for 100Hz recording)"


class TestExistingRecordingPlayback:
    """Play back existing recordings from the dataset."""

    def test_playback_real_audio_recording(self, base_url: str):
        """Find a move with audio and play it back."""
        import httpx

        _ensure_idle(base_url)
        state = httpx.get(f"{base_url}/api/state", timeout=5).json()
        move = next((m for m in state["moves"] if m["has_audio"]), None)
        if move is None:
            pytest.skip("No audio moves available for playback test")

        resp = httpx.post(
            f"{base_url}/api/play",
            json={"move_id": move["id"]},
            timeout=5,
        )
        assert resp.status_code == 200
        _wait_for_mode(base_url, "idle", timeout=move["duration"] + 15)

    def test_playback_real_silent_recording(self, base_url: str):
        """Find a silent move and play it back."""
        import httpx

        _ensure_idle(base_url)
        state = httpx.get(f"{base_url}/api/state", timeout=5).json()
        move = next((m for m in state["moves"] if not m["has_audio"]), None)
        if move is None:
            pytest.skip("No silent moves available for playback test")

        resp = httpx.post(
            f"{base_url}/api/play",
            json={"move_id": move["id"]},
            timeout=5,
        )
        assert resp.status_code == 200
        _wait_for_mode(base_url, "idle", timeout=move["duration"] + 15)


class TestRecordingRoundTrip:
    """Record, read JSON, play back, and verify timing and fidelity."""

    def test_record_then_playback_fidelity(
        self, base_url: str, hw_marionette, hw_reachy,
    ):
        """Record 3s, play back, observe poses, compare trajectories."""
        import httpx
        from pose_utils import compare_trajectories

        duration = 3.0
        _ensure_idle(base_url)

        resp = httpx.post(
            f"{base_url}/api/record",
            json={"duration": duration, "record_audio": False, "label": "roundtrip-fidelity"},
            timeout=5,
        )
        assert resp.status_code == 200
        move_id = resp.json()["move_id"]
        _wait_for_mode(base_url, "idle", timeout=COUNTDOWN_SECONDS + duration + 10)

        # Read the recorded JSON
        json_path = hw_marionette._dataset_dir / f"{move_id}.json"
        assert json_path.exists()
        data = json.loads(json_path.read_text())
        ref_times = data["time"]
        ref_frames = data["set_target_data"]

        # Play back and observe
        _ensure_idle(base_url)
        resp = httpx.post(
            f"{base_url}/api/play",
            json={"move_id": move_id},
            timeout=5,
        )
        assert resp.status_code == 200

        observed_times, observed_frames = _observe_playback(
            base_url, hw_reachy, duration,
        )

        assert len(observed_frames) > 50, (
            f"Too few observed frames: {len(observed_frames)}"
        )

        metrics = compare_trajectories(
            ref_times, ref_frames, observed_times, observed_frames,
        )
        print(f"\nRound-trip fidelity ({len(observed_frames)} frames):")
        print(metrics.summary())

        assert metrics.magic_mean < 50, (
            f"Mean magic distance too high: {metrics.magic_mean:.1f}\n"
            f"{metrics.summary()}"
        )

        # Cleanup
        httpx.delete(f"{base_url}/api/moves/{move_id}", timeout=5)

    def test_record_playback_timing(self, base_url: str, hw_marionette):
        """Record 3s, play back, verify wall-clock duration within 20%."""
        import httpx

        duration = 3.0
        _ensure_idle(base_url)

        resp = httpx.post(
            f"{base_url}/api/record",
            json={"duration": duration, "record_audio": False, "label": "roundtrip-timing"},
            timeout=5,
        )
        assert resp.status_code == 200
        move_id = resp.json()["move_id"]
        _wait_for_mode(base_url, "idle", timeout=COUNTDOWN_SECONDS + duration + 10)

        # Play back and measure
        _ensure_idle(base_url)
        t0 = time.time()
        resp = httpx.post(
            f"{base_url}/api/play",
            json={"move_id": move_id},
            timeout=5,
        )
        assert resp.status_code == 200
        _wait_for_mode(base_url, "idle", timeout=duration + 30)
        playback_time = time.time() - t0

        # Should be within 20% of the recording duration (plus overhead)
        assert playback_time > duration * 0.8, (
            f"Playback too fast: {playback_time:.2f}s (expected ~{duration}s)"
        )
        assert playback_time < duration * 1.2 + 10, (
            f"Playback too slow: {playback_time:.2f}s (expected ~{duration}s + overhead)"
        )
        print(f"\nRound-trip timing: playback took {playback_time:.2f}s for {duration}s recording")

        # Cleanup
        httpx.delete(f"{base_url}/api/moves/{move_id}", timeout=5)


class TestAntennaAndBodyYaw:
    """Verify recording JSON contains antenna and body_yaw data."""

    def test_recording_has_antenna_data(self, base_url: str, hw_marionette):
        """Every frame should have 'antennas' (2-element list)."""
        import httpx

        _ensure_idle(base_url)
        resp = httpx.post(
            f"{base_url}/api/record",
            json={"duration": 2.0, "record_audio": False, "label": "antenna-check"},
            timeout=5,
        )
        assert resp.status_code == 200
        move_id = resp.json()["move_id"]
        _wait_for_mode(base_url, "idle", timeout=COUNTDOWN_SECONDS + 2 + 10)

        json_path = hw_marionette._dataset_dir / f"{move_id}.json"
        data = json.loads(json_path.read_text())
        for frame in data["set_target_data"]:
            assert "antennas" in frame, "Frame missing 'antennas' key"
            assert len(frame["antennas"]) == 2, f"Expected 2 antenna values, got {len(frame['antennas'])}"

        httpx.delete(f"{base_url}/api/moves/{move_id}", timeout=5)

    def test_recording_has_body_yaw(self, base_url: str, hw_marionette):
        """Every frame should have 'body_yaw' (float)."""
        import httpx

        _ensure_idle(base_url)
        resp = httpx.post(
            f"{base_url}/api/record",
            json={"duration": 2.0, "record_audio": False, "label": "bodyyaw-check"},
            timeout=5,
        )
        assert resp.status_code == 200
        move_id = resp.json()["move_id"]
        _wait_for_mode(base_url, "idle", timeout=COUNTDOWN_SECONDS + 2 + 10)

        json_path = hw_marionette._dataset_dir / f"{move_id}.json"
        data = json.loads(json_path.read_text())
        for frame in data["set_target_data"]:
            assert "body_yaw" in frame, "Frame missing 'body_yaw' key"
            assert isinstance(frame["body_yaw"], (int, float)), f"body_yaw should be numeric, got {type(frame['body_yaw'])}"

        httpx.delete(f"{base_url}/api/moves/{move_id}", timeout=5)


class TestPlaybackAntennas:
    """Test synthetic moves with antenna data play back correctly."""

    def test_synthetic_antenna_motion_completes(
        self, base_url: str, hw_marionette, hw_reachy,
    ):
        """Create synthetic move with antenna oscillation, play back."""
        import httpx
        import numpy as np

        duration = 3.0
        _ensure_idle(base_url)

        # Create synthetic move with antenna oscillation
        dt = 1.0 / MOTION_SAMPLE_RATE
        n = int(duration * MOTION_SAMPLE_RATE)
        timestamps = [i * dt for i in range(n)]
        frames = []
        for i in range(n):
            t = i * dt
            antenna_val = 0.3 * np.sin(2 * np.pi * 0.5 * t)
            frames.append({
                "head": np.eye(4).tolist(),
                "antennas": [antenna_val, -antenna_val],
                "body_yaw": 0.0,
                "check_collision": False,
            })

        move_id = "synth-antenna-osc"
        data = {
            "description": "Synthetic antenna oscillation",
            "time": timestamps,
            "set_target_data": frames,
        }
        json_path = hw_marionette._dataset_dir / f"{move_id}.json"
        json_path.write_text(json.dumps(data), encoding="utf-8")
        hw_marionette._refresh_recordings()

        # Play back
        resp = httpx.post(
            f"{base_url}/api/play",
            json={"move_id": move_id},
            timeout=5,
        )
        assert resp.status_code == 200

        _wait_for_mode(base_url, "idle", timeout=duration + 15)

        state = httpx.get(f"{base_url}/api/state", timeout=5).json()
        assert state["mode"] == "idle"

        # Cleanup
        httpx.delete(f"{base_url}/api/moves/{move_id}", timeout=5)


class TestHardwareAudio:
    """Audio recording/playback tests — run last.

    Audio recording may hang on some platforms (known Ubuntu mic bug).
    These tests are isolated at the end so a hang doesn't block the
    silent tests above.  If the server gets stuck, _ensure_idle will
    skip remaining tests instead of cascading failures.
    """

    def test_record_with_audio(self, base_url: str):
        """Record 2s with mic, verify .wav file created."""
        import httpx

        _ensure_idle(base_url)
        resp = httpx.post(
            f"{base_url}/api/record",
            json={"duration": 2.0, "record_audio": True, "label": "hw-audio"},
            timeout=5,
        )
        assert resp.status_code == 200
        move_id = resp.json()["move_id"]

        # Audio recording can take longer due to mic buffering
        try:
            _wait_for_mode(base_url, "idle", timeout=COUNTDOWN_SECONDS + 2 + 20)
        except TimeoutError:
            global _server_stuck
            _server_stuck = True
            pytest.skip(
                "Audio recording timed out — likely mic/audio issue on this platform. "
                "Check that the default system mic is NOT the robot's mic."
            )

        state = httpx.get(f"{base_url}/api/state", timeout=5).json()
        move = next((m for m in state["moves"] if m["id"] == move_id), None)
        assert move is not None, f"Move {move_id} not found"
        assert move["has_audio"] is True

    def test_recording_with_audio_produces_wav(self, base_url: str, hw_marionette):
        """Record 3s with mic, verify WAV file duration matches recording."""
        import httpx

        _ensure_idle(base_url)
        duration = 3.0
        resp = httpx.post(
            f"{base_url}/api/record",
            json={"duration": duration, "record_audio": True, "label": "pipeline-audio"},
            timeout=5,
        )
        assert resp.status_code == 200
        move_id = resp.json()["move_id"]

        try:
            _wait_for_mode(base_url, "idle", timeout=COUNTDOWN_SECONDS + duration + 20)
        except TimeoutError:
            global _server_stuck
            _server_stuck = True
            pytest.skip("Audio recording timed out — mic/audio issue on this platform")

        # Verify WAV exists and has reasonable duration
        wav_path = hw_marionette._dataset_dir / f"{move_id}.wav"
        assert wav_path.exists(), f"WAV file not found: {wav_path}"

        with wave.open(str(wav_path), "rb") as wf:
            wav_duration = wf.getnframes() / wf.getframerate()
            assert wav_duration > duration * 0.7, (
                f"WAV too short: {wav_duration:.2f}s (expected ~{duration}s)"
            )
            # Audio may include countdown buffering, so upper bound is generous.
            # The main check is that audio exists and isn't empty.
            assert wav_duration < duration + COUNTDOWN_SECONDS + 5.0, (
                f"WAV too long: {wav_duration:.2f}s (expected ~{duration}s + countdown)"
            )

        # Also verify the JSON file reports has_audio
        state = httpx.get(f"{base_url}/api/state", timeout=5).json()
        move = next((m for m in state["moves"] if m["id"] == move_id), None)
        assert move is not None
        assert move["has_audio"] is True

        # Cleanup
        httpx.delete(f"{base_url}/api/moves/{move_id}", timeout=5)

    def test_playback_with_audio_completes(self, base_url: str):
        """Play back a move that has audio, verify sound path is exercised."""
        import httpx

        _ensure_idle(base_url)
        state = httpx.get(f"{base_url}/api/state", timeout=5).json()
        move = next((m for m in state["moves"] if m["has_audio"]), None)
        if move is None:
            pytest.skip("No audio move available — audio recording may not be supported")

        resp = httpx.post(
            f"{base_url}/api/play",
            json={"move_id": move["id"]},
            timeout=5,
        )
        assert resp.status_code == 200

        _wait_for_mode(base_url, "idle", timeout=move["duration"] + 15)

        state = httpx.get(f"{base_url}/api/state", timeout=5).json()
        assert state["mode"] == "idle"