Aislo/B05_Profile/B05_Profile_Engine_RouteInitial_Station.py

"""추천 노선 곡선 MC → 일반측점(PLAN 351-2 · 요구 235-3).

`B05_Profile_Engine_RouteInitial.generate_initial_route` 끝이 고른 선(다섯 갈래 · 큰 R 뒤)에
부름 — 맞춘 선 · 맞추기 전 선 두 벌을 냄(모달 「측점 맞춤」 켬끔 · 브레인 확정). 전처리 초기
노선 · 손 편집 · [측점 스냅]은 안 거침.

일반측점 = 누가거리 20 m 격자(No.N+0). 곡선 가운데 MC(BC · EC 누가거리 가운데 = 곡선 중심과
마주치는 법선 자리)를 일반측점 위에 둔다 → 곡선 수 ≤ 내부 일반측점 수. 앞 곡선이 뒤 누가거리를
바꾸므로 시점부터 차례로:

  · 곡선마다 교각점(IP)을 들어오는 접선 따라 밀고(앞 곡선 · 앞 직선은 그대로) R 을 맞춰 MC 를
    가까운 일반측점에 놓음 — 앞 MC 보다 20 m 이상 뒤 · 원래 선에서 `ROUTE_STATION_SNAP_MAX_GAP_M`
    안 · 최소 R 미달을 새로 만들지 않음.
  · 못 놓으면 다음 꺾임과 합침(같은 쪽으로 돌면 두 접선 교점 · 아니면 다음 IP 를 뺌) → 그래도
    안 되면 그 곡선을 끔(꺾임만) → 그래도 이탈 한도를 넘으면 실패.
  · 모두 놓는 길이 없으면 못 놓는 곡선만 그 자리에 두고 나머지는 맞춤(부분 · 「측점 밖 곡선 N개」).
  · 그것도 못 하면 None — 부르는 쪽이 원래 선을 씀.

고른 값은 편집 갈래(`simplify=False` · 곡선 켬 · `radii`)로 다시 짠다 — 화면이 곡선을 교각점
하나로 펴서(`flattenServerPlan`) [확인] 때 다시 짜는 입력과 같아 저장 뒤에도 MC 가 측점 위(TS 짝
불필요). 누가거리는 짜인 선의 정점(원호 5° 현)을 따라 잼 — 측점 생성과 같은 잣대.
"""

from __future__ import annotations

import math
from collections.abc import Callable, Iterator
from dataclasses import dataclass

import numpy as np
import shapely
from shapely.geometry import LineString, Point

from B05_Profile.B05_Profile_Engine_RouteInitial_Radius import _flat
from common_util.common_util_route_polyline import (
    STRAIGHT_INNER_ANGLE_DEG,
    PlannedPolyline,
    _arc_geometry,
    _arc_points,
    _inner_angle_deg,
    _line_intersection,
    _turn_sign,
    build_planned_polyline,
)
from config.config_system_design import ROUTE_STATION_SNAP_MAX_GAP_M

Xy = tuple[float, float]
# 점 묶음 → 지반 기울기가 상한을 넘은 만큼(0 = 상한 안)
Excess = Callable[[list[Xy]], float]

STATION_M = 20.0
# MC 가 측점에서 이만큼 안이면 「위」(m) — 저장 반올림(교각점 0.1 mm · R 1 mm)을 넉넉히 덮음
MC_TOL_M = 0.05
# IP 를 밀 수 있는 거리(m) — 다음 측점까지 한 칸(실제 한도는 원래 선에서 벗어남)
SHIFT_MAX_M = STATION_M
# 곡선 접선이 뒤 직선 절반에서 남길 여유(m) — 다음 IP 를 뒤로 이만큼 두 배 밀 수 있음
SLACK_M = 5.0
# 한 곡선이 뒤 꺾임을 몇 번까지 합쳐 볼지
MERGE_TRIES = 3
# R 맞춤 후보 수(최소 R ~ 자리 R 사이 · 지금 R 에 가까운 것부터)
RADIUS_STEPS = 8
# 막혀 앞 곡선의 다음 갈래로 되돌아가는 횟수 한도 — 넘으면 부분 맞춤으로
BACKTRACK_LIMIT = 30
# 한 꺾임 꼴(그대로 · 합친 꼴마다)에서 볼 놓기 수 — 막히면 R · 측점만 바꾸기보다 합치기로
PER_VARIANT = 3


