#!/usr/bin/env python3
"""
test_mprim_gen.py
------------------
Test tự động cho generator: sinh cả 3 config mẫu ra thư mục tạm rồi kiểm tra
output theo đúng các ràng buộc SBPL enforce lúc nạp file + chất lượng hình học.

Chạy:
    python3 test_mprim_gen.py

Các kiểm tra:
  1. Header khớp số primitive thực tế; parse trọn vẹn không dư token.
  2. additionalactioncostmult là SỐ NGUYÊN (SBPL đọc fscanf %d).
  3. Pose cuối rời rạc hóa ra đúng endpose_c (đúng phép check của
     ReadinMotionPrimitive — sai là SBPL từ chối cả file).
  4. Heading cuối khớp CHÍNH XÁC góc rời rạc đích (bug l=0 cũ gây lệch 10 độ).
  5. Khoảng cách 2 pose liên tiếp <= 1.05 cell (footprint sweep không sót cell)
     và ĐỀU trong từng primitive (max/min <= 1.5).
  6. Vi phạm nonholonomic (góc lệch vector di chuyển vs heading) < 9 độ với
     differential/ackermann — bắt lỗi kiểu cung lùi sai hình học (28 độ cũ).
  7. Không có primitive suy biến, không trùng endpose trong cùng start angle.
  8. Config sai (costmult lẻ, spacing > resolution, ackermann turn_radius <
     min_turning_radius) phải bị validate() chặn.
"""

import math
import os
import sys
import tempfile

sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))

from mprim_config import RobotConfig, KinematicType
from mprim_core import generate_mprim_file
from generate_mprim import KINEMATICS_MAP, load_config

CONFIG_DIR = os.path.join(os.path.dirname(os.path.abspath(__file__)), "configs")
CHECKS = []


def check(name, cond, detail=""):
    CHECKS.append((name, cond, detail))
    status = "PASS" if cond else "FAIL"
    print(f"  [{status}] {name}" + (f" — {detail}" if detail and not cond else ""))


def parse(path):
    toks = open(path).read().split()
    it = iter(toks)
    def nxt():
        return next(it)
    assert nxt() == "resolution_m:"
    res = float(nxt())
    assert nxt() == "numberofangles:"
    nang = int(nxt())
    assert nxt() == "totalnumberofprimitives:"
    ntot = int(nxt())
    prims = []
    while True:
        try:
            tag = nxt()
        except StopIteration:
            break
        assert tag == "primID:"
        pid = int(nxt())
        assert nxt() == "startangle_c:"
        sa = int(nxt())
        assert nxt() == "endpose_c:"
        end = (int(nxt()), int(nxt()), int(nxt()))
        assert nxt() == "additionalactioncostmult:"
        cm = nxt()
        assert nxt() == "intermediateposes:"
        n = int(nxt())
        poses = [(float(nxt()), float(nxt()), float(nxt())) for _ in range(n)]
        prims.append(dict(pid=pid, sa=sa, end=end, cm=cm, poses=poses))
    return res, nang, ntot, prims


def ang_diff(a, b):
    return (a - b + math.pi) % (2 * math.pi) - math.pi


def contxy2disc(x, cell):
    return int(math.floor(x / cell))


