Aislo/B05_Profile/B05_Profile_Engine_RidgeValley_Graph.py

"""B05 능선-계곡 길찾기 — 그래프 뼈대(격자 조회·노드·엣지).

`B05_Profile_Engine_RidgeValley` 에서 떼어낸 앞단이다(700줄 제한, 2026-09-02).
탐색·선형(fillet)·진입점 계산은 원래 파일에 남고, 여기에는 **비용면 격자 조회와
정속경사 세그먼트 판정·엣지 생성**만 둔다. 호출부는 원래 파일뿐이다.
"""

import math
from typing import Any

import numpy as np

# 엣지 후보 탐색 파라미터 (알고리즘 내부 상수)
MAX_EDGE_LEN_M = 400.0
MIN_EDGE_LEN_M = 20.0
MAX_NEIGHBORS_PER_NODE = 16


class _Grid:
    """비용면 격자에 대한 표고/유효성 조회 헬퍼."""

    def __init__(self, x, y, z, valid, grid_res):
        self.x = np.asarray(x, dtype=np.float64)
        self.y = np.asarray(y, dtype=np.float64)
        self.z = np.asarray(z, dtype=np.float64)
        self.valid = np.asarray(valid, dtype=bool)
        self.res = float(grid_res)

    def _idx(self, coords: np.ndarray, v: float) -> int:
        i = int(np.clip(np.searchsorted(coords, v), 0, len(coords) - 1))
        j = max(i - 1, 0)
        return j if abs(v - coords[j]) <= abs(coords[i] - v) else i

    def rc(self, px: float, py: float) -> tuple[int, int]:
        return self._idx(self.y, py), self._idx(self.x, px)

    def z_at(self, px: float, py: float) -> float:
        r, c = self.rc(px, py)
        return float(self.z[r, c])

    def valid_at(self, px: float, py: float) -> bool:
        in_bounds = (self.x[0] <= px <= self.x[-1]) and (self.y[0] <= py <= self.y[-1])
        if not in_bounds:
            return False
        r, c = self.rc(px, py)
        return bool(self.valid[r, c])


