Aislo/B05_Profile/B05_Profile_Engine_RouteInitial_Radius.py

"""추천 노선 큰 R — 2단(PLAN 328-2 · 요구 224-4 「R 은 클수록 좋음」).

`B05_Profile_Engine_RouteInitial_Pool._trace` 가 부름. 예상노선 추적(전처리 초기값)은 안 거침.

1단(`_trace`)이 예정노선 규칙(단순화 · 같은 방향 묶음 · R 맞춤)으로 짠 선을 받아, 곡선마다 R 을
[맞춘 R, 접선 자리 ÷ tan(IA/2)] 안에서 탐색 경로 점과 가장 먼 거리 ≤ `ROUTE_RECOMMEND_MAX_GAP_M`
인 가장 큰 값(1 m 내림)으로 고른다. 맞는 값이 없으면 맞춘 R 그대로.

추천 후보(`guard` 줌 · PLAN 406)는 거리 한도 대신 직접 잼 — 곡선 조각이 같은 자리 탐색 경로보다
복도 밖 · 기울기 · 계류 · 토공이 나빠지지 않는 가장 큰 R(`..._Merge.Guard` · 자리 R ~ 맞춘 R 사이
`GUARD_STEPS` 벌 · 1 m 내림).

꺾임만(`curve_every_kink` · 419) — 추천 후보에 R 없는 교점을 안 남김(곡선 또는 교점 지움).

고른 R 은 편집 갈래(`simplify=False` · `radii`)로 못박아 다시 짠다 — 화면이 곡선을 교각점 하나로
펴서(`flattenServerPlan`) [확인]이 다시 짜는 것과 같은 입력이라 저장 뒤 R 이 응답 R 과 같다.
"""

from __future__ import annotations

import math
from typing import Any

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

from common_util.common_util_route_polyline import (
    PlannedPolyline,
    _inner_angle_deg,
    build_planned_polyline,
)
from config.config_system_design import ROUTE_RECOMMEND_MAX_GAP_M

Xy = tuple[float, float]
# 직접 잼 R 후보 수(자리 R ~ 맞춘 R · 큰 것부터)
GUARD_STEPS = 12


def _flat(outline: PlannedPolyline) -> tuple[list[Xy], list[bool], list[float | None]]:
    """곡선 묶음을 교각점 하나로 편 노드 · 곡선 켬 · R — 화면 `flattenServerPlan` 과 같은 꼴."""
    starts = {curve.node_first: curve for curve in outline.curves}
    inside = {i for c in outline.curves for i in range(c.node_first + 1, c.node_last + 1)}
    points: list[Xy] = []
    flags: list[bool] = []
    radii: list[float | None] = []
    for index, node in enumerate(outline.nodes):
        if index in inside:
            continue
        curve = starts.get(index)
        points.append(curve.apex if curve else (node.x, node.y))
        flags.append(curve is not None)
        radii.append(round(curve.radius_m, 2) if curve else None)
    return points, flags, radii


def curve_every_kink(outline: PlannedPolyline, min_radius_m: float) -> PlannedPolyline:
    """꺾임만(R 없는 교점 · 삼각) 없앤 선(419) — 최소 R 원호가 들어가면 곡선(내각 155° 이상도) ·
    아니면 교점 지움(곧게). 꺾임만이 없으면 받은 그대로."""
    points, flags, radii = _flat(outline)
    if all(flags[1:-1]):
        return outline
    index = 1
    while index < len(points) - 1:
        if flags[index]:
            index += 1
            continue
        before, apex, after = points[index - 1 : index + 2]
        half_tan = math.tan(math.radians(180.0 - _inner_angle_deg(before, apex, after)) / 2)
        room = min(math.dist(before, apex), math.dist(apex, after)) / 2
        if half_tan > 1e-9 and room / half_tan >= min_radius_m:
            flags[index] = True
            index += 1
        else:
            del points[index], flags[index], radii[index]
    return build_planned_polyline(
        points, min_radius_m=min_radius_m, simplify=False, curve_flags=flags, radii=radii
    )


def _cross(u: np.ndarray, v: np.ndarray) -> np.ndarray:
    return u[..., 0] * v[..., 1] - u[..., 1] * v[..., 0]