@dataclass
class _Corner:
    arc: list[Xy]  # BC · 원호 점 · EC — 짜인 선 정점 그대로
    radius: float
    tangent: float
    length: float  # 정점을 따라 잰 BC → EC


@dataclass
class _Plan:
    points: list[Xy]
    flags: list[bool]
    radii: list[float | None]

    def copy(self) -> _Plan:
        return _Plan(list(self.points), list(self.flags), list(self.radii))


def _corner(before: Xy, at: Xy, after: Xy, radius: float) -> _Corner | None:
    """`build_planned_polyline` 편집 갈래 곡선 하나와 같은 셈(접선 자리 모자라면 R 줄임)."""
    inner = _inner_angle_deg(before, at, after)
    half_tan = math.tan(math.radians(180.0 - inner) / 2)
    available = min(math.dist(before, at), math.dist(at, after)) / 2
    if half_tan <= 1e-9 or available <= 0 or radius <= 0:
        return None
    tangent = radius * half_tan
    if tangent > available:
        radius, tangent = available / half_tan, available
    geometry = _arc_geometry(before, at, after, inner, radius, half_tan)
    if geometry is None:
        return None
    start, end, center = geometry
    clockwise = _turn_sign(before, at, after) < 0
    arc = [start, *_arc_points(center, start, end, clockwise=clockwise), end]
    length = sum(math.dist(a, b) for a, b in zip(arc, arc[1:]))
    return _Corner(arc, radius, tangent, length)


def curve_mc_chainages(outline: PlannedPolyline) -> list[float]:
    """곡선마다 MC 누가거리 — 짜인 선 정점을 따라 잰 (BC + EC) / 2(측점 스냅과 같은 정의)."""
    vertices = outline.vertices
    coords = np.asarray(vertices, dtype=float)
    along = np.concatenate([[0.0], np.cumsum(np.hypot(*np.diff(coords, axis=0).T))])
    found: list[float] = []
    cursor = 0
    for curve in outline.curves:
        first = next(i for i in range(cursor, len(vertices)) if vertices[i] == curve.start)
        last = next(i for i in range(first, len(vertices)) if vertices[i] == curve.end)
        found.append(float(along[first] + along[last]) / 2)
        cursor = last
    return found


def station_hits(outline: PlannedPolyline, tol: float = MC_TOL_M) -> int:
    """MC 가 일반측점(No.1 이상) 위인 곡선 수 — 같은 측점 둘은 하나만 셈."""
    stations = {
        n
        for mc in curve_mc_chainages(outline)
        if (n := round(mc / STATION_M)) >= 1 and abs(mc - n * STATION_M) <= tol
    }
    return len(stations)


def off_station_count(outline: PlannedPolyline) -> int:
    """측점 밖 곡선 수 — 모달 상태줄 「측점 밖 곡선 N개」."""
    return len(outline.curves) - station_hits(outline)


def _unit(a: Xy, b: Xy) -> Xy:
    length = math.dist(a, b)
    return ((b[0] - a[0]) / length, (b[1] - a[1]) / length)


class _Reference:
    """원래 선 — 맞춘 조각과 같은 자리만 잘라 잼(정점 배열 · 누가거리)."""

    def __init__(self, vertices: list[Xy]) -> None:
        # 겹친 정점(길이 0 조각)은 뺌 — 내려 잴 때 NaN
        vertices = [xy for i, xy in enumerate(vertices) if not i or xy != vertices[i - 1]]
        self.line = LineString(vertices)
        self.coords = np.asarray(vertices, dtype=float)
        self.along = np.concatenate([[0.0], np.cumsum(np.hypot(*np.diff(self.coords, axis=0).T))])

    def window(self, local: list[Xy]) -> list[Xy]:
        """맞춘 조각과 같은 자리의 원래 선 — 조각 양 끝을 내린 사이."""
        s0, s1 = sorted(self.line.project(Point(xy)) for xy in (local[0], local[-1]))
        inside = self.coords[(self.along > s0) & (self.along < s1)]
        ends = [
            (
                np.interp(s, self.along, self.coords[:, 0]),
                np.interp(s, self.along, self.coords[:, 1]),
            )
            for s in (s0, s1)
        ]
        return [ends[0], *map(tuple, inside), ends[1]]

    def gap(self, local: list[Xy]) -> float:
        """맞춘 조각과 원래 선의 같은 자리 사이 가장 먼 거리(m)."""
        piece = LineString([xy for i, xy in enumerate(local) if not i or xy != local[i - 1]])
        window = LineString(self.window(local))
        gap = float(shapely.hausdorff_distance(piece, window, densify=0.1))
        return gap if math.isfinite(gap) else math.inf