def verify_file(path, kinematic, min_turning_radius=None):
    res, nang, ntot, prims = parse(path)
    step = 2 * math.pi / nang

    check("header khớp số primitive parse được", ntot == len(prims),
          f"header {ntot} != parsed {len(prims)}")

    bad_cm = [p["cm"] for p in prims if not p["cm"].lstrip("-").isdigit()]
    check("costmult toàn số nguyên (fscanf %d)", not bad_cm, f"lẻ: {sorted(set(bad_cm))}")

    # phép check của SBPL ReadinMotionPrimitive
    bad_end = 0
    for p in prims:
        lx, ly, lth = p["poses"][-1]
        got = (contxy2disc(res / 2 + lx, res), contxy2disc(res / 2 + ly, res),
               int(round((lth % (2 * math.pi)) / step)) % nang)
        if got != p["end"]:
            bad_end += 1
    check("pose cuối rời rạc hóa đúng endpose_c (điều kiện SBPL nạp file)", bad_end == 0,
          f"{bad_end} primitive sai")

    worst_heading = max(abs(ang_diff(p["poses"][-1][2], p["end"][2] * step)) for p in prims)
    check("heading cuối khớp chính xác góc đích", worst_heading < 1e-4,
          f"lệch tối đa {math.degrees(worst_heading):.2f} độ")

    worst_gap, worst_ratio = 0.0, 1.0
    for p in prims:
        gaps = [math.hypot(x1 - x0, y1 - y0)
                for (x0, y0, _), (x1, y1, _) in zip(p["poses"], p["poses"][1:])]
        moving = [g for g in gaps if g > 1e-9]
        if moving:
            worst_gap = max(worst_gap, max(moving))
            worst_ratio = max(worst_ratio, max(moving) / min(moving))
    check("bước giữa 2 pose <= 1.05 cell (footprint sweep không sót)",
          worst_gap <= res * 1.05, f"max {worst_gap:.4f}m")
    check("spacing đều trong từng primitive (max/min <= 1.5)", worst_ratio <= 1.5,
          f"tỉ lệ {worst_ratio:.2f}")

    if kinematic in (KinematicType.DIFFERENTIAL, KinematicType.ACKERMANN):
        worst_nh = 0.0
        for p in prims:
            if p["end"][0] == 0 and p["end"][1] == 0:
                continue
            for (x0, y0, t0), (x1, y1, t1) in zip(p["poses"], p["poses"][1:]):
                dx, dy = x1 - x0, y1 - y0
                if math.hypot(dx, dy) < 1e-9:
                    continue
                hd = math.atan2(dy, dx)
                th = 0.5 * (t0 + t1)
                worst_nh = max(worst_nh, min(abs(ang_diff(hd, th)),
                                             abs(ang_diff(hd, th + math.pi))))
        check("vi phạm nonholonomic < 9 độ", math.degrees(worst_nh) < 9.0,
              f"max {math.degrees(worst_nh):.2f} độ")

    degenerate = [p for p in prims
                  if p["end"] == (0, 0, p["sa"])]
    check("không có primitive suy biến (0,0,không đổi góc)", not degenerate,
          f"{len(degenerate)} cái")

    dup = 0
    for sa in range(nang):
        seen = set()
        for p in prims:
            if p["sa"] != sa:
                continue
            if p["end"] in seen:
                dup += 1
            seen.add(p["end"])
    check("không trùng endpose trong cùng start angle", dup == 0, f"{dup} cặp trùng")


def test_configs_generate():
    with tempfile.TemporaryDirectory() as tmp:
        for fname in ("differential.py", "omnidirectional.py", "ackermann.py"):
            cfg = load_config(os.path.join(CONFIG_DIR, fname))
            out = os.path.join(tmp, cfg.name + ".mprim")
            n, warnings = generate_mprim_file(cfg, KINEMATICS_MAP[cfg.kinematic_type], out)
            print(f"\n== {fname}: {n} primitives, {len(warnings)} cảnh báo ==")
            for w in warnings:
                print(f"   (warn) {w}")
            verify_file(out, cfg.kinematic_type,
                        min_turning_radius=getattr(cfg, "min_turning_radius_m", None))


def test_validation_rejects_bad_configs():
    print("\n== validate() phải chặn config sai ==")

    def must_fail(name, **overrides):
        base = dict(name="t", kinematic_type=KinematicType.DIFFERENTIAL)
        base.update(overrides)
        try:
            RobotConfig(**base).validate()
            check(name, False, "validate() đã KHÔNG chặn")
        except ValueError:
            check(name, True)

    must_fail("chặn costmult không nguyên", forwardandturncostmult=2.5)
    must_fail("chặn costmult < 1", backwardcostmult=0)
    must_fail("chặn pose_spacing_m > resolution", pose_spacing_m=0.08)
    must_fail("chặn forward quá ngắn (quantize về 0,0)", forward_short_m=0.02)
    must_fail("chặn turn_steps rỗng", turn_steps=())
    must_fail("chặn ackermann turn_radius < min_turning_radius",
              kinematic_type=KinematicType.ACKERMANN,
              min_turning_radius_m=0.45, turn_radius_m=0.30)


if __name__ == "__main__":
    test_configs_generate()
    test_validation_rejects_bad_configs()
    failed = [c for c in CHECKS if not c[1]]
    print(f"\n{'='*60}\nTổng: {len(CHECKS)} check, {len(failed)} FAIL")
    sys.exit(1 if failed else 0)