#!/usr/bin/env python3
"""Phase 2 demo: Wigner-D vs dense rotation accuracy + timing."""

from __future__ import annotations

import sys
import time
from pathlib import Path

import numpy as np

sys.path.insert(0, str(Path(__file__).resolve().parents[1]))

from hoa64 import encode_points, rotate_yaw_pitch_roll, doa_from_intensity
from hoa64.analysis import angular_error_deg
from hoa64.rotate import rotate_source_directions


def main() -> None:
    az0, el0 = 25.0, -12.0
    yaw, pitch, roll = 40.0, 15.0, -10.0
    a0 = encode_points([az0], [el0], [1.0])

    az1, el1 = rotate_source_directions(az0, el0, yaw=yaw, pitch=pitch, roll=roll)
    a_gt = encode_points([float(az1)], [float(el1)], [1.0])

    a_w = rotate_yaw_pitch_roll(
        a0, yaw=yaw, pitch=pitch, roll=roll, method="wigner"
    )
    a_d = rotate_yaw_pitch_roll(
        a0, yaw=yaw, pitch=pitch, roll=roll, method="dense", n_azi=72, n_el=36
    )

    rel_w = np.linalg.norm(a_w - a_gt) / np.linalg.norm(a_gt)
    rel_d = np.linalg.norm(a_d - a_gt) / np.linalg.norm(a_gt)
    print(f"Ground-truth source after rot: az={float(az1):.2f} el={float(el1):.2f}")
    print(f"Wigner rel error vs re-encode: {rel_w:.3e}")
    print(f"Dense  rel error vs re-encode: {rel_d:.3e}")

    az_w, el_w = doa_from_intensity(a_w)
    err = angular_error_deg(float(az1), float(el1), az_w, el_w)
    print(f"Intensity DOA after Wigner: ({az_w:.2f},{el_w:.2f})  err={err:.3f}°")

    # timing
    for _ in range(20):
        rotate_yaw_pitch_roll(a0, yaw=yaw, pitch=pitch, roll=roll, method="wigner")
    t0 = time.perf_counter()
    n = 200
    for _ in range(n):
        rotate_yaw_pitch_roll(a0, yaw=yaw, pitch=pitch, roll=roll, method="wigner")
    tw = (time.perf_counter() - t0) / n

    t0 = time.perf_counter()
    n2 = 5
    for _ in range(n2):
        rotate_yaw_pitch_roll(
            a0, yaw=yaw, pitch=pitch, roll=roll, method="dense", n_azi=48, n_el=24
        )
    td = (time.perf_counter() - t0) / n2
    print(f"Timing: wigner={tw*1e3:.3f} ms  dense={td*1e3:.3f} ms  speedup≈{td/tw:.0f}×")
    print("Phase 2: Wigner-D rotation is default for pose loops and tools.")


if __name__ == "__main__":
    main()