"""Blødgør skarpe sving (>45°) i et AutoDrive-vejnet ved at tilføje kurve-omveje.

Eksisterende waypoints og id'er røres ikke (kun nye noder tilføjes), så markører og
mod-referencer holder. For hvert sving p->b->c:
  - kanten p-b deles i Pin (r meter før b), kanten b-c deles i Pout (r meter efter b)
  - Pin -> kvadratisk Bézier (kontrolpunkt b) -> Pout, med nok punkter til at hvert
    knæk er <= MAX_STEP grader
  - er det omvendte sving (c,b,p) også på listen, bliver kurven tovejs
"""
import json, math, re, sys

MAX_STEP = 20.0     # grader pr. knæk inde i kurven
R_CAP = 10.0        # max benlængde (m) -> max afvigelse fra hjørnet ~3,2 m
R_FRAC = 0.45       # max andel af en kant der bruges til benet
R_MIN = 1.5         # kortere ben end dette -> spring over


def read(path):
    raw = open(path, "rb").read()
    bom = raw.startswith(b"\xef\xbb\xbf")
    s = raw.decode("utf-8-sig")
    w0 = s.index("<waypoints>"); w1 = s.index("</waypoints>")
    w = s[w0:w1]
    g = lambda t: re.search(rf"<{t}>(.*?)</{t}>", w, re.S).group(1)
    ids = [int(v) for v in g("id").split(",")]
    assert ids == list(range(1, len(ids) + 1)), "id'er er ikke 1..N"
    xs = [float(v) for v in g("x").split(",")]
    ys = [float(v) for v in g("y").split(",")]
    zs = [float(v) for v in g("z").split(",")]
    lst = lambda t: [[int(v) for v in e.split(",") if v != "-1"] for e in g(t).split(";")]
    out, inc = lst("out"), lst("incoming")
    fl = [int(v) for v in g("flags").split(",")]
    N = {i: dict(x=xs[k], y=ys[k], z=zs[k], out=out[k], inc=inc[k], fl=fl[k]) for k, i in enumerate(ids)}
    return dict(s=s, bom=bom, w0=w0, w1=w1, w=w, N=N)


def write(doc, path):
    N = doc["N"]; ids = sorted(N)
    f3 = lambda v: "%.3f" % v
    joinl = lambda L: ",".join(map(str, L)) if L else "-1"
    parts = {
        "id": ",".join(map(str, ids)),
        "x": ",".join(f3(N[i]["x"]) for i in ids),
        "y": ",".join(f3(N[i]["y"]) for i in ids),
        "z": ",".join(f3(N[i]["z"]) for i in ids),
        "out": ";".join(joinl(N[i]["out"]) for i in ids),
        "incoming": ";".join(joinl(N[i]["inc"]) for i in ids),
        "flags": ",".join(str(N[i]["fl"]) for i in ids),
    }
    w = doc["w"]
    for t, v in parts.items():
        w = re.sub(rf"<{t}>.*?</{t}>", lambda m, v=v, t=t: f"<{t}>{v}</{t}>", w, count=1, flags=re.S)
    s = doc["s"][:doc["w0"]] + w + doc["s"][doc["w1"]:]
    data = s.encode("utf-8")
    open(path, "wb").write((b"\xef\xbb\xbf" if doc["bom"] else b"") + data)


def xz(n):
    return (n["x"], n["z"])


def dist(a, b):
    return math.hypot(a["x"] - b["x"], a["z"] - b["z"])


def new_node(N, x, y, z, fl):
    i = max(N) + 1
    N[i] = dict(x=x, y=y, z=z, out=[], inc=[], fl=fl)
    return i


def link(N, a, b):
    if b not in N[a]["out"]:
        N[a]["out"].append(b)
    if a not in N[b]["inc"]:
        N[b]["inc"].append(a)


def apply_splits(N, requests):
    """requests: {(u,v) u<v: set(afstand fra u)} -> {(u,v,afstand): ny node}."""
    made = {}
    for (u, v), ds in requests.items():
        L = dist(N[u], N[v])
        pts = []
        for d in sorted(ds):
            if pts and d - pts[-1] < 0.5:
                made[(u, v, d)] = made[(u, v, pts[-1])]; continue
            t = d / L
            m = new_node(N, *(N[u][a] + (N[v][a] - N[u][a]) * t for a in "xyz"),
                         N[v]["fl"] if t > 0.5 else N[u]["fl"])
            made[(u, v, d)] = m; pts.append(d)
        chain = [u] + [made[(u, v, d)] for d in pts] + [v]
        for a, b in ((u, v), (v, u)):
            if b in N[a]["out"]:
                seq = chain if a == u else chain[::-1]
                N[a]["out"] = [seq[1] if o == b else o for o in N[a]["out"]]
                N[b]["inc"] = [seq[-2] if o == a else o for o in N[b]["inc"]]
                for x, y in zip(seq[1:-1], seq[2:]):
                    link(N, x, y)
                N[seq[1]]["inc"].append(a) if a not in N[seq[1]]["inc"] else None
    return made


def smooth(doc, turns):
    N = doc["N"]; want = set(turns); plan = []; req = {}; seen = set()
    for p, b, c in turns:
        if (p, b, c) in seen:
            continue
        dual = (c, b, p) in want
        seen.add((p, b, c)); seen.add((c, b, p)) if dual else None
        r = min(R_CAP, R_FRAC * dist(N[p], N[b]), R_FRAC * dist(N[b], N[c]))
        if r < R_MIN:
            continue
        keys = []
        for o in (p, c):
            u, v = min(o, b), max(o, b)
            d = r if u == b else dist(N[u], N[v]) - r
            req.setdefault((u, v), set()).add(round(d, 3)); keys.append((u, v, round(d, 3)))
        plan.append((p, b, c, dual, r, keys))
    orig = {i: dict(N[i]) for i in N}
    made_split = apply_splits(N, req)
    made = []
    for p, b, c, dual, r, keys in plan:
        pin, pout = made_split[keys[0]], made_split[keys[1]]
        P0, P1, P2 = N[pin], N[b], N[pout]
        th = turn_angle(orig[p], orig[b], orig[c])
        n = max(2, math.ceil(th / MAX_STEP) + 1)
        chain = [pin]
        for k in range(1, n):
            t = k / n
            q = lambda a: (1 - t) ** 2 * P0[a] + 2 * (1 - t) * t * P1[a] + t * t * P2[a]
            chain.append(new_node(N, q("x"), q("y"), q("z"), N[b]["fl"]))
        chain.append(pout)
        for a, bb in zip(chain, chain[1:]):
            link(N, a, bb)
            if dual:
                link(N, bb, a)
        made.append(dict(turn=[p, b, c], dual=dual, angle=round(th, 1), r=round(r, 1), nodes=chain))
    return made


def turn_angle(A, B, C):
    v1 = (B["x"] - A["x"], B["z"] - A["z"]); v2 = (C["x"] - B["x"], C["z"] - B["z"])
    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)))))


if __name__ == "__main__":
    src, sharp_json, dst, log = sys.argv[1:5]
    doc = read(src)
    n0 = len(doc["N"])
    turns = [tuple(x[:3]) for x in json.load(open(sharp_json)) if x[4] > 45.5]
    made = smooth(doc, turns)
    write(doc, dst)
    json.dump(made, open(log, "w"), indent=0)
    print(f"sving: {len(turns)}  kurver lavet: {len(made)}  (tovejs {sum(m['dual'] for m in made)})"
          f"  nye noder: {len(doc['N']) - n0}  sprunget over: {len(turns) - len(made) - sum(m['dual'] for m in made)}")