"""Savegame2 30/9: parkering til alle køretøjer + Sell Slurry-vognfejlen.

- REX-vintraktorer: parkDestination = deres vinredskabs parkering (faste sæt).
- Motorkøretøjer uden plads (FH16 x3, Finsnitter): ledig bås omdøbes "P <navn>", parkDestination sættes.
  Køretøjet flyttes ikke — AutoDrive bakker det ind, når det parkeres.
- Frakoblede vogne uden plads: ledig bås + "Connect P <navn>" (nyt punkt delt ind på bakkesporet ved
  vognens trækøje) og vognen stilles ind i båsen, bagende 0,5 m fra P.
- Sell Slurry 2: ploven kobles af og stilles på "P Plov"; flowets vogn bliver gylletanken på
  "P Sell Slurry vogn 2".

Skriver <fil>.new i savegame-mappen. Brug: parkall.py <savegame-mappe>
"""
import math, os, re, sys, zipfile
import xml.etree.ElementTree as ET
sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
from adsmooth import read, write, apply_splits, dist

SG = sys.argv[1].rstrip("/") + "/"
GAME = os.path.expanduser("~/.local/share/Steam/steamapps/common/Farming Simulator 25/")
MODS = os.path.expanduser("~/FS25-data/mods/")
REAR_GAP = 0.5            # vognens bagende så langt fra P (m)
KINGPIN_BACK = 1.2        # sættevogn: kongebolt bag forenden (ukalibreret skøn)

# motorkøretøjer: driverName -> kolonne; vogne: fil -> (navn, kolonne, sættevogn?)
MOTOR = ["FH16 780 1", "FH16 780 2", "Milk Sale", "Finsnitter"]
TRAILERS = {"dpw1800": ("DPW 1800", "syd", False), "bigBodyS750": ("Big Body 750", "syd", False),
            "gtw330": ("Overlæssevogn GTW 330", "syd", False), "steelDropDeck": ("Steel Drop Deck", "vest", True)}


def xmltext(fn):
    if fn.startswith("data/"):
        return open(GAME + fn, encoding="utf-8", errors="ignore").read()
    mod, rest = fn.replace("$moddir$", "").split("/", 1)
    return zipfile.ZipFile(MODS + mod + ".zip").read(rest).decode("utf-8", "ignore")


def size_of(fn):
    m = re.search(r"<size [^>]*>", xmltext(fn)).group(0)
    L = float(re.search(r'length="([\d.]+)"', m).group(1))
    lo = re.search(r'lengthOffset="(-?[\d.]+)"', m)
    return L, float(lo.group(1)) if lo else 0.0


def heading(el):
    r = [float(t) for t in el.find("component").get("rotation").split()]
    return math.radians(r[1] + (180 if abs(r[0]) > 90 else 0))


def angn(a):
    return (a + math.pi) % (2 * math.pi) - math.pi


doc = read(SG + "AutoDrive_config.xml"); N = doc["N"]
mk = {}   # mm-index -> dict(wp, name, group)
for m in re.finditer(r"<mm(\d+)>\s*<id>([\d.]+)</id>\s*<name>([^<]*)</name>\s*<group>([^<]*)</group>", doc["s"]):
    mk[int(m.group(1))] = dict(wp=int(float(m.group(2))), name=m.group(3), group=m.group(4))
byname = {m["name"]: i for i, m in mk.items()}

VT = ET.parse(SG + "vehicles.xml"); VR = VT.getroot()
veh = {v.get("uniqueId"): v for v in VR.iter("vehicle")}
def adname(v):
    ad = v.find("AutoDrive"); return ad.get("driverName") if ad is not None else None
def park_of(v):
    ad = v.find("AutoDrive"); p = ad.get("parkDestination") if ad is not None else None
    return int(p) if p and p.lstrip("-").isdigit() and int(p) > 0 else None
def set_park(v, idx):
    ad = v.find("AutoDrive")
    if ad is None:
        last = list(v)[-1]; ad = ET.SubElement(v, "AutoDrive"); ad.tail = last.tail; last.tail = "\n        "
    ad.set("parkDestination", str(idx))
parent = {a.get("attachedVehicleUniqueId"): v.get("uniqueId") for v in VR.iter("vehicle") for a in v.iter("attachedImplement")}


