"""Test ripetibilità marker-finder: polso fermo, N detection consecutive,
calcola deviazione standard di tvec e rotazione (rvec). Dirige se bias o noise."""

from __future__ import annotations

import math
import statistics as stats
import time
from typing import Any

import requests

ORC = "http://localhost:8890"
H1 = "http://localhost:8767"
N_FRAMES = 30
SETTLE_S = 0.5  # tra detection


def post(url, body=None, t=10):
    r = requests.post(url, json=body or {}, timeout=t)
    return r.json()


def get(url, t=5):
    return requests.get(url, timeout=t).json()


def go_home():
    return post(f"{H1}/arms/go_neutral", {})


def detect():
    return post(f"{ORC}/api/calibration/preview", {})


def main() -> int:
    print("=== repeatability test (left, H1) — N=", N_FRAMES, flush=True)
    print("[0] go_neutral + settle", flush=True)
    print(go_home(), flush=True)
    time.sleep(3.0)
    pos = get(f"{ORC}/api/wrist")["left_wrist"]["position"]
    print(f"polso fermo a {[round(v,4) for v in pos]}", flush=True)

    tvecs: list[list[float]] = []
    rvecs: list[list[float]] = []
    quats: list[list[float]] = []
    centers: list[list[float]] = []
    timings: list[float] = []
    found = 0
    for i in range(N_FRAMES):
        d = detect()
        if not d.get("found"):
            print(f"  frame {i}: NOT FOUND", flush=True)
            continue
        found += 1
        pose = d["pose_camera"]
        tvecs.append(pose["tvec"])
        rvecs.append(pose["rvec"])
        quats.append(pose["quaternion_xyzw"])
        centers.append(d["center_px"])
        timings.append(d.get("timings_ms", {}).get("marker_detect", 0))
        if i < 3 or i == N_FRAMES - 1:
            print(f"  frame {i}: tvec={[round(v,4) for v in pose['tvec']]}", flush=True)
        time.sleep(SETTLE_S)

    print(f"\nfound: {found}/{N_FRAMES}", flush=True)
    if found < 5:
        return 1

    def std(xs):
        return stats.pstdev(xs) if len(xs) > 1 else 0.0

    def fmt(label, vals):
        return f"  {label}: mean=[{vals[0]:+.4f},{vals[1]:+.4f},{vals[2]:+.4f}]  std=[{std([t[0] for t in tvecs])*1000:.2f},{std([t[1] for t in tvecs])*1000:.2f},{std([t[2] for t in tvecs])*1000:.2f}]mm"

    mean_t = [sum(t[i] for t in tvecs) / len(tvecs) for i in range(3)]
    std_t_mm = [std([t[i] for t in tvecs]) * 1000 for i in range(3)]
    mean_r = [sum(r[i] for r in rvecs) / len(rvecs) for i in range(3)]
    std_r_deg = [std([r[i] for r in rvecs]) * 180 / math.pi for i in range(3)]
    mean_c = [sum(c[i] for c in centers) / len(centers) for i in range(2)]
    std_c_px = [std([c[i] for c in centers]) for i in range(2)]

    radial_t = math.sqrt(sum(s ** 2 for s in std_t_mm))
    radial_r = math.sqrt(sum(s ** 2 for s in std_r_deg))

    print("\n--- statistics su T_cam_marker (camera frame) ---", flush=True)
    print(f"mean tvec  : [{mean_t[0]:+.4f}, {mean_t[1]:+.4f}, {mean_t[2]:+.4f}] m", flush=True)
    print(f"std  tvec  : [{std_t_mm[0]:.2f}, {std_t_mm[1]:.2f}, {std_t_mm[2]:.2f}] mm   (radial {radial_t:.2f} mm)", flush=True)
    print(f"std  rvec  : [{std_r_deg[0]:.3f}, {std_r_deg[1]:.3f}, {std_r_deg[2]:.3f}] deg (radial {radial_r:.3f} deg)", flush=True)
    print(f"mean center px: [{mean_c[0]:.1f}, {mean_c[1]:.1f}]   std: [{std_c_px[0]:.2f}, {std_c_px[1]:.2f}] px", flush=True)
    print(f"detect latency mean: {sum(timings)/len(timings):.1f} ms", flush=True)

    print("\n--- diagnosi ---", flush=True)
    if radial_t < 5.0:
        print(f"σ_trans={radial_t:.1f}mm < 5mm → noise basso, residuo dominato da BIAS (verosimilmente intrinsics).", flush=True)
    elif radial_t > 20.0:
        print(f"σ_trans={radial_t:.1f}mm > 20mm → noise alto, marker o lighting.", flush=True)
    else:
        print(f"σ_trans={radial_t:.1f}mm intermedio: noise + bias entrambi presenti.", flush=True)
    return 0


if __name__ == "__main__":
    raise SystemExit(main())