sps_skel/lib/kreisel_junctions.py

# -*- coding: utf-8 -*-
"""
kreisel_junctions.py - Weichenknoten zwischen Kreiseln aus der Geometrie.

Zwei Kreisel, die sich beruehren, bilden eine Weichenstelle: am Beruehrpunkt
laufen beide Umlaeufe in dieselbe Richtung, ein Traeger kann dort von einem
Kreisel auf den anderen wechseln. dxfmakros exportiert solche Stellen als
"ILS Weiche" (Merkmal "Erzeugt aus: Kreisel-Beruehrpunkt") - aber nicht
immer: in data/export.csv fehlen die Weichen zwischen Kreisel 9/8, 2/3 und
3/4, obwohl sich die Kreisel dort beruehren. Darum wird hier jede Weiche aus
der Kreisel-Geometrie bestimmt und eine exportierte ILS Weiche nur noch ihrem
Beruehrpunkt zugeordnet (Id und Name).

Geometrie
---------
Ein Kreisel ist eine Stadion-Form: zwei Umlenkungen (Halbkreise vom Radius
r) im Abstand L, verbunden durch die beiden geraden Bahnen. Mittelpunkt ist
die CSV-Position, die Laengsachse zeigt in Richtung "Drehung". r und L kommen
aus der Boundingbox (kurze Seite = 2r, lange Seite = L + 2r), bei schraeger
Lage aus dem Merkmal "Abstand (Kreiselachse A - Kreiselachse) in Meter".
Jeder Punkt auf dem Umlauf hat eine Laufposition s (mm) in Drehrichtung
("Drehrichtung" UZS/GUZ) - damit ist "stromauf"/"stromab" definiert.

Weichenknoten und steuernde Separatoren
---------------------------------------
Beruehrpunkte, die auf derselben Umlenkung eines Kreisels liegen, bilden
EINEN Weichenknoten (Kreisel 5 beruehrt mit seiner Umlenkung Kreisel 6 und
Kreisel 4 - zwei Weichen, eine Uebergabestelle). Ein Separator steuert den
Knoten, wenn er auf einem der beteiligten Kreisel unmittelbar stromauf vor
dem Knoten liegt: vom Knoten gegen die Drehrichtung zurueckgegangen, kommt
zuerst dieser Separator und keine andere Weiche. Liegt dazwischen eine
fremde Weiche, steuert der Separator diese, nicht den Knoten.

Aus der Anzahl steuernder Separatoren und Weichen ergibt sich der TRO-Typ
(tro_flow.derive_tros, Schritt 3c): 1Sep1Swi, 2Sep1Swi, 2Sep2Swi, 3Sep2Swi.
"""

from __future__ import annotations

import math
from dataclasses import dataclass, field

from material_flow import (
    LANE_LEFT,
    LANE_RIGHT,
    ROTATION_CCW,
    ROTATION_CW,
    Element,
    _as_float,
    _merkmal,
)
from parameter import get_float, get_int

# Zahlenwerte aus cfg/parameter.ini, Abschnitt [kreisel_junctions] - Bedeutung
# und Einheit stehen dort (lib/parameter.py).
_P = "kreisel_junctions"
CONTACT_TOLERANCE = get_float(_P, "contact_tolerance")
SEPARATOR_TOLERANCE = get_float(_P, "separator_tolerance")
WEICHE_TOLERANCE = get_float(_P, "weiche_tolerance")
SAMPLE_STEP = get_float(_P, "sample_step")
MIN_SAMPLES = get_int(_P, "min_samples")
CONTACT_TIE = get_float(_P, "contact_tie")
MAX_CONTACT_LENGTH = get_float(_P, "max_contact_length")
END_TOLERANCE = get_float(_P, "end_tolerance")
DEFAULT_RADIUS = get_float(_P, "default_radius")
DIRECTION_STEP = get_float(_P, "direction_step")
GLEICHLAUF_MIN_COS = get_float(_P, "gleichlauf_min_cos")

# Abschnitte eines Umlaufs: die beiden Umlenkungen und die beiden Geraden.
END_PLUS = "+"        # Umlenkung in Achsrichtung
END_MINUS = "-"       # Umlenkung gegen die Achsrichtung
STRAIGHT = "gerade"