def bay_axis(p_wp):
    """Retning fra P ud mod båsens munding, og sporets noder sorteret efter afstand fra P."""
    P = N[p_wp]
    # nabo-kæden fra P: gå til nabo længst væk, 6 skridt, og brug den retning
    cur, prev = p_wp, None
    for _ in range(6):
        nb = [x for x in set(N[cur]["out"]) | set(N[cur]["inc"]) if x != prev]
        if not nb: break
        nxt = max(nb, key=lambda x: dist(N[x], P)); prev, cur = cur, nxt
    e = math.atan2(N[cur]["x"] - P["x"], N[cur]["z"] - P["z"])
    return e


def along_lat(p_wp, e, x, z):
    dx, dz = x - N[p_wp]["x"], z - N[p_wp]["z"]
    return dx * math.sin(e) + dz * math.cos(e), dx * math.cos(e) - dz * math.sin(e)


def split_point_on_track(p_wp, e, d):
    """(u, v, afstand fra min(u,v)) for punktet d m fra P på bakkesporet."""
    track = []
    for i, n in N.items():
        a, l = along_lat(p_wp, e, n["x"], n["z"])
        if abs(l) < 0.35 and -0.1 < a < 26:
            track.append((a, i))
    track.sort()
    for (a1, i1), (a2, i2) in zip(track, track[1:]):
        linked = i2 in N[i1]["out"] or i1 in N[i2]["out"]
        if linked and a1 <= d <= a2:
            u, v = min(i1, i2), max(i1, i2)
            du = d - (a1 if u == i1 else a2)
            return u, v, round(abs(du), 3)
    raise SystemExit(f"fandt ikke sporet {d:.1f} m fra P (wp {p_wp})")


def ground_offset(v):
    c = v.find("component"); x, y, z = [float(t) for t in c.get("position").split()]
    near = min(N.values(), key=lambda n: (n["x"] - x) ** 2 + (n["z"] - z) ** 2)
    return y - near["y"]


def place(v, p_wp, e, root_along):
    """Stil køretøjet med roden root_along m fra P langs e, front mod mundingen."""
    tx = N[p_wp]["x"] + root_along * math.sin(e); tz = N[p_wp]["z"] + root_along * math.cos(e)
    ty = N[p_wp]["y"] + ground_offset(v)
    cs = v.findall("component"); p0 = [float(t) for t in cs[0].get("position").split()]
    d = angn(e - heading(v))
    for c in cs:
        q = [float(t) for t in c.get("position").split()]; r = [float(t) for t in c.get("rotation").split()]
        rx, rz = q[0] - p0[0], q[2] - p0[2]
        nx = tx + rx * math.cos(d) + rz * math.sin(d); nz = tz - rx * math.sin(d) + rz * math.cos(d)
        c.set("position", "%.3f %.3f %.3f" % (nx, q[1] - p0[1] + ty, nz))
        r[1] = (r[1] + math.degrees(d) + 180) % 360 - 180
        c.set("rotation", "%.2f %.2f %.2f" % tuple(r))


free = {"syd": [], "vest": []}
for i, m in mk.items():
    if re.fullmatch(r"P Ledig \d+", m["name"]):
        x, z = N[m["wp"]]["x"], N[m["wp"]]["z"]
        if -2328 < z < -2320 and x > 1003: free["syd"].append(i)
        elif 905 < x < 920: free["vest"].append(i)
free["syd"].sort(key=lambda i: N[mk[i]["wp"]]["x"])
free["vest"].sort(key=lambda i: N[mk[i]["wp"]]["z"])
renames, connects, log = {}, [], []

# 1) REX -> vinredskabets plads
for v in VR.iter("vehicle"):
    if v.get("filename", "").endswith("seriesREX4.xml") and park_of(v) is None:
        kids = [a.get("attachedVehicleUniqueId") for a in v.iter("attachedImplement")]
        pk = [park_of(veh[k]) for k in kids if k in veh and park_of(veh[k])]
        if pk:
            set_park(v, pk[0]); log.append(f"{adname(v)} -> {mk[pk[0]]['name']} (fast sæt)")

# 2) motorkøretøjer
for v in VR.iter("vehicle"):
    if adname(v) in MOTOR and park_of(v) is None:
        slot = free["vest"].pop(0); renames[slot] = "P " + adname(v); set_park(v, slot)
        log.append(f"{adname(v)} -> {mk[slot]['name']} => 'P {adname(v)}'")

