"""Fjern sving > MAX_TURN på AD-ruter mellem markører ved at tilføje kurve-omveje.

Kun nye waypoints tilføjes; eksisterende id'er, koordinater og forbindelser bevares
(kanter der deles får et mellempunkt på samme linje). AD's pathfinder vælger selv
kurven, fordi den er kortere og har mindre vinkel-straf.

Brug: adfix.py <ind.xml> <ud.xml> <log.json>
"""
import json, math, re, sys, collections
sys.path.insert(0, __file__.rsplit("/", 1)[0])
from adsmooth import read, write, dist, new_node, link, apply_splits
from adroutes import transit, dijkstra, angle, is_reverse

MAX_TURN = 45.5          # grænse (45° præcis er tilladt)
CURVE_STEP = 7.9         # max knæk i kurven (<8° => AD regner 27 km/h, så kurven vinder)
R_TRY = (8.0, 6.0, 4.5, 3.0, 2.0, 1.5, 1.0)
TARGET_EDGE = 7.9        # max knæk overalt i kurven inkl. ind-/udgang


def markers(doc):
    return [int(float(i)) for i in re.findall(r"<mm\d+>\s*<id>([\d.]+)</id>", doc["s"])]


def node_path(best, pre, target):
    s = best.get(target); seq = []
    while s:
        seq.append(s); s = pre[s]
    seq.reverse()
    return [seq[0][0]] + [b for _, b in seq] if seq else []


def collect(N, mks):
    """Returnér {(p,b,c): (antal, path, index_of_b)} for sving > MAX_TURN på ruter."""
    T = transit(N); found = {}
    cnt = collections.Counter()
    for st in mks:
        best, pre = dijkstra(N, T, st)
        for tg in mks:
            if tg == st or tg not in best:
                continue
            path = node_path(best, pre, tg)
            backing = False
            for i in range(1, len(path) - 1):
                p, b, c = path[i - 1], path[i], path[i + 1]
                a = angle(N, p, b, c)
                # bakning: starter på en bakforbindelse (p ikke i b.incoming), slutter når retningen vender (>90°)
                if p not in N[b]["inc"]:
                    backing = True
                if backing:
                    if a > 90:
                        backing = False
                    continue
                # vendepunktet lige før baklængs-kørsel (bakforbindelse c -> næste) er bevidst skarpt
                if i + 2 < len(path) and c not in N[path[i + 2]]["inc"]:
                    continue
                if a > MAX_TURN and not is_reverse(N, p, b, c):
                    key = (p, b, c) + window(N, path, i)
                    cnt[key] += 1
                    back = sum(dist(N[path[j]], N[path[j + 1]]) for j in range(i))
                    fwd = sum(dist(N[path[j]], N[path[j + 1]]) for j in range(i, len(path) - 1))
                    cur = found.get(key)
                    if cur is None:
                        found[key] = [(path, i, back), (path, i, fwd)]
                    else:
                        if back < cur[0][2]: cur[0] = (path, i, back)
                        if fwd < cur[1][2]: cur[1] = (path, i, fwd)
    return {k: (cnt[k], v[0], v[1]) for k, v in found.items()}


def window(N, path, i, reach=8.5):
    """Rutens noder inden for `reach` meter før og efter path[i] (skelner grene)."""
    lo = i; acc = 0.0
    while lo > 0 and acc < reach:
        acc += dist(N[path[lo - 1]], N[path[lo]]); lo -= 1
    hi = i; acc = 0.0
    while hi < len(path) - 1 and acc < reach:
        acc += dist(N[path[hi]], N[path[hi + 1]]); hi += 1
    return (tuple(path[lo:i]), tuple(path[i + 1:hi + 1]))


def point_along(N, path, i, R, direction):
    """Gå R meter fra path[i] bagud (-1) eller fremad (+1). Returnér
    (u, v, d_from_u) for punktet på kanten u->v i kørselsretning, eller None."""
    acc = 0.0; j = i
    while True:
        k = j + direction
        if k < 0 or k >= len(path):
            return None
        a, b = (path[k], path[j]) if direction < 0 else (path[j], path[k])
        L = dist(N[a], N[b])
        if acc + L >= R:
            rest = R - acc                      # meter fra path[j] mod path[k]
            return (a, b, L - rest) if direction < 0 else (a, b, rest)
        acc += L; j = k


def tangent(N, u, v):
    L = dist(N[u], N[v])
    return ((N[v]["x"] - N[u]["x"]) / L, (N[v]["z"] - N[u]["z"]) / L)


def pos_on(N, u, v, d):
    t = d / dist(N[u], N[v])
    return {a: N[u][a] + (N[v][a] - N[u][a]) * t for a in "xyz"}


def ang2(v1, v2):
    l1 = math.hypot(*v1); l2 = math.hypot(*v2)
    if l1 < 1e-6 or l2 < 1e-6:
        return 0.0
    return math.degrees(math.acos(max(-1, min(1, (v1[0] * v2[0] + v1[1] * v2[1]) / (l1 * l2)))))