@dataclass
class Loop:
    """Umlauf eines Kreisels als Stadion-Form mit Laufrichtung.

    Lokale Koordinaten: a entlang der Achse ("Drehung"), b quer dazu (links
    positiv). Die Umlenkungen liegen bei a = +half und a = -half.
    """

    kreisel: Element
    cx: float
    cy: float
    ux: float              # Achsrichtung (Einheitsvektor)
    uy: float
    half: float            # halber Umlenkungsabstand L/2 (mm)
    radius: float          # Bahnabstand / 2 (mm)
    clockwise: bool        # True = UZS

    @property
    def length(self) -> float:
        """Umfang des Umlaufs (mm)."""
        return 4 * self.half + 2 * math.pi * self.radius

    def _local(self, x: float, y: float) -> tuple[float, float]:
        """Weltpunkt -> lokale Koordinaten (a, b)."""
        dx, dy = x - self.cx, y - self.cy
        return dx * self.ux + dy * self.uy, -dx * self.uy + dy * self.ux

    def lane(self, x: float, y: float) -> str:
        """Bahn eines Punkts: LANE_LEFT links der Achse, sonst LANE_RIGHT
        (dieselbe Konvention wie tro_flow._kreisel_bahn)."""
        _a, b = self._local(x, y)
        return LANE_LEFT if b > 0 else LANE_RIGHT

    def project(self, x: float, y: float) -> tuple[float, float, str]:
        """Naechster Punkt auf dem Umlauf.

        Parameter:
            x, y: Weltpunkt (mm).

        Rueckgabe:
            (s, Abstand, Abschnitt): s = Laufposition in Drehrichtung
            (0 <= s < length), Abstand des Punkts vom Umlauf (mm), Abschnitt
            END_PLUS, END_MINUS oder STRAIGHT.
        """
        a, b = self._local(x, y)
        h, r = self.half, self.radius
        cands: list[tuple[float, float, str]] = []
        # Gegen den Uhrzeigersinn (lokal): untere Gerade (b=-r) in +a, rechte
        # Umlenkung, obere Gerade (b=+r) in -a, linke Umlenkung.
        if -h <= a <= h:
            cands.append((abs(b + r), a + h, STRAIGHT))
            cands.append((abs(b - r), 2 * h + math.pi * r + (h - a), STRAIGHT))
        if a >= h:
            theta = math.atan2(b, a - h)                       # -pi/2 .. pi/2
            cands.append((abs(math.hypot(a - h, b) - r),
                          2 * h + r * (theta + math.pi / 2), END_PLUS))
        if a <= -h:
            phi = math.atan2(b, a + h) % (2 * math.pi)         # pi/2 .. 3pi/2
            cands.append((abs(math.hypot(a + h, b) - r),
                          4 * h + math.pi * r + r * (phi - math.pi / 2), END_MINUS))
        if not cands:                                          # Innenpunkt nahe Achse
            cands.append((abs(b + r), a + h, STRAIGHT))
        dist, s_ccw, section = min(cands)
        if section == STRAIGHT and a >= h - END_TOLERANCE:
            section = END_PLUS
        elif section == STRAIGHT and a <= -h + END_TOLERANCE:
            section = END_MINUS
        s = (self.length - s_ccw) % self.length if self.clockwise else s_ccw % self.length
        return s, dist, section

    def point(self, s: float) -> tuple[float, float]:
        """Weltpunkt zur Laufposition s (Umkehrung von project)."""
        s_ccw = (self.length - s) % self.length if self.clockwise else s % self.length
        h, r = self.half, self.radius
        if s_ccw < 2 * h:
            a, b = s_ccw - h, -r
        elif s_ccw < 2 * h + math.pi * r:
            theta = (s_ccw - 2 * h) / r - math.pi / 2
            a, b = h + r * math.cos(theta), r * math.sin(theta)
        elif s_ccw < 4 * h + math.pi * r:
            a, b = h - (s_ccw - 2 * h - math.pi * r), r
        else:
            phi = (s_ccw - 4 * h - math.pi * r) / r + math.pi / 2
            a, b = -h + r * math.cos(phi), r * math.sin(phi)
        return (self.cx + a * self.ux - b * self.uy, self.cy + a * self.uy + b * self.ux)

    def direction(self, s: float) -> tuple[float, float]:
        """Laufrichtung (Einheitsvektor) an der Laufposition s."""
        x0, y0 = self.point(s - DIRECTION_STEP)
        x1, y1 = self.point(s + DIRECTION_STEP)
        n = math.hypot(x1 - x0, y1 - y0) or 1.0
        return (x1 - x0) / n, (y1 - y0) / n


def kreisel_loop(kreisel: Element) -> Loop | None:
    """Umlauf eines Kreisels aus Position, Drehung, Boundingbox, Drehrichtung.

    Parameter:
        kreisel: Element mit kind "Kreisel".

    Rueckgabe:
        Loop oder None, wenn Position, Drehrichtung oder Abmessungen fehlen.
    """
    if kreisel.x is None or kreisel.y is None:
        return None
    if kreisel.rotation not in (ROTATION_CW, ROTATION_CCW):
        return None
    drehung = _as_float(_merkmal(kreisel.merkmale, "Drehung")) or 0.0
    rad = math.radians(drehung)
    bw, bh = kreisel.bbox_x, kreisel.bbox_y
    if bw and bh and abs(drehung) % 90 < 1e-6:
        short, long_ = min(bw, bh), max(bw, bh)
        radius, half = short / 2, (long_ - short) / 2
    else:
        abstand = _as_float(_merkmal(
            kreisel.merkmale, "Abstand (Kreiselachse A - Kreiselachse) in Meter"))
        if abstand is None:
            return None
        radius, half = DEFAULT_RADIUS, abstand * 1000 / 2
    return Loop(kreisel, kreisel.x, kreisel.y, math.cos(rad), math.sin(rad),
                half, radius, kreisel.rotation == ROTATION_CW)