def _collect_nodes(
    skeleton: dict[str, Any], spacing_m: float
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
    """능선/계곡 polyline 정점을 spacing 간격으로 다운샘플해 노드 배열을 만든다."""
    pos, kind, on_main = [], [], []

    def _add(polys, k, is_main):
        for item in polys:
            pl = item["polyline"]
            acc = spacing_m
            prev = None
            for p in pl:
                step = spacing_m if prev is None else math.hypot(p[0] - prev[0], p[1] - prev[1])
                acc += step
                prev = p
                if acc >= spacing_m:
                    acc = 0.0
                    pos.append([p[0], p[1], p[2]])
                    kind.append(k)
                    on_main.append(is_main)

    _add(skeleton.get("minor_ridge", []), 0, False)
    _add(skeleton.get("minor_valley", []), 1, False)
    _add(skeleton.get("main_ridge", []), 0, True)
    _add(skeleton.get("main_valley", []), 1, True)

    if not pos:
        return (np.zeros((0, 3)), np.zeros(0, dtype=np.int8), np.zeros(0, dtype=bool))
    return (
        np.asarray(pos, dtype=np.float64),
        np.asarray(kind, dtype=np.int8),
        np.asarray(on_main, dtype=bool),
    )


def _build_barrier_mask(grid: _Grid, skeleton: dict[str, Any]) -> np.ndarray:
    """주능선/주계곡 셀을 True로 표시한 구획 경계 마스크."""
    barrier = np.zeros(grid.z.shape, dtype=bool)
    for key in ("main_ridge", "main_valley"):
        for item in skeleton.get(key, []):
            for p in item["polyline"]:
                r, c = grid.rc(p[0], p[1])
                barrier[r, c] = True
    return barrier


def _segment_feasible(
    a: np.ndarray,
    b: np.ndarray,
    grid: _Grid,
    barrier: np.ndarray,
    blocked_circles: list[dict[str, float]],
    min_grade: float,
    max_grade: float,
    tol: float,
    endpoint_free_m: float,
    enforce_grade_window: bool = True,
) -> bool:
    """a→b 직선이 정속경사 세그먼트로 성립하는지 검사한다."""
    dx, dy = b[0] - a[0], b[1] - a[1]
    length = math.hypot(dx, dy)
    if length < 1e-6:
        return False
    design_grade = (b[2] - a[2]) / length
    g = abs(design_grade)
    if enforce_grade_window:
        if not (min_grade <= g <= max_grade):
            return False
    elif g > max_grade:
        return False

    step = max(grid.res, 1.0)
    n_steps = max(int(length / step), 1)
    for i in range(n_steps + 1):
        t = i / n_steps
        px, py = a[0] + t * dx, a[1] + t * dy
        if not grid.valid_at(px, py):
            return False
        s = t * length
        z_design = a[2] + design_grade * s
        z_terrain = grid.z_at(px, py)
        allowed = tol * max(s, length - s) + grid.res
        if abs(z_terrain - z_design) > allowed:
            return False
        if min(s, length - s) > endpoint_free_m:
            r, c = grid.rc(px, py)
            if barrier[r, c]:
                return False
        for circ in blocked_circles:
            if math.hypot(px - circ["x"], py - circ["y"]) < circ["radius_m"]:
                return False
    return True


def _build_edges(
    pos: np.ndarray,
    kind: np.ndarray,
    grid: _Grid,
    barrier: np.ndarray,
    blocked_circles: list[dict[str, float]],
    min_grade: float,
    max_grade: float,
    tol: float,
    endpoint_free_m: float,
) -> dict[int, list[tuple[int, float]]]:
    """능선↔계곡 노드 쌍의 정속경사 직선 엣지를 만든다 (무방향, 길이 저장)."""
    from scipy.spatial import cKDTree

    adj: dict[int, list[tuple[int, float]]] = {i: [] for i in range(len(pos))}
    if len(pos) == 0:
        return adj

    ridge_idx = np.nonzero(kind == 0)[0]
    valley_idx = np.nonzero(kind == 1)[0]
    if len(ridge_idx) == 0 or len(valley_idx) == 0:
        return adj

    valley_tree = cKDTree(pos[valley_idx, :2])
    for ri in ridge_idx:
        cand = valley_tree.query_ball_point(pos[ri, :2], MAX_EDGE_LEN_M)
        cand = sorted(
            cand,
            key=lambda j: (
                (pos[ri, 0] - pos[valley_idx[j], 0]) ** 2
                + (pos[ri, 1] - pos[valley_idx[j], 1]) ** 2
            ),
        )
        added = 0
        for j in cand:
            vi = int(valley_idx[j])
            length = math.hypot(pos[ri, 0] - pos[vi, 0], pos[ri, 1] - pos[vi, 1])
            if length < MIN_EDGE_LEN_M:
                continue
            if pos[ri, 2] <= pos[vi, 2]:
                continue
            if not _segment_feasible(
                pos[ri],
                pos[vi],
                grid,
                barrier,
                blocked_circles,
                min_grade,
                max_grade,
                tol,
                endpoint_free_m,
            ):
                continue
            adj[int(ri)].append((vi, length))
            adj[vi].append((int(ri), length))
            added += 1
            if added >= MAX_NEIGHBORS_PER_NODE:
                break
    return adj


def _endpoint_connectors(
    pt: dict[str, float],
    pos: np.ndarray,
    grid: _Grid,
    barrier: np.ndarray,
    blocked_circles: list[dict[str, float]],
    max_uphill_grade: float,
    max_downhill_grade: float,
    tol: float,
    endpoint_free_m: float,
) -> list[tuple[int, float]]:
    """BP/CP/EP를 그래프 노드에 잇는 연결 세그먼트 후보."""
    from scipy.spatial import cKDTree

    if len(pos) == 0:
        return []
    p = np.array([pt["x"], pt["y"], grid.z_at(pt["x"], pt["y"])])
    tree = cKDTree(pos[:, :2])
    cand = tree.query_ball_point(p[:2], MAX_EDGE_LEN_M)
    cand = sorted(cand, key=lambda j: (p[0] - pos[j, 0]) ** 2 + (p[1] - pos[j, 1]) ** 2)
    out = []
    for j in cand:
        length = math.hypot(p[0] - pos[j, 0], p[1] - pos[j, 1])
        if length < 1e-6:
            out.append((int(j), max(length, 0.01)))
            continue
        applicable = max_uphill_grade if pos[j, 2] > p[2] else max_downhill_grade
        if _segment_feasible(
            p,
            pos[j],
            grid,
            barrier,
            blocked_circles,
            0.0,
            applicable,
            tol,
            endpoint_free_m,
            enforce_grade_window=False,
        ):
            out.append((int(j), length))
        if len(out) >= MAX_NEIGHBORS_PER_NODE:
            break
    return out


def _turn_angle(p_prev, p_curr, p_next) -> float:
    """진행방향 변화(교각) [rad]. 0 = 직진."""
    v1 = (p_curr[0] - p_prev[0], p_curr[1] - p_prev[1])
    v2 = (p_next[0] - p_curr[0], p_next[1] - p_curr[1])
    n1, n2 = math.hypot(*v1), math.hypot(*v2)
    if n1 < 1e-9 or n2 < 1e-9:
        return 0.0
    cosang = max(-1.0, min(1.0, (v1[0] * v2[0] + v1[1] * v2[1]) / (n1 * n2)))
    return math.acos(cosang)