r"""
Sequence-anchored buoy-position fitter (decision: declare the shape, measure only the positions).

Given a course SHAPE template (`course_templates.py`) and the boat tracks, fit the physical buoy
positions by exploiting the guarantee that **every boat rounds every mark in order**:

  1. Walk each boat's track and detect its roundings IN THE KNOWN ORDER (`mark_seq`) — closest approach
     to the next expected mark, committed when the mark goes abeam. This ignores spurious turns (tacks,
     jockeying, the figure-8 crossing) that wrecked the old "top-N densest clusters" fitter.
  2. Each physical buoy = the mean of every rounding apex assigned to it, across all boats AND all the
     marks that share it (e.g. the figure-8 windward buoy C is averaged over M1 and M5 of every boat).
  3. Iterate (re-detect against the refined marks) to a fixed point, push marks >=2 m off the tracks,
     and validate the rectangle.

Robust because: buoy count + roles are DECLARED (no top-N, no east/west guess); positions are averaged
over many roundings (variance ~1/sqrt(N)); and it needs no wind seed (unlike apex clustering). Needs a
rough bootstrap (the race's existing/prior marks, or a cluster fit) good enough to assign roundings.

Pure geometry; takes/returns plain lat-lon.
"""
from __future__ import annotations

import collections
import itertools
import math
import sys
from pathlib import Path

_HERE = Path(__file__).resolve().parent
sys.path.insert(0, str(_HERE))
from course_templates import CourseTemplate, expand_template, validate_rectangle  # noqa: E402
from fit_marks import clear_marks, cluster, to_latlon, turn_apexes  # noqa: E402
from segment_gsys import to_xy  # noqa: E402

Pt = tuple[float, float]


def bootstrap_buoys(tracks_xy: list[list[Pt]], tpl: CourseTemplate, *, cluster_radius: float = 18.0,
                    assign_r: float = 25.0, min_angle_deg: float = 22.0, top_n: int = 8
                    ) -> dict[str, Pt]:
    """From-scratch buoy bootstrap that LABELS the marks by the sailing order (not geography). Cluster
    every boat's turn-apexes into candidate buoys, then for each boat read the ORDER it visits those
    candidates and vote each sequence position (M1..Mn) onto a candidate. Position → buoy via the
    template's `mark_seq`, so a re-rounded buoy (e.g. figure-8 windward at M1 and M5) collects both votes.
    Returns {buoy_id: (x,y)} for every buoy that got a vote."""
    apex_all = [a for t in tracks_xy for a in turn_apexes(t, min_angle_deg=min_angle_deg)]
    cand = [c for c, _n in cluster(apex_all, cluster_radius)[:top_n]]
    if not cand:
        return {}
    votes: dict[str, collections.Counter] = {b: collections.Counter() for b in tpl.buoys}
    for t in tracks_xy:
        vseq: list[int] = []
        for a in turn_apexes(t, min_angle_deg=min_angle_deg):
            ci = min(range(len(cand)), key=lambda k: math.dist(a, cand[k]))
            if math.dist(a, cand[ci]) > assign_r:
                continue
            if not vseq or vseq[-1] != ci:                   # dedupe consecutive → distinct visits in order
                vseq.append(ci)
        for pos, ci in enumerate(vseq[:tpl.n_marks]):
            votes[tpl.mark_seq[pos]][ci] += 1
    return {b: cand[votes[b].most_common(1)[0][0]] for b in tpl.buoys if votes[b]}