@dataclass
class Contact:
    """Beruehrpunkt zweier Kreisel = eine Weiche."""

    weiche_id: str                       # TeileId der ILS Weiche oder "W<a>_<b>"
    kreisel: tuple[str, str]             # beide Kreisel-Ids, sortiert
    x: float
    y: float
    exported: bool                       # True = als ILS Weiche im CSV
    s: dict[str, float] = field(default_factory=dict)        # Kreisel -> s
    section: dict[str, str] = field(default_factory=dict)    # Kreisel -> Abschnitt


@dataclass
class Junction:
    """Weichenknoten: Beruehrpunkte auf derselben Umlenkung, plus Steuerung."""

    contacts: list[Contact]
    kreisel: list[str]                                       # beteiligte Kreisel
    separators: list[str] = field(default_factory=list)      # steuernde Separatoren
    sep_kreisel: dict[str, str] = field(default_factory=dict)  # Separator -> Kreisel
    sep_lane: dict[str, str] = field(default_factory=dict)     # Separator -> "<Id>-L/R"

    @property
    def type_name(self) -> str:
        """TRO-Typ aus Separator- und Weichenzahl, z. B. "2Sep1Swi"."""
        return f"{len(self.separators)}Sep{len(self.contacts)}Swi"


def _find_contact(a: Loop, b: Loop) -> tuple[float, float, float] | None:
    """Beruehrpunkt (x, y, Laenge des Beruehrbereichs) oder None."""
    steps = max(int(a.length / SAMPLE_STEP), MIN_SAMPLES)
    near: list[tuple[float, float, float]] = []
    for i in range(steps):
        x, y = a.point(i * a.length / steps)
        _s, dist, _sec = b.project(x, y)
        if dist <= CONTACT_TOLERANCE:
            near.append((dist, x, y))
    if not near:
        return None
    # Bei tangentialer Beruehrung sind viele Abtastpunkte fast gleich nah -
    # der Mittelpunkt der naechsten liegt am eigentlichen Beruehrpunkt.
    best = min(d for d, _x, _y in near)
    mitte = [(x, y) for d, x, y in near if d <= best + CONTACT_TIE]
    x = sum(p[0] for p in mitte) / len(mitte)
    y = sum(p[1] for p in mitte) / len(mitte)
    return x, y, len(near) * a.length / steps


def find_contacts(elements: list[Element], findings: list[str]) -> tuple[dict[str, Loop], list[Contact]]:
    """Alle Beruehrpunkte zwischen Kreiseln, mit Zuordnung der ILS Weichen.

    Parameter:
        elements: alle Elemente des CSV-Exports (Kreisel, ILS Weichen).
        findings: Fundliste; Hinweise zu fehlenden/gegenlaeufigen Weichen.

    Rueckgabe:
        (Kreisel-Id -> Loop, Beruehrpunkte). Ein Beruehrpunkt ohne
        exportierte ILS Weiche bekommt die Id "W<KreiselA>_<KreiselB>".
    """
    loops = {e.teile_id: lp for e in elements if e.kind == "Kreisel"
             and (lp := kreisel_loop(e)) is not None}
    weichen = [e for e in elements if e.kind == "Weiche"]
    ids = sorted(loops)
    contacts: list[Contact] = []
    for i, ka in enumerate(ids):
        for kb in ids[i + 1:]:
            hit = _find_contact(loops[ka], loops[kb])
            if hit is None:
                continue
            x, y, laenge = hit
            if laenge > MAX_CONTACT_LENGTH:
                findings.append(
                    f"Kreisel {ka}/{kb}: beruehren sich auf {laenge:.0f}mm Laenge "
                    f"(parallele Bahnen?) - keine Weiche angenommen")
                continue
            c = Contact("", (ka, kb), x, y, False)
            for k in (ka, kb):
                c.s[k], _d, c.section[k] = loops[k].project(x, y)
            da = loops[ka].direction(c.s[ka])
            db = loops[kb].direction(c.s[kb])
            if da[0] * db[0] + da[1] * db[1] < GLEICHLAUF_MIN_COS:
                findings.append(
                    f"Kreisel {ka}/{kb}: Beruehrpunkt bei {x:.0f}, {y:.0f} laeuft "
                    f"gegenlaeufig (Drehrichtung pruefen) - kein Weichenknoten")
                continue
            contacts.append(c)

    for c in contacts:
        passend = [w for w in weichen if w.x is not None and w.y is not None
                   and math.hypot(w.x - c.x, w.y - c.y) <= WEICHE_TOLERANCE]
        if passend:
            w = min(passend, key=lambda w: math.hypot(w.x - c.x, w.y - c.y))
            c.weiche_id, c.exported = w.teile_id, True
        else:
            c.weiche_id = f"W{c.kreisel[0]}_{c.kreisel[1]}"
            findings.append(
                f"Kreisel {c.kreisel[0]}/{c.kreisel[1]}: Beruehrpunkt bei "
                f"{c.x:.0f}, {c.y:.0f} ohne ILS Weiche im Export - als Weiche "
                f"{c.weiche_id} angenommen")
    # ILS Weichen ohne passenden Beruehrpunkt (z. B. an gegenlaeufigen
    # Kreiseln) bleiben unberuehrt - sie wertet tro_flow wie bisher ueber ihre
    # CSV-Nachbarn aus (_find_weiche_separators).
    return loops, contacts