@dataclass
class _State:
    """차례 셈의 한 걸음 — 곡선 k 앞까지 정해진 선."""

    plan: _Plan
    k: int  # 다음에 볼 꺾임점
    end_xy: Xy  # 앞 요소 끝(앞 곡선 EC 또는 앞 꺾임점)
    end_s: float
    prev_tangent: float  # 앞 곡선 접선 길이 — 그 곡선 R 이 줄지 않게
    last_station: float  # 앞 MC 측점(없으면 0 = 시점)
    placed: int = 0  # 측점에 놓은 곡선 수


def _try(state: _State, plan: _Plan, k: int, at: Xy, radius: float) -> tuple[_Corner, float] | None:
    """IP 를 `at` 에 두고 R 을 준 곡선과 그 MC — 앞 곡선이 줄거나 BC 가 앞 요소 앞이면 None."""
    before, after = plan.points[k - 1], plan.points[k + 1]
    corner = _corner(before, at, after, radius)
    if corner is None or state.prev_tangent > math.dist(before, at) / 2 + 1e-6:
        return None
    u = _unit(before, at)
    bc = corner.arc[0]
    if (bc[0] - state.end_xy[0]) * u[0] + (bc[1] - state.end_xy[1]) * u[1] < -1e-6:
        return None
    return corner, state.end_s + math.dist(state.end_xy, bc) + corner.length / 2


def _solve(
    state: _State, plan: _Plan, k: int, target: float, radius: float
) -> tuple[Xy, _Corner, float] | None:
    """IP 를 들어오는 접선 따라 d 만큼 밀어 MC = target — 할선법(기울기 ≈ 1)."""
    base = plan.points[k]
    u = _unit(plan.points[k - 1], base)

    def at_of(d: float) -> Xy:
        return (round(base[0] + u[0] * d, 4), round(base[1] + u[1] * d, 4))

    def miss(d: float) -> float | None:
        found = _try(state, plan, k, at_of(d), radius)
        return None if found is None else found[1] - target

    # 앞 곡선 접선이 앞 직선 절반을 다 쓰면 뒤로는 못 감 — 앞으로 조금 민 데서 시작
    d0 = next((d for d in (0.0, 1.0, 3.0, -1.0) if miss(d) is not None), None)
    if d0 is None:
        return None
    f0 = miss(d0)
    assert f0 is not None
    d1 = max(-SHIFT_MAX_M, min(SHIFT_MAX_M, d0 - f0))
    for _ in range(10):
        if abs(f0) <= MC_TOL_M / 5:
            done = _try(state, plan, k, at_of(d0), radius)
            return None if done is None else (at_of(d0), done[0], done[1])
        f1 = miss(d1)
        if f1 is None or d1 == d0:
            return None
        slope = (f1 - f0) / (d1 - d0)
        if abs(slope) < 0.2:
            return None
        d0, f0 = d1, f1
        d1 = max(-SHIFT_MAX_M, min(SHIFT_MAX_M, d1 - f1 / slope))
    return None


def _absorb_next(plan: _Plan, k: int) -> bool:
    """곡선 k 에 뒤 꺾임 하나를 합침 — 같은 쪽 = 두 접선 교점 · 아니면 다음 IP 뺌."""
    if k + 2 >= len(plan.points):
        return False
    before, at, nxt, beyond = (plan.points[i] for i in range(k - 1, k + 3))
    same = _turn_sign(before, at, nxt) == _turn_sign(at, nxt, beyond) != 0
    cross = _line_intersection(before, at, nxt, beyond) if same else None
    ahead = cross is not None and (
        (cross[0] - before[0]) * (at[0] - before[0]) + (cross[1] - before[1]) * (at[1] - before[1])
        > 0
        and (beyond[0] - cross[0]) * (beyond[0] - nxt[0])
        + (beyond[1] - cross[1]) * (beyond[1] - nxt[1])
        > 0
    )
    radius = max(r for r in (plan.radii[k], plan.radii[k + 1], 0.0) if r is not None)
    if ahead:
        assert cross is not None
        plan.points[k : k + 2] = [(round(cross[0], 4), round(cross[1], 4))]
    else:
        del plan.points[k + 1]
    plan.flags[k : k + 2] = [True]
    plan.radii[k : k + 2] = [radius or None]
    return True


