r"""Unit tests for the template-based course fitter — synthetic figure-8 tracks with KNOWN buoys, so we
can assert the fitter recovers the true positions (ground truth the real races don't have)."""
from __future__ import annotations

import math
import sys
from pathlib import Path

sys.path.insert(0, str(Path(__file__).resolve().parent))
from course_templates import FIGURE8, expand_template, validate_rectangle  # noqa: E402
from fit_marks import to_latlon  # noqa: E402
from fit_template import align_apexes, fit_buoys, fit_template_course  # noqa: E402

REF = (59.32, 18.10)
# A rectangle like the real gsys course: short gate sides ~40 m, long beats ~200 m. Clockwise A-C-D-B.
BUOYS = {"A": (60.0, -100.0), "C": (-140.0, -80.0), "D": (-150.0, 40.0), "B": (50.0, 20.0)}


def _densify(a, b, step=5.0, jitter=0.0, rng=0):
    """Points along a→b every ~step m, with a deterministic pseudo-jitter (no Math.random)."""
    d = math.dist(a, b)
    n = max(1, int(d / step))
    out = []
    for k in range(n + 1):
        t = k / n
        x = a[0] + (b[0] - a[0]) * t
        y = a[1] + (b[1] - a[1]) * t
        if jitter:
            j = ((rng * 2654435761 + k * 40503) % 1000) / 1000.0 - 0.5
            x += j * jitter
            y += ((rng * 97 + k * 7) % 1000 / 1000.0 - 0.5) * jitter
        out.append((x, y))
    return out


def _figure8_track(buoys, *, boat=0, jitter=1.0, marks="CDAB CD"):
    """A boat sailing start→C→D→A→B→C→D→finish through the buoys (xy), lightly jittered per boat."""
    start_mid = ((buoys["A"][0] + buoys["B"][0]) / 2, (buoys["A"][1] + buoys["B"][1]) / 2)
    seq = [(start_mid[0], start_mid[1] - 30)]                     # approach from below the line
    off = (boat * 3.0, -boat * 2.0)                              # per-boat lane offset
    for m in ("C", "D", "A", "B", "C", "D"):
        seq.append((buoys[m][0] + off[0], buoys[m][1] + off[1]))
    seq.append((start_mid[0], start_mid[1] - 30))                # cross back to finish
    track = []
    for i in range(len(seq) - 1):
        track += _densify(seq[i], seq[i + 1], jitter=jitter, rng=boat * 17 + i)
    return track


def _latlon_track(track_xy):
    return [to_latlon(x, y, REF) for x, y in track_xy]


def test_align_apexes_monotonic_picks_nearest_in_order():
    targets = [(0.0, 0.0), (100.0, 0.0)]
    apexes = [(2.0, 1.0), (50.0, 40.0), (98.0, -1.0)]           # mark0 near apex0, mark1 near apex2
    got = align_apexes(apexes, targets, radius=20.0)
    assert [mi for mi, _ in got] == [0, 1]
    assert got[0][1] == (2.0, 1.0) and got[1][1] == (98.0, -1.0)


def test_recovers_known_buoys_from_perfect_bootstrap():
    tracks = [_figure8_track(BUOYS, boat=b, jitter=1.5) for b in range(4)]
    tracks_xy = tracks                                           # already xy
    buoy_xy, counts = fit_buoys(tracks_xy, FIGURE8.mark_seq, dict(BUOYS))
    # recovers each buoy to within ~a boat-length (boats round a few m wide + the 2 m clearance push)
    for b in "ABCD":
        assert math.dist(buoy_xy[b], BUOYS[b]) < 8.0, (b, buoy_xy[b], BUOYS[b])
    assert counts["C"] >= 4 and counts["D"] >= 4                # C,D rounded twice each → >=1/boat


def test_recovers_from_offset_bootstrap():
    tracks_xy = [_figure8_track(BUOYS, boat=b, jitter=1.5) for b in range(4)]
    boot = {b: (p[0] + 12, p[1] - 10) for b, p in BUOYS.items()}   # 15 m offset bootstrap
    buoy_xy, _ = fit_buoys(tracks_xy, FIGURE8.mark_seq, boot)
    for b in "ABCD":
        assert math.dist(buoy_xy[b], BUOYS[b]) < 8.0, (b, buoy_xy[b])


def test_rectangle_validation():
    ok, m = validate_rectangle(BUOYS)
    assert ok and m["short_diff"] < 0.25 and m["long_diff"] < 0.25
    bad = dict(BUOYS, D=(-150.0, 300.0))                        # stretch one long side
    assert not validate_rectangle(bad)[0]


def test_fit_template_course_shape():
    tracks = [_latlon_track(_figure8_track(BUOYS, boat=b, jitter=1.5)) for b in range(4)]
    course, qa = fit_template_course(tracks, REF, FIGURE8, init_buoys_ll={
        b: to_latlon(p[0], p[1], REF) for b, p in BUOYS.items()})
    assert len(course["waypoints"]) == 6                       # C,D,A,B,C,D
    assert course["startLine"]["port"] and course["finishLine"]["starboard"]
    assert qa["rect_ok"], qa["rect"]


def test_expand_template_gate_orientation():
    ll = {b: to_latlon(p[0], p[1], REF) for b, p in BUOYS.items()}
    c = expand_template(FIGURE8, ll)
    # start line port=B, starboard=A ; finish line port=A, starboard=B (same physical line, reversed)
    assert c["startLine"]["port"] == c["finishLine"]["starboard"]
    assert c["startLine"]["starboard"] == c["finishLine"]["port"]
    # rounding hand: M1,M2,M5,M6 (C,D) to port; M3,M4 (A,B) to starboard
    assert [w["rounding"] for w in c["waypoints"]] == ["port", "port", "starboard", "starboard", "port", "port"]