def design(N, back, fwd, R):
    """Kubisk Bézier fra punkt R før b til R efter b. back/fwd = (path, i, plads) for
    de ruter der har mindst plads før hhv. efter b. Returnér plan eller None."""
    a = point_along(N, back[0], back[1], R, -1); b = point_along(N, fwd[0], fwd[1], R, +1)
    if not a or not b:
        return None
    (u1, v1, d1), (u2, v2, d2) = a, b
    # ligger punktet (næsten) oven i en eksisterende node, bruges noden selv
    snap = lambda u, v, d: ("node", u) if d < 0.3 else ("node", v) if dist(N[u], N[v]) - d < 0.3 else ("split", u, v, d)
    e_in, e_out = snap(u1, v1, d1), snap(u2, v2, d2)
    if e_in[0] == "node":
        d1 = 0.0 if e_in[1] == u1 else dist(N[u1], N[v1])
    if e_out[0] == "node":
        d2 = 0.0 if e_out[1] == u2 else dist(N[u2], N[v2])
    P0 = pos_on(N, u1, v1, d1); P3 = pos_on(N, u2, v2, d2)
    t0 = tangent(N, u1, v1); t3 = tangent(N, u2, v2)
    # snappet til en node: retningen skal matche rutens kant IND i (start) / UD af (slut) noden
    bp, fp = back[0], fwd[0]
    if e_in[0] == "node" and e_in[1] == u1:
        k = bp.index(u1)
        if k == 0:
            return None
        t0 = tangent(N, bp[k - 1], u1)
    if e_out[0] == "node" and e_out[1] == v2:
        k = fp.index(v2)
        if k >= len(fp) - 1:
            return None
        t3 = tangent(N, v2, fp[k + 1])
    chord = math.hypot(P3["x"] - P0["x"], P3["z"] - P0["z"])
    k = chord * 0.4
    P1 = {"x": P0["x"] + t0[0] * k, "z": P0["z"] + t0[1] * k, "y": P0["y"] + (P3["y"] - P0["y"]) / 3}
    P2 = {"x": P3["x"] - t3[0] * k, "z": P3["z"] - t3[1] * k, "y": P0["y"] + 2 * (P3["y"] - P0["y"]) / 3}
    total = ang2(t0, t3)
    n = max(3, math.ceil(total / CURVE_STEP) + 2)
    while True:
        pts = []
        for s in range(n + 1):
            t = s / n
            pts.append({ax: (1 - t) ** 3 * P0[ax] + 3 * (1 - t) ** 2 * t * P1[ax] + 3 * (1 - t) * t * t * P2[ax] + t ** 3 * P3[ax] for ax in "xyz"})
        # tjek alle knæk inkl. ind- og udgang
        dirs = [t0] + [(pts[s + 1]["x"] - pts[s]["x"], pts[s + 1]["z"] - pts[s]["z"]) for s in range(n)] + [t3]
        worst = max(ang2(dirs[s], dirs[s + 1]) for s in range(len(dirs) - 1))
        if worst <= TARGET_EDGE:
            break
        n += 1
        if n > 80 or chord / n < 0.15:
            return None
    return dict(end_in=e_in, end_out=e_out, split_in=(u1, v1, d1), split_out=(u2, v2, d2), inner=pts[1:-1], R=R, total=round(total, 1), worst=round(worst, 1))


def run(src, dst, logp):
    doc = read(src); N = doc["N"]; n0 = len(N)
    mks = [m for m in markers(doc) if m in N]
    turns = collect(N, mks)
    order = sorted(turns.items(), key=lambda kv: -kv[1][0])
    plans = []; covered = set(); skipped = []
    for key, (cnt, back, fwd) in order:
        p, b, c = key[:3]
        if key in covered:
            continue
        path, i = back[0], back[1]
        plan = None
        for R in R_TRY:
            if R > back[2] - 0.3 or R > fwd[2] - 0.3:
                continue
            plan = design(N, back, fwd, R)
            if plan:
                break
        if not plan:
            skipped.append([p, b, c, cnt, round(angle(N, p, b, c), 1)]); continue
        # marker alle sving i vinduet som dækket (fx 63°+27° i samme hjørne)
        lo = path.index(plan["split_in"][1]) if plan["split_in"][1] in path else i
        hi = path.index(plan["split_out"][0]) if plan["split_out"][0] in path else i
        for j in range(max(1, lo), min(len(path) - 1, hi + 1)):
            covered.add((path[j - 1], path[j], path[j + 1]) + window(N, path, j))
        plan.update(turn=[p, b, c], count=cnt); plans.append(plan)
    # 1) del kanter (samlet pr. kant), 2) byg kurver
    req = {}; keyof = []
    for pl in plans:
        ks = []
        for e in (pl["end_in"], pl["end_out"]):
            if e[0] == "node":
                ks.append(("node", e[1])); continue
            _, u, v, d = e
            lo, hi = min(u, v), max(u, v)
            dd = round(d if u == lo else dist(N[u], N[v]) - d, 3)
            req.setdefault((lo, hi), set()).add(dd); ks.append((lo, hi, dd))
        keyof.append(ks)
    made = apply_splits(N, req)
    for pl, ks in zip(plans, keyof):
        res = lambda k: k[1] if k[0] == "node" else made[k]
        a, b = res(ks[0]), res(ks[1])
        # bivej (bit 1) kun hvis begge ender er bivej — ellers ganger AD kurven med 20
        sub = N[a]["fl"] & N[b]["fl"] & 1
        fl = (N[pl["turn"][1]]["fl"] & ~1) | sub
        chain = [a] + [new_node(N, q["x"], q["y"], q["z"], fl) for q in pl["inner"]] + [b]
        for x, y in zip(chain, chain[1:]):
            link(N, x, y)
        pl["nodes"] = chain
        del pl["inner"]
    write(doc, dst)
    json.dump(dict(plans=plans, skipped=skipped), open(logp, "w"))
    print(f"sving>{MAX_TURN}°: {len(turns)}  kurver: {len(plans)}  dækket af andre kurver: {len(turns)-len(plans)-len(skipped)}"
          f"  sprunget over: {len(skipped)}  nye noder: {len(N)-n0}")
    return len(turns)


if __name__ == "__main__":
    run(*sys.argv[1:4])