Aislo/B05_Profile/B05_Profile_Engine_RouteInitial_Search.py

"""초기 계획노선 갈래의 격자 탐색(PLAN 20 · 23장) — `B05_Profile_Engine_RouteInitial` 이 부름.

탐색 = 예상노선 둘레 복도(`CORRIDOR_M`) 안 `SEARCH_CELL_M` 격자 · 16 방향 Dijkstra(scipy csgraph).
상태 = (칸, 진행 방향, 꺾은 뒤 곧게 온 걸음 수). 탐색 경로의 꺾임점이 곧 노드다.

그래프(`RouteGraph`)는 요청마다 **한 번만** 짠다 — 복도 안 모든 링크를 상태 간선으로 펼쳐 두고,
탐색 상한 · 목표 비중 · 벌점 크기가 바뀌는 탐색은 간선 비용만 다시 계산한다(지형상 불가능
상한 이분 탐색 · 후보 여러 벌을 한 요청 안에서).
"""

from __future__ import annotations

import math
from dataclasses import dataclass

import numpy as np
from scipy import ndimage
from scipy.sparse import csr_matrix
from scipy.sparse.csgraph import breadth_first_order, connected_components, dijkstra

from B05_Profile.B05_Profile_Engine_RouteInitial_Metrics import GRADE_SMOOTH_M, smoothed_ground

# 토공 가늠 폭(m) = 유효너비 3 + 길어깨 0.5 × 2(별표2 간선).
ROAD_WIDTH_M = 4.0
# 탐색 복도 반폭(m) — 예상노선에서 이보다 멀리는 가지 않음(법령 값 아님 · 알고리즘 값).
CORRIDOR_M = 100.0
SEARCH_CELL_M = 4.0  # 탐색 격자 칸(m) — 비용면(2 m)을 솎아 씀
LENGTH_COST = 0.05  # 1 m 당 거리 비용 — 목표가 평평한 곳에서 헤매지 않게
TURN_COST = 1.0  # 꺾을 때 벌점 = 최소 곡선반지름 원호 길이(R·θ) × 이 값 — 곧게 달리기 선호
SOFT_STREAM_COST = 20.0  # 계류 버퍼 안 나란히 달리기(지형상 불가능할 때만 · 1 m 당)
BOUND_BISECT = 7  # 지형상 불가능할 때 막는 기울기를 찾는 이분 탐색 횟수(구간 1/128 까지)
STREAM_CROSS_COS = math.cos(math.radians(45.0))  # 버퍼 안 이동 방향 한도(계류 쪽 · 반대쪽 ±45°)
OBJECTIVES = ("expected", "earthwork", "grade")
# 16 방향(행, 열) — +x 에서 반시계. 나이트 걸음(1, 2) 등을 넣어 22.5° 안팎 방향을 곧게 달림.
DIRECTIONS = (
    (0, 1),
    (1, 2),
    (1, 1),
    (2, 1),
    (1, 0),
    (2, -1),
    (1, -1),
    (1, -2),
    (0, -1),
    (-1, -2),
    (-1, -1),
    (-2, -1),
    (-1, 0),
    (-2, 1),
    (-1, 1),
    (-1, 2),
)


@dataclass
class Terrain:
    """지반 격자 — 탐색 비용면(`B05_Profile_Engine_Solver._load_or_build_cost_surface`) 그대로."""

    x: np.ndarray  # 열 좌표(오름차순)
    y: np.ndarray  # 행 좌표(오름차순)
    z: np.ndarray  # [행, 열] 표고
    valid: np.ndarray  # [행, 열] 지형 있음


class NoRoute(ValueError):
    """제약을 지키며 시점과 종점을 잇는 길이 없음."""


def _cell_size(coords: np.ndarray) -> float:
    return float((coords[-1] - coords[0]) / (len(coords) - 1)) if len(coords) > 1 else 1.0