def _gap(samples: np.ndarray, before: Xy, apex: Xy, after: Xy, radii: np.ndarray) -> np.ndarray:
    """R 마다 점들이 「직선 · 원호 · 직선」(앞뒤 직선은 절반까지)에서 가장 먼 거리(m)."""
    a = np.asarray(apex, dtype=float)
    p = samples - a  # (S, 2) 교각점 기준
    units = [(np.asarray(end, dtype=float) - a) for end in (before, after)]
    halves = [float(np.hypot(*u)) / 2 for u in units]
    units = [u / (2 * h) for u, h in zip(units, halves)]
    inner = math.radians(_inner_angle_deg(before, apex, after))
    tangent = radii[:, None] * math.tan((math.pi - inner) / 2)  # (M, 1)
    legs = []
    for unit, half in zip(units, halves):
        along = p @ unit
        side = np.abs(_cross(unit, p))
        legs.append(np.hypot(side, np.maximum(0.0, np.maximum(tangent - along, along - half))))
    bisector = (units[0] + units[1]) / np.hypot(*(units[0] + units[1]))
    center = (radii / math.sin(inner / 2))[:, None, None] * bisector  # (M, 1, 2)
    starts = [tangent[..., None] * unit - center for unit in units]  # 중심 → 접선점
    q = p[None] - center  # (M, S, 2) 중심 → 점
    turn = _cross(starts[0], starts[1])
    within = (_cross(starts[0], q) * turn >= 0) & (_cross(q, starts[1]) * turn >= 0)
    arc = np.where(within, np.abs(np.hypot(q[..., 0], q[..., 1]) - radii[:, None]), np.inf)
    return np.minimum(np.minimum(legs[0], legs[1]), arc).max(axis=1)


def widen(
    outline: PlannedPolyline,
    path: list[Xy],
    min_radius_m: float,
    max_gap_m: float = ROUTE_RECOMMEND_MAX_GAP_M,
    *,
    guard: Any = None,
) -> PlannedPolyline:
    """곡선마다 허용 이탈 안 가장 큰 R(1 m 내림)로 못박아 다시 짠 선. 곡선이 없으면 그대로.

    `guard` 를 주면 이탈 한도 대신 직접 잼(`..._Merge.Guard.curve_ok`)을 지나는 가장 큰 R."""
    if not outline.curves:
        return outline
    points, flags, radii = _flat(outline)
    line = LineString(path)
    coords = np.asarray(path, dtype=float)
    along = np.concatenate([[0.0], np.cumsum(np.hypot(*np.diff(coords, axis=0).T))])
    for index in range(1, len(points) - 1):
        if not flags[index]:
            continue
        before, apex, after = points[index - 1], points[index], points[index + 1]
        half_tan = math.tan(math.radians(180.0 - _inner_angle_deg(before, apex, after)) / 2)
        if half_tan <= 1e-9:
            continue
        room = min(math.dist(before, apex), math.dist(apex, after)) / 2 / half_tan
        candidates = np.arange(math.floor(room), float(radii[index]), -1.0)  # 큰 것부터
        # 이 곡선이 쓰는 자리 = 앞뒤 직선 가운데점 사이 — 그 사이 탐색 경로 점만 잼
        ends = [
            line.project(Point((apex[0] + end[0]) / 2, (apex[1] + end[1]) / 2))
            for end in (before, after)
        ]
        samples = coords[(along >= min(ends)) & (along <= max(ends))]
        if not candidates.size or not len(samples):
            continue
        if guard is not None:
            if len(samples) < 2:
                continue
            steps = np.geomspace(room, float(radii[index]), GUARD_STEPS)
            reference = [tuple(p) for p in samples]
            radii[index] = next(
                (
                    r
                    for r in sorted({float(math.floor(r)) for r in steps}, reverse=True)
                    if r > radii[index] and guard.curve_ok(before, apex, after, r, reference)
                ),
                radii[index],
            )
            continue
        ok = candidates[_gap(samples, before, apex, after, candidates) <= max_gap_m]
        if ok.size:
            radii[index] = float(ok[0])
    return build_planned_polyline(
        points, min_radius_m=min_radius_m, simplify=False, curve_flags=flags, radii=radii
    )