class _Snapper:
    def __init__(
        self,
        outline: PlannedPolyline,
        min_radius_m: float,
        max_gap_m: float,
        excess: Excess | None,
    ) -> None:
        points, flags, radii = _flat(outline)
        points = [(round(x, 4), round(y, 4)) for x, y in points]  # 화면이 펴는 자릿수
        self.start = _State(_Plan(points, flags, radii), 1, points[0], 0.0, 0.0, 0.0)
        self.reference = _Reference(outline.vertices)
        self.min_radius = min_radius_m
        self.max_gap = max_gap_m
        self.excess = excess
        self.keep = False

    def _steeper(self, local: list[Xy]) -> bool:
        """맞춘 조각이 원래 선 같은 자리보다 상한을 더 넘나."""
        if self.excess is None:
            return False
        return self.excess(local) > self.excess(self.reference.window(local)) + 1e-6

    @staticmethod
    def _kink(state: _State, plan: _Plan) -> _State:
        at = plan.points[state.k]
        end_s = state.end_s + math.dist(state.end_xy, at)
        return _State(plan, state.k + 1, at, end_s, 0.0, state.last_station, state.placed)

    def _placements(self, state: _State, plan: _Plan) -> Iterator[_State]:
        """곡선 k 를 남은 측점에 놓은 걸음들 — 가까운 측점 · 지금 R 에 가까운 것부터."""
        k = state.k
        before, at, after = plan.points[k - 1], plan.points[k], plan.points[k + 1]
        start = _try(state, plan, k, at, plan.radii[k] or self.min_radius)
        if start is None:
            return
        corner, mc = start
        floor_r = min(self.min_radius, corner.radius)
        half_tan = math.tan(math.radians(180.0 - _inner_angle_deg(before, at, after)) / 2)
        room = min(math.dist(before, at), math.dist(at, after)) / 2 / half_tan
        steps = np.geomspace(max(floor_r, 1e-3), room, RADIUS_STEPS) if room > floor_r else []
        radii = [round(corner.radius, 3)] + sorted(
            {round(float(r), 2) for r in steps}, key=lambda r: abs(r - corner.radius)
        )
        lowest = state.last_station + STATION_M
        stations = {
            max(lowest, STATION_M * math.floor(mc / STATION_M)),
            max(lowest, STATION_M * math.ceil(mc / STATION_M)),
        }
        found: list[tuple[tuple[bool, float, float], Xy, _Corner, float, float]] = []
        for station in stations:
            for radius in radii:
                solved = _solve(state, plan, k, station, radius)
                if solved is None or solved[1].radius < floor_r - 1e-6:
                    continue
                new_at, new_corner, new_mc = solved
                # 뒤 직선 절반에 여유를 남긴 것 먼저 — 다음 IP 가 뒤로도 밀리게
                tight = math.dist(new_at, after) / 2 - new_corner.tangent < SLACK_M
                rank = (tight, abs(station - mc), abs(radius - corner.radius))
                found.append((rank, new_at, new_corner, new_mc, station))
        given = 0
        for _, new_at, new_corner, new_mc, station in sorted(found, key=lambda item: item[0]):
            middle = ((new_at[0] + after[0]) / 2, (new_at[1] + after[1]) / 2)
            local = [state.end_xy, *new_corner.arc, middle]
            if self.reference.gap(local) > self.max_gap or self._steeper(local):
                continue
            placed = plan.copy()
            placed.points[k], placed.radii[k] = new_at, round(new_corner.radius, 3)
            yield _State(
                placed,
                k + 1,
                new_corner.arc[-1],
                new_mc + new_corner.length / 2,
                new_corner.tangent,
                station,
                state.placed + 1,
            )
            given += 1
            if given >= PER_VARIANT:
                return

    def _options(self, state: _State) -> Iterator[_State]:
        """꺾임점 k 의 갈래 — 측점에 놓기 → 뒤 꺾임과 합쳐 놓기 → (완만하면) 곡선 끔."""
        plan, k = state.plan, state.k
        before, at, after = plan.points[k - 1 : k + 2]
        if not plan.flags[k] or _corner(before, at, after, plan.radii[k] or 1.0) is None:
            kink = plan.copy()
            kink.flags[k] = False  # 꺾임만(나란한 직선 등 곡선 못 끼움 포함)
            yield self._kink(state, kink)
            return
        trial = plan.copy()
        for _ in range(MERGE_TRIES + 1):
            yield from self._placements(state, trial)
            trial = trial.copy()
            if not _absorb_next(trial, k):
                break
        # 곡선 끔 — 법이 곡선을 안 둬도 되는 완만한 꺾임 · 원래 선에서 한도 안일 때만
        middle = ((at[0] + after[0]) / 2, (at[1] + after[1]) / 2)
        if (
            _inner_angle_deg(before, at, after) >= STRAIGHT_INNER_ANGLE_DEG
            and self.reference.gap([state.end_xy, at, middle]) <= self.max_gap
        ):
            kink = plan.copy()
            kink.flags[k], kink.radii[k] = False, None
            yield self._kink(state, kink)
        if self.keep:
            yield from self._kept(state)

    def _kept(self, state: _State) -> Iterator[_State]:
        """못 놓는 곡선은 그 자리 그대로(브레인 확정 · 측점 밖 곡선으로 셈)."""
        plan, k = state.plan, state.k
        found = _try(state, plan, k, plan.points[k], plan.radii[k] or self.min_radius)
        if found is None:
            return
        corner, mc = found
        at, after = plan.points[k], plan.points[k + 1]
        middle = ((at[0] + after[0]) / 2, (at[1] + after[1]) / 2)
        if self.reference.gap([state.end_xy, *corner.arc, middle]) > self.max_gap:
            return
        kept = plan.copy()
        kept.radii[k] = round(corner.radius, 3)
        yield _State(
            kept,
            k + 1,
            corner.arc[-1],
            mc + corner.length / 2,
            corner.tangent,
            max(state.last_station, STATION_M * math.floor(mc / STATION_M)),
            state.placed,
        )

    def run(self, keep: bool) -> _State | None:
        """시점부터 깊이 우선 — 막히면 앞 곡선의 다음 갈래로(되돌림 `BACKTRACK_LIMIT` 번까지).

        `keep` = 못 놓는 곡선을 그 자리에 두는 갈래를 끝에 더함(부분 맞춤).
        """
        self.keep = keep
        stack = [self._options(self.start)]
        backtracks = 0
        while stack:
            state = next(stack[-1], None)
            if state is None:
                stack.pop()
                backtracks += 1
                if backtracks > BACKTRACK_LIMIT:
                    return None
                continue
            if state.k >= len(state.plan.points) - 1:
                return state
            stack.append(self._options(state))
        return None