def align_apexes(apexes: list[Pt], targets: list[Pt], *, radius: float
                 ) -> list[tuple[int, Pt]]:
    """Optimal MONOTONIC assignment of a subset of a boat's ordered turn-apexes to the ordered mark
    `targets`, each mark taking the (order-preserving) apex within `radius` that minimises total offset.
    A DP (edit-distance-like: skip an apex, skip a mark, or assign) — robust to the many spurious apexes
    (tacks, jockeying, the figure-8 crossing) that a greedy nearest-pick misaligns. Returns
    [(mark_index, apex_point)] for the assigned marks (in order)."""
    n, m = len(targets), len(apexes)
    INF = math.inf
    skip_mark = radius                                   # leaving a mark unassigned costs `radius`, so any
    #                                                      apex within radius is preferred over skipping it
    dp = [[INF] * (m + 1) for _ in range(n + 1)]
    bt = [[None] * (m + 1) for _ in range(n + 1)]
    for j in range(m + 1):
        dp[0][j] = 0.0                                   # 0 marks placed → cost 0 (skip any apexes)
    for i in range(1, n + 1):
        dp[i][0] = dp[i - 1][0] + skip_mark              # no apexes left → mark i unassigned (penalised)
        bt[i][0] = ("skipmark", None)
        for j in range(1, m + 1):
            best, arg = dp[i][j - 1], ("skipapex", None)          # skip apex j-1 (free — spurious turn)
            if dp[i - 1][j] + skip_mark < best:                   # skip mark i-1 (leave unassigned)
                best, arg = dp[i - 1][j] + skip_mark, ("skipmark", None)
            d = math.dist(targets[i - 1], apexes[j - 1])
            if d <= radius and dp[i - 1][j - 1] + d < best:        # assign apex j-1 to mark i-1
                best, arg = dp[i - 1][j - 1] + d, ("assign", j - 1)
            dp[i][j], bt[i][j] = best, arg
    out: list[tuple[int, Pt]] = []
    i, j = n, m
    while i > 0:
        act, aj = bt[i][j]
        if act == "assign":
            out.append((i - 1, apexes[aj]))
            i, j = i - 1, aj
        elif act == "skipmark":
            i -= 1
        else:
            j -= 1
    out.reverse()
    return out


def fit_buoys(tracks_xy: list[list[Pt]], mark_seq: tuple[str, ...], init_xy: dict[str, Pt],
              *, iters: int = 8, radius: float = 28.0, min_angle_deg: float = 20.0
              ) -> tuple[dict[str, Pt], dict[str, int]]:
    """Iteratively refine buoy positions. Each boat's hard-turn apexes (`turn_apexes`, no wind filter —
    every rounding is a hard turn; a LOW angle threshold keeps gentle roundings as candidates) are aligned
    to the mark sequence (`align_apexes`); each physical buoy = the mean of every apex assigned to it
    across all boats and all the marks that share it. `radius` is kept below the narrowest gate so adjacent
    marks are never confused (the monotonic alignment + tight radius pick the right apex from the many
    candidates). Returns (buoy_xy, sample_count per buoy)."""
    boat_apexes = [turn_apexes(t, min_angle_deg=min_angle_deg) for t in tracks_xy]
    buoy_xy = dict(init_xy)
    counts: dict[str, int] = {}
    for _ in range(iters):
        acc: dict[str, list[Pt]] = {b: [] for b in buoy_xy}
        targets = [buoy_xy[b] for b in mark_seq]
        for apexes in boat_apexes:
            for mi, pt in align_apexes(apexes, targets, radius=radius):
                acc[mark_seq[mi]].append(pt)
        new = {b: ((sum(p[0] for p in pts) / len(pts), sum(p[1] for p in pts) / len(pts)) if pts
                   else buoy_xy[b]) for b, pts in acc.items()}
        counts = {b: len(pts) for b, pts in acc.items()}
        moved = max(math.hypot(new[b][0] - buoy_xy[b][0], new[b][1] - buoy_xy[b][1]) for b in buoy_xy)
        buoy_xy = new
        if moved < 0.5:
            break
    return buoy_xy, counts


def _seg_cross(p1: Pt, p2: Pt, q1: Pt, q2: Pt) -> bool:
    def ccw(a, b, c):
        return (c[1] - a[1]) * (b[0] - a[0]) > (b[1] - a[1]) * (c[0] - a[0])
    return ccw(p1, q1, q2) != ccw(p2, q1, q2) and ccw(p1, p2, q1) != ccw(p1, p2, q2)


def start_crossings(timed_track: list[tuple[int, Pt]], A: Pt, B: Pt, C: Pt, *,
                    extend: float = 0.4, commit: int = 45, toward: bool = True) -> list[int]:
    """Seconds at which one boat crosses the line A-B (extended `extend` past each buoy for fit slack).
    `toward` True → crossing onto the COURSE side (the windward mark C's side), committed ~`commit` s
    (the START); False → crossing back off the course (a FINISH). `commit<=0` skips the commitment test
    (use for finishes, where the track may end right after)."""
    dx, dy = B[0] - A[0], B[1] - A[1]
    Ae = (A[0] - dx * extend, A[1] - dy * extend)
    Be = (B[0] + dx * extend, B[1] + dy * extend)
    def side(p):
        return dx * (p[1] - A[1]) - dy * (p[0] - A[0])
    want = (side(C) > 0) if toward else (side(C) <= 0)
    out = []
    for i in range(1, len(timed_track)):
        (_s0, p0), (s1, p1) = timed_track[i - 1], timed_track[i]
        if _seg_cross(p0, p1, Ae, Be) and (side(p1) > 0) == want:
            if commit <= 0:
                out.append(s1)
                continue
            j = min(range(len(timed_track)), key=lambda k: abs(timed_track[k][0] - (s1 + commit)))
            if (side(timed_track[j][1]) > 0) == want and timed_track[j][0] >= s1 + commit - 10:
                out.append(s1)
    return out