def _distance_grid(
    lines: list[list[tuple[float, float]]], xs: np.ndarray, ys: np.ndarray, cell: float
) -> np.ndarray:
    """격자 칸마다 선까지 거리(m). 선이 격자에 안 닿으면 모두 무한."""
    marked = np.zeros((len(ys), len(xs)), dtype=bool)
    for line in lines:
        for (x0, y0), (x1, y1) in zip(line, line[1:]):
            count = max(2, int(math.hypot(x1 - x0, y1 - y0) / (cell * 0.5)) + 1)
            px = np.linspace(x0, x1, count)
            py = np.linspace(y0, y1, count)
            cols = np.rint((px - xs[0]) / cell).astype(int)
            rows = np.rint((py - ys[0]) / cell).astype(int)
            inside = (cols >= 0) & (cols < len(xs)) & (rows >= 0) & (rows < len(ys))
            marked[rows[inside], cols[inside]] = True
    if not marked.any():
        return np.full(marked.shape, np.inf)
    return ndimage.distance_transform_edt(~marked) * cell


class SearchGrid:
    """탐색 격자 — 복도 · 거리 · 기울기를 한 번만 만든다."""

    def __init__(
        self,
        expected: list[tuple[float, float]],
        terrain: Terrain,
        streams: list[list[tuple[float, float]]],
        *,
        stream_offset_m: float,
        grade_smooth_m: float = GRADE_SMOOTH_M,
    ) -> None:
        step = max(1, int(round(SEARCH_CELL_M / _cell_size(terrain.x))))
        self.xs, self.ys = terrain.x[::step], terrain.y[::step]
        # 기울기 · 옆경사는 비교값과 같은 고른 지반에서 잰다(`smoothed_ground`)
        self.z = smoothed_ground(terrain, grade_smooth_m)[::step, ::step]
        self.stream_offset_m = stream_offset_m
        self.cell = _cell_size(self.xs)
        self.d_expected = _distance_grid([expected], self.xs, self.ys, self.cell)
        self.d_stream = _distance_grid(streams, self.xs, self.ys, self.cell)
        valid = np.asarray(terrain.valid[::step, ::step], dtype=bool)
        self.corridor = (self.d_expected <= CORRIDOR_M) & valid
        self.grad_y, self.grad_x = np.gradient(self.z, self.cell)
        finite = np.where(np.isfinite(self.d_stream), self.d_stream, 0.0)
        self.away_y, self.away_x = np.gradient(finite, self.cell)  # 계류에서 멀어지는 쪽
        self.index = np.full(self.z.shape, -1, dtype=np.int64)
        self.cells = int(self.corridor.sum())
        self.index[self.corridor] = np.arange(self.cells)
        rows, cols = np.nonzero(self.corridor)
        self.centres = np.column_stack([self.xs[cols], self.ys[rows]])

    def nearest(self, point: tuple[float, float]) -> int:
        dx = self.centres[:, 0] - point[0]
        return int(np.argmin(np.hypot(dx, self.centres[:, 1] - point[1])))

    def links(self, dr: int, dc: int, *, limit: float) -> tuple:
        """복도 안 한 방향 링크 모두 — (앞 번호, 뒤 번호, 길이, 세 목표[3, n] · 1 m 당,
        넘은 기울기[n], 계류 버퍼 안 나란히[n], 지반 기울기[n])."""
        rows, cols = self.z.shape
        first = (slice(max(0, -dr), rows - max(0, dr)), slice(max(0, -dc), cols - max(0, dc)))
        second = (slice(max(0, dr), rows - max(0, -dr)), slice(max(0, dc), cols - max(0, -dc)))
        a, b = self.index[first], self.index[second]
        ok = (a >= 0) & (b >= 0)
        norm = math.hypot(dr, dc)
        grade = (np.abs(self.z[second] - self.z[first]) / (self.cell * norm))[ok]
        # 계류 버퍼 안에서는 계류 쪽 · 반대쪽 ±45° 로만(건너기 · 빠져나가기)
        in_buffer = (self.d_stream[first] < self.stream_offset_m) | (
            self.d_stream[second] < self.stream_offset_m
        )
        ax = self.away_x[first] + self.away_x[second]
        ay = self.away_y[first] + self.away_y[second]
        size = np.hypot(ax, ay)
        across = np.abs(ax * dc + ay * dr) >= STREAM_CROSS_COS * norm * size
        along = (in_buffer & ~across & (size > 1e-6))[ok]
        over = np.maximum(0.0, grade - limit)
        # 옆경사 = 진행 방향에 수직인 지반 기울기(두 칸 평균)
        cross = np.abs(
            (self.grad_x[first][ok] + self.grad_x[second][ok]) * -dr
            + (self.grad_y[first][ok] + self.grad_y[second][ok]) * dc
        ) / (2.0 * norm)
        # 토공 가늠(1 m 당 m³) — 옆경사 위 반절 · 반성 단면 W²·s/4 + 상한 넘는 기울기가 측점
        # 반 간격(10 m) 동안 쌓는 높이 차 단면(지형상 불가능할 때만 0 이 아님)
        earth = ROAD_WIDTH_M**2 * cross / 4.0 + ROAD_WIDTH_M * over * 10.0
        offset = 0.5 * (self.d_expected[first][ok] + self.d_expected[second][ok])
        terms = np.stack([offset, earth, (grade / limit) ** 2])
        return a[ok], b[ok], self.cell * norm, terms, over, along, grade