# 3) frakoblede vogne
req, pending = {}, []
for v in VR.iter("vehicle"):
    f = v.get("filename", "").split("/")[-1].replace(".xml", "")
    if f in TRAILERS and park_of(v) is None and v.get("uniqueId") not in parent:
        name, col, semi = TRAILERS[f]
        slot = free[col].pop(0); pw = mk[slot]["wp"]; e = bay_axis(pw)
        L, lo = size_of(v.get("filename"))
        root = REAR_GAP - lo + L / 2
        d = REAR_GAP + L - (KINGPIN_BACK if semi else 0.0)
        u, w, du = split_point_on_track(pw, e, d)
        req.setdefault((u, w), set()).add(du)
        place(v, pw, e, root); set_park(v, slot)
        renames[slot] = "P " + name
        pending.append((slot, name, (u, w, du), d))
        log.append(f"{name} ({f}) -> {mk[slot]['name']} => 'P {name}', Connect {d:.2f} m, rod {root:.2f} m")

# 4) Sell Slurry: plov af, gylletank som flowets vogn
plow_park = byname["P Plov"]; tank_park = byname["P Sell Slurry vogn 2"]
plow = next(v for v in VR.iter("vehicle") if park_of(v) == plow_park)
tank = next(v for v in VR.iter("vehicle") if park_of(v) == tank_park)
tractor = veh[parent[plow.get("uniqueId")]]
aj = tractor.find("attacherJoints")
for a in list(aj):
    if a.get("attachedVehicleUniqueId") == plow.get("uniqueId"):
        aj.remove(a)
pw = mk[plow_park]["wp"]; e = bay_axis(pw)
cw = mk[byname["Connect P Plov"]]["wp"]; dcon, _ = along_lat(pw, e, N[cw]["x"], N[cw]["z"])
L, lo = size_of(plow.get("filename"))
place(plow, pw, e, dcon - 0.1 - lo - L / 2)      # liftmonteret: forkant + 0,1 m = Connect
log.append(f"plov koblet af '{adname(tractor)}' og stillet på P Plov (Connect {dcon:.2f} m)")
RP = SG + "ADSmartPickup_runs.xml"; runs = open(RP, encoding="utf-8-sig", newline="").read()   # bevar CRLF
m = re.search(r'(<run [^>]*name="Sell Slurry"[^>]*wagons=")([^"]*)(")', runs)
pairs = dict(p.split("=") for p in m.group(2).split(";") if "=" in p)
pairs[tractor.get("uniqueId")] = tank.get("uniqueId")
runs = runs[:m.start(2)] + ";".join(f"{k}={pairs[k]}" for k in sorted(pairs)) + runs[m.end(2):]
log.append(f"Sell Slurry: '{adname(tractor)}' får gylletanken på P Sell Slurry vogn 2")

# skriv AD-config: del spor, så markørnavne + nye Connect-markører
made = apply_splits(N, req)
write(doc, SG + "AutoDrive_config.xml.new")
s = open(SG + "AutoDrive_config.xml.new", encoding="utf-8-sig").read()
bom = open(SG + "AutoDrive_config.xml", "rb").read(3) == b"\xef\xbb\xbf"
for slot, new in renames.items():
    s, n = re.subn(r"(<mm%d>\s*<id>[\d.]+</id>\s*<name>)[^<]*(</name>\s*<group>)[^<]*(</group>)" % slot,
                   r"\g<1>%s\g<2>Parkering\g<3>" % new, s)
    assert n == 1, slot
nxt = max(mk) + 1; add = ""
for slot, name, key, d in pending:
    add += ("        <mm%d>\n            <id>%d.000000</id>\n            <name>Connect P %s</name>\n"
            "            <group>Connect</group>\n        </mm%d>\n") % (nxt, made[key], name, nxt)
    nxt += 1
i = s.rfind("    </mapmarker>"); s = s[:i] + add + s[i:]
open(SG + "AutoDrive_config.xml.new", "wb").write((b"\xef\xbb\xbf" if bom else b"") + s.encode("utf-8"))
body = ET.tostring(VR, encoding="unicode")
open(SG + "vehicles.xml.new", "w", encoding="utf-8").write('<?xml version="1.0" encoding="utf-8" standalone="no"?>\n' + body + "\n")
open(SG + "ADSmartPickup_runs.xml.new", "w", encoding="utf-8", newline="").write("\ufeff" + runs)
print("\n".join(log)); print("skrevet .new")