def _group(contacts: list[Contact]) -> list[list[Contact]]:
    """Beruehrpunkte auf derselben Umlenkung eines Kreisels zusammenfassen."""
    parent = list(range(len(contacts)))

    def root(i: int) -> int:
        while parent[i] != i:
            parent[i] = parent[parent[i]]
            i = parent[i]
        return i

    seen: dict[tuple[str, str], int] = {}
    for i, c in enumerate(contacts):
        for k, section in c.section.items():
            if section == STRAIGHT:
                continue
            j = seen.setdefault((k, section), i)
            parent[root(i)] = root(j)
    groups: dict[int, list[Contact]] = {}
    for i, c in enumerate(contacts):
        groups.setdefault(root(i), []).append(c)
    return list(groups.values())


def find_junctions(
    elements: list[Element],
    findings: list[str],
    separators: list[Element] | None = None,
) -> list[Junction]:
    """Weichenknoten zwischen Kreiseln mit ihren steuernden Separatoren.

    Parameter:
        elements: alle Elemente des CSV-Exports.
        findings: Fundliste (siehe find_contacts).
        separators: Separatoren, die steuern duerfen; None = alle.

    Rueckgabe:
        Weichenknoten, sortiert nach erster Weichen-Id. Knoten ohne
        steuernden Separator sind enthalten (separators leer) - der Aufrufer
        entscheidet, was damit geschieht.
    """
    loops, contacts = find_contacts(elements, findings)
    if separators is None:
        separators = [e for e in elements if e.kind == "Separator"]

    # Separator -> (Kreisel, s): auf dem naechstgelegenen Umlauf
    sep_pos: dict[str, tuple[str, float]] = {}
    sep_lane: dict[str, str] = {}
    for sep in separators:
        if sep.x is None or sep.y is None:
            continue
        best: tuple[float, str, float] | None = None
        for k, lp in loops.items():
            s, dist, _sec = lp.project(sep.x, sep.y)
            if dist <= SEPARATOR_TOLERANCE and (best is None or dist < best[0]):
                best = (dist, k, s)
        if best is not None:
            sep_pos[sep.teile_id] = (best[1], best[2])
            sep_lane[sep.teile_id] = f"{best[1]}-{loops[best[1]].lane(sep.x, sep.y)}"

    junctions: list[Junction] = []
    for group in _group(contacts):
        kreisel = sorted({k for c in group for k in c.kreisel})
        j = Junction(sorted(group, key=lambda c: c.weiche_id), kreisel)
        in_group = {id(c) for c in group}
        for k in kreisel:
            # Ereignisse auf Kreisel k: (s, Art, Id); Art "sep" oder "swi"
            events = [(s, "sep", sid) for sid, (kk, s) in sep_pos.items() if kk == k]
            events += [(c.s[k], "own" if id(c) in in_group else "swi", c.weiche_id)
                       for c in contacts if k in c.s]
            events.sort()
            n = len(events)
            for idx, (_s, art, _id) in enumerate(events):
                if art != "own":
                    continue
                # gegen die Laufrichtung zurueck bis zum ersten fremden Ereignis
                for step in range(1, n):
                    _ps, part, pid = events[(idx - step) % n]
                    if part == "own":
                        continue
                    if part == "sep" and pid not in j.separators:
                        j.separators.append(pid)
                        j.sep_kreisel[pid] = k
                        j.sep_lane[pid] = sep_lane[pid]
                    break
        j.separators.sort()
        junctions.append(j)
    junctions.sort(key=lambda j: j.contacts[0].weiche_id)
    return junctions