def _turn_steps(min_radius_m: float, cell: float) -> int:
    """꺾은 뒤 곧게 가야 할 칸 수 — 앞뒤 원호 접선이 한 직선에 들어가게.

    원호를 끼우는 쪽(`build_planned_polyline`)은 직선의 절반까지만 접선으로 쓰므로 직선은
    2 · R · tan(θ/2) 이상이어야 한다. θ = 이웃 방향 사이 가장 큰 각(나이트 걸음 26.57°).
    걸음은 칸 길이 단위로 센다 — 곧은 · 대각 걸음 1, 나이트 걸음 2.
    """
    widest = math.atan2(1.0, 2.0)
    return max(1, math.ceil(2.0 * min_radius_m * math.tan(widest / 2.0) / cell))


class RouteGraph:
    """상태 그래프 — 복도 안 모든 링크를 한 번 펼쳐 두고 탐색마다 비용 · 가릴 간선만 바꾼다.

    상태 = (칸, 방향, 층). 층 = 꺾은 뒤 곧게 온 칸 수 + 1(K + 1 에서 멈춤) · 층 0 = 시점.
    이웃 방향으로 꺾기는 K 칸 곧게 온 뒤에만(층 K + 1) → 노드마다 최소 반지름 원호가 들어감.
    시점은 한 칸 더(K + 1 칸) — 기점을 격자 칸 가운데에서 예상노선 끝점으로 옮기며 첫 직선이
    짧아지는 몫. 종점은 K 칸 곧게 들어온 상태를 먼저, 없으면 아무 상태(마지막 곡선 자리가
    모자라면 목록에 뜸).
    """

    def __init__(
        self,
        grid: SearchGrid,
        expected: list[tuple[float, float]],
        *,
        limit: float,
        min_radius_m: float,
    ) -> None:
        self.grid, self.expected, self.limit = grid, expected, limit
        count = len(DIRECTIONS)
        self.layers = _turn_steps(min_radius_m, grid.cell) + 2  # 꺾을 수 있는 층 = layers - 1
        full = self.layers - 1
        angles = [math.atan2(dr, dc) for dr, dc in DIRECTIONS]
        heads, tails, link_of, turn = [], [], [], []
        per_link = []
        offset = 0
        for out, (dr, dc) in enumerate(DIRECTIONS):
            a, b, length, terms, over, along, grade = grid.links(dr, dc, limit=limit)
            ids = np.arange(offset, offset + a.size)
            offset += a.size
            per_link.append((np.full(a.size, length), terms, over, along, grade))
            units = max(1, int(length / grid.cell + 1e-9))  # 곧은 · 대각 1, 나이트 2
            for walked in range(self.layers):  # 곧게
                heads.append(self._state(a, out, walked))
                tails.append(self._state(b, out, min(walked + units, full)))
                link_of.append(ids)
                turn.append(np.zeros(a.size))
            for came in ((out - 1) % count, (out + 1) % count):  # 이웃 방향에서 꺾어 들어옴
                bend = abs((angles[out] - angles[came] + math.pi) % (2.0 * math.pi) - math.pi)
                heads.append(self._state(a, came, full))
                tails.append(self._state(b, out, min(units + 1, full)))
                link_of.append(ids)
                turn.append(np.full(a.size, TURN_COST * min_radius_m * bend))
        if offset == 0:
            raise NoRoute("예상노선 둘레에 지날 수 있는 링크가 없습니다.")
        self.length = np.concatenate([p[0] for p in per_link])
        self.terms = np.hstack([p[1] for p in per_link])
        self.over = np.concatenate([p[2] for p in per_link])
        self.along = np.concatenate([p[3] for p in per_link])
        self.grade = np.concatenate([p[4] for p in per_link])
        self.size = grid.cells * count * self.layers
        # 간선 틀(CSR)은 한 번만 — 탐색마다 못 쓰는 간선을 걸러 내고 비용만 갈아 끼움
        link_of = np.concatenate(link_of)
        order = csr_matrix(
            (np.arange(1.0, link_of.size + 1.0), (np.concatenate(heads), np.concatenate(tails))),
            shape=(self.size, self.size),
        )
        self.indices, self.indptr = order.indices, order.indptr
        edge = (order.data - 1.0).astype(np.int64)
        self.edge_link = link_of[edge]
        self.edge_turn = np.concatenate(turn)[edge]
        start, end = grid.nearest(expected[0]), grid.nearest(expected[-1])
        self.starts = [self._state(start, d, 0) for d in range(count)]
        self.straight = [self._state(end, d, full) for d in range(count)]
        self.ends = [self._state(end, d, w) for d in range(count) for w in range(self.layers)]

    def _state(self, cell, direction: int, walked: int):
        return (cell * len(DIRECTIONS) + direction) * self.layers + walked

    def _usable(self, cap: float, strict_stream: bool) -> np.ndarray:
        """링크별 쓸 수 있음 — `cap` 넘는 기울기 · (엄격하면) 계류 버퍼 안 나란히 빼고."""
        usable = self.grade <= cap
        return usable & ~self.along if strict_stream else usable

    def _graph(self, usable: np.ndarray, link_cost: np.ndarray | None) -> csr_matrix:
        """쓸 수 있는 간선만 남긴 그래프 + 시점 상태 모두로 가는 원점(마지막 상태 `size`)."""
        keep = usable[self.edge_link]
        indptr = np.concatenate([[0], np.cumsum(keep)])[self.indptr]
        indptr = np.append(indptr, indptr[-1] + len(self.starts))
        indices = np.concatenate([self.indices[keep], self.starts])
        if link_cost is None:
            data = np.ones(indices.size)
        else:
            cost = link_cost[self.edge_link[keep]] + self.edge_turn[keep]
            data = np.concatenate([cost, np.full(len(self.starts), 1e-9)])
        return csr_matrix((data, indices, indptr), shape=(self.size + 1, self.size + 1))

    def joined(self, cap: float) -> bool:
        """`cap` 까지 기울기를 풀면 시점 · 종점이 이어지나(계류는 벌점 — 기울기만 따짐)."""
        graph = self._graph(self._usable(cap, False), None)
        reached = breadth_first_order(graph, self.size, return_predecessors=False)
        return bool(np.isin(self.ends, reached).any())

    def solve(
        self,
        weights: dict[str, float],
        *,
        cap: float,
        strict_stream: bool,
        grade_cost: float = 0.0,
    ) -> list[tuple[float, float]]:
        """노드(꺾임점) 목록 — 첫 · 끝은 예상노선 양 끝 그대로.

        세 목표는 쓸 수 있는 링크 평균이 1 이 되게 정규화(길이 가중)한 뒤 비중을 곱한다.
        `grade_cost` = 상한을 넘은 기울기 벌점(1 m 당 × 넘은 비율 / 상한) — `cap` 이 상한보다
        클 때(지형상 불가능)만 뜻이 있다.
        """
        usable = self._usable(cap, strict_stream)
        if not usable.any():
            raise NoRoute("예상노선 둘레에 제약을 지키며 지날 수 있는 링크가 없습니다.")
        length = self.length[usable]
        scale = (self.terms[:, usable] * length).sum(axis=1) / max(length.sum(), 1e-9)
        share = np.array([weights[key] for key in OBJECTIVES]) / max(sum(weights.values()), 1e-9)
        factor = np.divide(share, scale, out=np.zeros_like(share), where=scale > 1e-12)
        penalty = grade_cost * self.over / self.limit
        if not strict_stream:
            penalty = penalty + SOFT_STREAM_COST * self.along
        link_cost = self.length * (LENGTH_COST + factor @ self.terms + penalty)
        distance, predecessors = dijkstra(
            self._graph(usable, link_cost), indices=self.size, return_predecessors=True
        )
        final = min(self.straight, key=lambda s: distance[s])
        if not np.isfinite(distance[final]):
            final = min(self.ends, key=lambda s: distance[s])
        if not np.isfinite(distance[final]):
            raise NoRoute("복도 안에서 제약을 지키며 시점과 종점을 잇는 길이 없습니다.")
        chain = [int(final)]
        while predecessors[chain[-1]] >= 0:
            chain.append(int(predecessors[chain[-1]]))
        chain = chain[::-1][1:]  # 원점 뺌
        # 꺾임점 = 방향이 바뀌는 칸. 첫 · 끝은 예상노선 끝점 그대로(기점 · 종점은 옮기지 않음).
        count, layers = len(DIRECTIONS), self.layers
        nodes = [tuple(self.expected[0])]
        for previous, current in zip(chain[1:], chain[2:]):
            if (previous // layers) % count != (current // layers) % count:
                cell = previous // layers // count
                nodes.append((float(self.grid.centres[cell, 0]), float(self.grid.centres[cell, 1])))
        nodes.append(tuple(self.expected[-1]))
        return nodes

    def minimax_grade(self) -> float:
        """복도 안에서 시점과 종점을 잇는 데 **어떤 길도 넘어야 하는** 가장 작은 기울기 상한(비율).

        꺾기 규칙 없이 칸끼리만 이은 그래프에서 이분 탐색 — 그러니 실제 필요한 값의 아래 한계다.
        """
        grid = self.grid
        pieces = []
        for dr, dc in DIRECTIONS:
            a, b, *_, grade = grid.links(dr, dc, limit=self.limit)
            pieces.append((a, b, grade))
        heads = np.concatenate([p[0] for p in pieces])
        tails = np.concatenate([p[1] for p in pieces])
        grades = np.concatenate([p[2] for p in pieces])
        start, end = grid.nearest(self.expected[0]), grid.nearest(self.expected[-1])

        def joined(cap: float) -> bool:
            keep = grades <= cap
            graph = csr_matrix(
                (np.ones(int(keep.sum())), (heads[keep], tails[keep])),
                shape=(grid.cells, grid.cells),
            )
            _, labels = connected_components(graph, directed=False)
            return bool(labels[start] == labels[end])

        low, high = 0.0, float(grades.max()) if grades.size else 0.0
        if not joined(high):
            raise NoRoute("복도 안에서 시점과 종점이 이어지지 않습니다.")
        for _ in range(12):  # 아래 한계일 뿐 — 구간 1/4096 이면 넉넉함
            middle = 0.5 * (low + high)
            low, high = (low, middle) if joined(middle) else (middle, high)
        return high

    def terrain_bound(self) -> float:
        """반지름 규칙까지 지키며 시점 · 종점을 잇는 데 **어떤 길도 넘어야 하는** 기울기(비율).

        아래 한계 = 꺾기 규칙 없는 최소최대(`minimax_grade`). 이어지는 값을 1.5 배씩 찾은 뒤
        그 사이를 상태 그래프 그대로 이분 탐색한다(계류는 벌점 — 기울기만 따짐).
        """
        ceiling = float(self.grade.max(initial=0.0))
        low = max(self.minimax_grade(), self.limit)
        high = low
        while not self.joined(high):
            if high >= ceiling:
                raise NoRoute("복도 안에서 반지름 규칙을 지키며 시점과 종점을 잇는 길이 없습니다.")
            low, high = high, min(high * 1.5, ceiling)
        for _ in range(BOUND_BISECT):
            if high - low <= 1e-4:
                break
            middle = 0.5 * (low + high)
            low, high = (low, middle) if self.joined(middle) else (middle, high)
        return high