def snap_curves_to_stations(
    outline: PlannedPolyline,
    min_radius_m: float,
    max_gap_m: float = ROUTE_STATION_SNAP_MAX_GAP_M,
    *,
    excess: Excess | None = None,
) -> PlannedPolyline | None:
    """곡선 MC 를 일반측점에 놓아 다시 짠 선 · 못 하면 None(원래 선을 씀). 곡선 없으면 그대로.

    모두 놓는 길을 먼저 찾고, 없으면 못 놓는 곡선만 그 자리에 둔 부분 맞춤(`off_station_count`).
    `excess`(점 묶음 → 기울기가 상한을 넘은 만큼)를 주면 옮긴 조각이 원래 자리보다 더 넘을 때 그
    놓기를 버림 — 급한 산지에서 측점 맞춤이 기울기를 망치지 않게.
    """
    if not outline.curves:
        return outline
    snapper = _Snapper(outline, min_radius_m, max_gap_m, excess)
    state = snapper.run(keep=False) or snapper.run(keep=True)
    if state is None:
        return None
    snapped = build_planned_polyline(
        state.plan.points,
        min_radius_m=min_radius_m,
        simplify=False,
        curve_flags=state.plan.flags,
        radii=state.plan.radii,
    )
    return snapped if station_hits(snapped) >= state.placed else None