def detect_start(timed_tracks: dict[str, list[tuple[int, Pt]]], A: Pt, B: Pt, C: Pt, *,
                 window: int = 60) -> tuple[int | None, int | None, dict[str, int]]:
    """The race START: the earliest window of <= `window` s in which EVERY boat crosses the start line
    A-B onto the course. (Mid-race M3/M4 and finish crossings are spread over minutes; only the mass
    start is this tight.) Returns (gun_sec = first crossing in the window, spread_s, {boat: n_crossings})."""
    bc = {lab: start_crossings(t, A, B, C) for lab, t in timed_tracks.items()}
    counts = {lab: len(cs) for lab, cs in bc.items()}
    allc = sorted((s, lab) for lab, cs in bc.items() for s in cs)
    for s, _lab in allc:
        seen: dict[str, int] = {}
        for s2, lab2 in allc:
            if s2 < s:
                continue
            if s2 > s + window:
                break
            seen.setdefault(lab2, s2)
        if len(seen) == len(bc) and len(bc) > 0:
            return min(seen.values()), max(seen.values()) - min(seen.values()), counts
    return None, None, counts


def _label_rectangle(pts: list[Pt], tol: float = 0.35) -> list[tuple[Pt, Pt]] | None:
    """If four points form a rectangle (opposite sides within `tol`), return its TWO SHORT sides (the
    gates) as [(p,q),(p,q)]; else None. Points are hull-ordered around their centroid."""
    cx = sum(p[0] for p in pts) / 4
    cy = sum(p[1] for p in pts) / 4
    order = sorted(pts, key=lambda p: math.atan2(p[1] - cy, p[0] - cx))
    d = [math.dist(order[i], order[(i + 1) % 4]) for i in range(4)]     # 4 sides, cyclic
    def rel(a, b):
        return abs(a - b) / max(a, b, 1e-6)
    if rel(d[0], d[2]) > tol or rel(d[1], d[3]) > tol:                  # opposite sides must match
        return None
    if d[0] + d[2] <= d[1] + d[3]:                                      # gates = shorter opposite pair
        return [(order[0], order[1]), (order[2], order[3])]
    return [(order[1], order[2]), (order[3], order[0])]


def _orient_options(gates: list[tuple[Pt, Pt]]) -> list[dict[str, Pt]]:
    """Every way to label the two gates as the start gate (A,B) vs the windward gate (C,D), and the ends
    within each — the completion score decides which is right."""
    g0, g1 = gates
    out = []
    for ab, cd in ((g0, g1), (g1, g0)):
        for a, b in (ab, (ab[1], ab[0])):
            for c, dd in (cd, (cd[1], cd[0])):
                out.append({"A": a, "B": b, "C": c, "D": dd})
    return out


def global_fit(timed_tracks: dict[str, list[tuple[int, Pt]]], *, top_n: int = 16,
               min_angle_deg: float = 22.0, cluster_r: float = 18.0
               ) -> tuple[dict[str, Pt] | None, int, dict]:
    """Find the buoy positions by GLOBAL best-fit: pool every boat's turn-apexes into candidate buoys,
    then over all 4-candidate rectangles and orientations pick the one that makes the MOST boats sail a
    COMPLETE figure-8 (round all six marks in order + cross start & finish), tie-broken by the tightest
    rectangle. Returns (buoys{A,B,C,D} or None, n_complete, rect_metrics)."""
    xy_tracks = [[p for _s, p in t] for t in timed_tracks.values()]
    apex = [a for t in xy_tracks for a in turn_apexes(t, min_angle_deg=min_angle_deg)]
    cand = [c for c, _n in cluster(apex, cluster_r)[:top_n]]
    best: tuple[tuple, dict, dict] | None = None
    min_beat = 80.0                                          # reject tiny degenerate rectangles (real
    #                                                          figure-8 beats are ~100-230 m, not ~30 m)
    for quad in itertools.combinations(cand, 4):
        gates = _label_rectangle(list(quad))
        if gates is None:
            continue
        for a in _orient_options(gates):
            A, B, C, D = a["A"], a["B"], a["C"], a["D"]
            beat = (math.dist(A, C) + math.dist(B, D)) / 2  # the long side (a beat)
            if beat < min_beat:
                continue
            wp = [C, D, A, B, C, D]
            n = sum(1 for t in timed_tracks.values() if race_window(t, wp, A, B, C) is not None)
            if n == 0:
                continue
            rect = validate_rectangle(a)[1]
            # most boats complete; then the LARGEST such course (the real one spans the whole area);
            # then the tightest rectangle.
            score = (n, round(beat / 10), -(rect["short_diff"] + rect["long_diff"]))
            if best is None or score > best[0]:
                best = (score, a, rect)
    if best is None:
        return None, 0, {}
    return best[1], best[0][0], best[2]


def race_window(timed_track: list[tuple[int, Pt]], waypoints_xy: list[Pt], A: Pt, B: Pt, C: Pt, *,
                round_r: float = 28.0, leave: float = 12.0) -> tuple[int, int] | None:
    """One boat's CLEAN race, or None if it doesn't complete. Returns (start_sec, finish_sec): the boat's
    own start-line crossing (onto the course), then it must round every waypoint IN ORDER, then cross the
    finish line back off the course. Boats that miss a mark or whose data ends mid-race return None."""
    starts = start_crossings(timed_track, A, B, C, commit=20, toward=True)
    if not starts:
        return None
    start_sec = starts[0]
    secs = [s for s, _ in timed_track]
    pos = [p for _, p in timed_track]
    i0 = next((i for i, s in enumerate(secs) if s >= start_sec), 0)
    ti, min_d, apex_i, last_round = 0, math.inf, None, None
    for i in range(i0, len(pos)):
        if ti >= len(waypoints_xy):
            break
        d = math.dist(pos[i], waypoints_xy[ti])
        if d < min_d:
            min_d, apex_i = d, i
        if apex_i is not None and min_d < round_r and d > min_d + leave:
            last_round = secs[apex_i]
            ti, min_d, apex_i = ti + 1, math.inf, None
    if ti < len(waypoints_xy):                       # didn't round every mark in order
        return None
    fins = [s for s in start_crossings(timed_track, A, B, C, commit=0, toward=False) if s > last_round]
    if not fins:                                     # never crossed the finish
        return None
    return start_sec, min(fins)


def fit_template_course(tracks_latlon: list[list[Pt]], ref: Pt, tpl: CourseTemplate,
                        *, init_buoys_ll: dict[str, Pt] | None = None, name: str | None = None,
                        min_clear_m: float = 2.0, iters: int = 6) -> tuple[dict, dict]:
    """Fit a course of shape `tpl` from the tracks. Bootstraps from `init_buoys_ll` (rough prior marks) if
    given, else labels the buoys from scratch by sailing order (`bootstrap_buoys`). Returns (engine course
    block, qa dict with rectangle metrics / sample counts). qa['rect_ok'] is False if the fit is unusable."""
    tracks_xy = [[to_xy(la, lo, ref) for la, lo in t] for t in tracks_latlon]
    init_xy = ({b: to_xy(ll[0], ll[1], ref) for b, ll in init_buoys_ll.items()}
               if init_buoys_ll else bootstrap_buoys(tracks_xy, tpl))
    if len(init_xy) < len(tpl.buoys):                        # bootstrap couldn't place every buoy
        return {}, {"rect_ok": False, "rect": {}, "counts": {}, "reason": "bootstrap incomplete"}
    buoy_xy, counts = fit_buoys(tracks_xy, tpl.mark_seq, init_xy, iters=iters)
    cleared = clear_marks([buoy_xy[b] for b in tpl.buoys], tracks_xy, min_m=min_clear_m)
    buoy_xy = {b: cleared[i] for i, b in enumerate(tpl.buoys)}
    ok, rect = validate_rectangle(buoy_xy, tpl.rect_tol)
    buoy_ll = {b: to_latlon(xy[0], xy[1], ref) for b, xy in buoy_xy.items()}
    course = expand_template(tpl, buoy_ll, name=name)      # rounding sides default to "port" (as today)
    return course, {"rect": rect, "counts": counts, "rect_ok": ok}