"""B05 능선-계곡 정속경사 임도 길찾기 (대안 알고리즘).

격자 Dijkstra와 분리된 방식: 지형 스켈레톤에서 주/지 능선·계곡 polyline을
얻고, 능선↔계곡 노드 쌍을 잇는 정속경사 직선 세그먼트로 그래프를 만든 뒤
교각 페널티 Dijkstra로 노드 시퀀스를 탐색한다. 최종 선형은 직선+최소회전반경
원호(fillet)로 구성한다. 반환 스키마는 solve_optimal_route()와 동일하다.
"""

import heapq
import math
from pathlib import Path
from typing import Any

import numpy as np

from B05_wf2_Route.B05_wf2_Route_Engine_Geometry import (
    circle_intrusions,
    circumradius_2d,
    point_to_polyline_dist_2d,
    resample_polyline_2d,
)
from B05_wf2_Route.B05_wf2_Route_Engine_Skeleton import load_or_build_skeleton
from B05_wf2_Route.B05_wf2_Route_Engine_Solver import (
    _MODELS_SUBDIR,
    _load_or_build_cost_surface,
)
from config.config_system import (
    FOREST_ROAD_MIN_CURVE_R_M,
    ROUTE_ALT_GRADE_TOLERANCE,
    ROUTE_ALT_MAX_GRADE,
    ROUTE_ALT_MIN_GRADE,
    ROUTE_DEFAULT_GRADE_CLASS,
    ROUTE_REQUIRED_POINT_TOLERANCE_M,
    SKELETON_NODE_SPACING_M,
)

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


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)


def _search_segment(
    start_pt: dict[str, float],
    end_pt: dict[str, float],
    pos: np.ndarray,
    adj: dict[int, list[tuple[int, float]]],
    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,
    min_radius: float,
) -> list[list[float]] | None:
    """start→end를 그래프 위에서 탐색해 노드 좌표 시퀀스를 반환 (실패 시 None)."""
    start_xyz = [start_pt["x"], start_pt["y"], grid.z_at(start_pt["x"], start_pt["y"])]
    end_xyz = [end_pt["x"], end_pt["y"], grid.z_at(end_pt["x"], end_pt["y"])]

    direct_limit = max_uphill_grade if end_xyz[2] > start_xyz[2] else max_downhill_grade
    if _segment_feasible(
        np.asarray(start_xyz),
        np.asarray(end_xyz),
        grid,
        barrier,
        blocked_circles,
        0.0,
        direct_limit,
        tol,
        endpoint_free_m,
        enforce_grade_window=False,
    ):
        return [start_xyz, end_xyz]

    start_conn = _endpoint_connectors(
        start_pt,
        pos,
        grid,
        barrier,
        blocked_circles,
        max_uphill_grade,
        max_downhill_grade,
        tol,
        endpoint_free_m,
    )
    end_conn = _endpoint_connectors(
        end_pt,
        pos,
        grid,
        barrier,
        blocked_circles,
        max_uphill_grade,
        max_downhill_grade,
        tol,
        endpoint_free_m,
    )
    if not start_conn or not end_conn:
        return None
    end_conn_map = {j: length for j, length in end_conn}

    START, MAX_TURN = -1, math.radians(MAX_TURN_DEG)

    def _xy(i):
        return start_xyz if i == START else pos[i]

    def _seg_len(i, j):
        a, b = _xy(i), _xy(j)
        return math.hypot(a[0] - b[0], a[1] - b[1])

    def _fillet_ok(theta, len_in, len_out):
        if theta < 1e-6:
            return True
        t_len = min_radius * math.tan(theta / 2.0)
        return t_len <= len_in / 2.0 and t_len <= len_out / 2.0

    dist: dict[tuple[int, int], float] = {}
    parent: dict[tuple[int, int], tuple[int, int]] = {}
    pq: list[tuple[float, int, int]] = []
    for j, length in start_conn:
        dist[(START, j)] = length
        heapq.heappush(pq, (length, START, j))

    best_state, best_cost = None, float("inf")
    while pq:
        d, u, v = heapq.heappop(pq)
        if d > dist.get((u, v), float("inf")):
            continue
        if v in end_conn_map:
            theta = _turn_angle(_xy(u), _xy(v), end_xyz)
            if theta <= MAX_TURN and _fillet_ok(theta, _seg_len(u, v), end_conn_map[v]):
                total = d + end_conn_map[v] + TURN_PENALTY_W * theta
                if total < best_cost:
                    best_cost, best_state = total, (u, v)
        for w, length in adj.get(v, []):
            if w == u:
                continue
            theta = _turn_angle(_xy(u), _xy(v), pos[w])
            if theta > MAX_TURN:
                continue
            if not _fillet_ok(theta, _seg_len(u, v), length):
                continue
            nd = d + length + TURN_PENALTY_W * theta
            if nd < dist.get((v, w), float("inf")):
                dist[(v, w)] = nd
                parent[(v, w)] = (u, v)
                heapq.heappush(pq, (nd, v, w))

    if best_state is None:
        return None
    seq = [end_xyz]
    state = best_state
    while state is not None:
        u, v = state
        seq.append([float(pos[v][0]), float(pos[v][1]), float(pos[v][2])])
        state = parent.get(state)
        if state is None and u == START:
            break
    seq.append(start_xyz)
    return seq[::-1]


def _fillet_alignment(
    nodes: list[list[float]], radius: float, step_m: float
) -> tuple[list[list[float]], list[float]]:
    """노드 시퀀스를 직선 + 최소회전반경 원호(fillet) 선형으로 변환한다."""
    n = len(nodes)
    if n < 2:
        return [list(p) for p in nodes], [float("inf")] * n

    seg_len, seg_grade = [], []
    for i in range(n - 1):
        length = math.hypot(nodes[i + 1][0] - nodes[i][0], nodes[i + 1][1] - nodes[i][1])
        seg_len.append(max(length, 1e-9))
        seg_grade.append((nodes[i + 1][2] - nodes[i][2]) / max(length, 1e-9))

    t_len = [0.0] * n
    theta = [0.0] * n
    for i in range(1, n - 1):
        th = _turn_angle(nodes[i - 1], nodes[i], nodes[i + 1])
        theta[i] = th
        if th < 1e-6:
            continue
        t = radius * math.tan(th / 2.0)
        t_len[i] = min(t, seg_len[i - 1] / 2.0, seg_len[i] / 2.0)

    poly: list[list[float]] = []
    radii: list[float] = []

    def _append(pt, rad):
        poly.append([float(pt[0]), float(pt[1]), float(pt[2])])
        radii.append(rad)

    _append(nodes[0], float("inf"))
    for i in range(n - 1):
        ax, ay, az = nodes[i]
        bx, by, bz = nodes[i + 1]
        length, g = seg_len[i], seg_grade[i]
        ux, uy = (bx - ax) / length, (by - ay) / length
        s0, s1 = t_len[i], length - t_len[i + 1]
        n_pts = max(int((s1 - s0) / step_m), 1)
        for k in range(n_pts + 1):
            s = s0 + (s1 - s0) * (k / n_pts)
            _append((ax + ux * s, ay + uy * s, az + g * s), float("inf"))
        if i < n - 2 and t_len[i + 1] > 1e-9 and theta[i + 1] > 1e-6:
            th = theta[i + 1]
            t = t_len[i + 1]
            eff_r = t / math.tan(th / 2.0)
            cx0, cy0 = bx - ux * t, by - uy * t
            nx_, ny_, _nz = nodes[i + 2]
            length2 = seg_len[i + 1]
            vx, vy = (nx_ - bx) / length2, (ny_ - by) / length2
            cx1, cy1 = bx + vx * t, by + vy * t
            z_in = az + g * (length - t)
            z_out = bz + seg_grade[i + 1] * t
            arc_len = eff_r * th
            n_arc = max(int(arc_len / step_m), 2)
            cross = ux * vy - uy * vx
            sign = 1.0 if cross >= 0 else -1.0
            ox, oy = cx0 + (-uy * sign) * eff_r, cy0 + (ux * sign) * eff_r
            ang0 = math.atan2(cy0 - oy, cx0 - ox)
            for k in range(1, n_arc):
                a = ang0 + sign * th * (k / n_arc)
                frac = k / n_arc
                _append(
                    (
                        ox + eff_r * math.cos(a),
                        oy + eff_r * math.sin(a),
                        z_in + (z_out - z_in) * frac,
                    ),
                    eff_r,
                )
            _ = (cx1, cy1)
    _append(nodes[-1], float("inf"))

    cleaned, cleaned_r = [poly[0]], [radii[0]]
    for p, r in zip(poly[1:], radii[1:]):
        if math.hypot(p[0] - cleaned[-1][0], p[1] - cleaned[-1][1]) > 0.05:
            cleaned.append(p)
            cleaned_r.append(r)
    if len(cleaned) >= 2:
        cleaned[-1] = poly[-1]
    return cleaned, cleaned_r


def resolve_grade_bounds(options: dict[str, Any]) -> dict[str, float]:
    """options에서 방향별 경사 상/하한을 해석한다."""

    def _opt(key: str, default: float) -> float:
        v = options.get(key)
        return float(v) if v is not None else default

    min_uphill_grade = _opt("min_uphill_grade", ROUTE_ALT_MIN_GRADE)
    min_downhill_grade = _opt("min_downhill_grade", ROUTE_ALT_MIN_GRADE)
    max_uphill_grade = _opt("max_uphill_grade", ROUTE_ALT_MAX_GRADE)
    max_downhill_grade = _opt("max_downhill_grade", ROUTE_ALT_MAX_GRADE)
    return {
        "min_uphill_grade": min_uphill_grade,
        "min_downhill_grade": min_downhill_grade,
        "max_uphill_grade": max_uphill_grade,
        "max_downhill_grade": max_downhill_grade,
        "min_grade_for_edges": max(min_uphill_grade, min_downhill_grade),
        "max_grade_for_edges": min(max_uphill_grade, max_downhill_grade),
    }


def solve_ridge_valley_route(
    project_root: Path,
    filter_key: str,
    smooth: bool,
    points_data: dict[str, Any],
    options: dict[str, Any],
    method: str = "dtm",
) -> dict[str, Any]:
    """능선-계곡 정속경사 방식으로 BP→(CP…)→EP 경로를 계산한다."""
    project_root = Path(project_root)
    models_dir = project_root / _MODELS_SUBDIR

    (x_coords, y_coords, z_grid, valid_mask, _dz_dx, _dz_dy, grid_res) = (
        _load_or_build_cost_surface(project_root, models_dir, filter_key, method, smooth)
    )
    grid = _Grid(x_coords, y_coords, z_grid, valid_mask, grid_res)

    bp = points_data.get("bp")
    ep = points_data.get("ep")
    cp_list = sorted(points_data.get("cp", []), key=lambda x: x.get("order", 0))
    ap_list = points_data.get("ap", [])
    fp_list = points_data.get("fp", [])
    if not bp or not ep:
        raise ValueError("BP/EP 가 배치되어 있지 않습니다.")

    skeleton = load_or_build_skeleton(project_root, filter_key, method, smooth)
    barrier = _build_barrier_mask(grid, skeleton)

    gb = resolve_grade_bounds(options)
    min_uphill_grade = gb["min_uphill_grade"]
    min_downhill_grade = gb["min_downhill_grade"]
    max_uphill_grade = gb["max_uphill_grade"]
    max_downhill_grade = gb["max_downhill_grade"]
    max_grade = gb["max_grade_for_edges"]
    min_grade = gb["min_grade_for_edges"]
    tol = float(options.get("alt_grade_tolerance") or ROUTE_ALT_GRADE_TOLERANCE)
    min_curve_radius_m = options.get("min_curve_radius_m")
    if not min_curve_radius_m or min_curve_radius_m <= 0:
        grade_class = options.get("grade_class", ROUTE_DEFAULT_GRADE_CLASS)
        min_curve_radius_m = FOREST_ROAD_MIN_CURVE_R_M.get(grade_class, 12.0)
    min_curve_radius_m = float(min_curve_radius_m)
    endpoint_free_m = float(SKELETON_NODE_SPACING_M) * 1.5

    blocked = list(fp_list)
    if not options.get("allow_avoid_pass_through", False):
        blocked = blocked + list(ap_list)

    pos, kind, _on_main = _collect_nodes(skeleton, float(SKELETON_NODE_SPACING_M))
    adj = _build_edges(
        pos, kind, grid, barrier, blocked, min_grade, max_grade, tol, endpoint_free_m
    )

    sequence = [bp] + cp_list + [ep]

    def _label(idx):
        if idx == 0:
            return "BP"
        if idx == len(sequence) - 1:
            return "EP"
        return f"CP{sequence[idx].get('order', idx)}"

    all_nodes: list[list[float]] = []
    node_seg_marks: list[int] = []
    for i in range(len(sequence) - 1):
        seg_nodes = _search_segment(
            sequence[i],
            sequence[i + 1],
            pos,
            adj,
            grid,
            barrier,
            blocked,
            max_uphill_grade,
            max_downhill_grade,
            tol,
            endpoint_free_m,
            min_curve_radius_m,
        )
        if not seg_nodes:
            raise ValueError(
                f"세그먼트 {i + 1} ({_label(i)} -> {_label(i + 1)}) 능선-계곡 정속경사 경로 "
                f"탐색 실패: 오르막 {min_uphill_grade * 100:.0f}~{max_uphill_grade * 100:.0f}%·"
                f"내리막 {min_downhill_grade * 100:.0f}~{max_downhill_grade * 100:.0f}%·"
                f"허용오차 ±{tol * 100:.1f}%·최소곡선반지름 {min_curve_radius_m:.0f}m 제약으로 "
                f"성립하는 지능선-지계곡 연결이 없습니다."
            )
        if i == 0:
            all_nodes.extend(seg_nodes)
        else:
            all_nodes.extend(seg_nodes[1:])
        node_seg_marks.append(len(all_nodes) - 1)

    polyline, _vertex_radii = _fillet_alignment(
        all_nodes, min_curve_radius_m, step_m=max(grid.res, 2.0)
    )

    n = len(polyline)
    chainage_m = [0.0] * n
    length_m = 0.0
    max_grade_pct = 0.0
    max_uphill_pct = 0.0
    max_downhill_pct = 0.0
    grade_sums = 0.0
    slope_violations = 0
    for i in range(n - 1):
        x1, y1, z1 = polyline[i]
        x2, y2, z2 = polyline[i + 1]
        hd = math.hypot(x2 - x1, y2 - y1)
        chainage_m[i + 1] = chainage_m[i] + hd
        if hd > 0.01:
            dz = z2 - z1
            s = abs(dz) / hd
            length_m += hd
            grade_sums += s * hd
            max_grade_pct = max(max_grade_pct, s)
            if dz > 0:
                max_uphill_pct = max(max_uphill_pct, s)
                applicable = max_uphill_grade
            else:
                max_downhill_pct = max(max_downhill_pct, s)
                applicable = max_downhill_grade
            if s > applicable + tol:
                slope_violations += 1
    avg_grade_pct = (grade_sums / length_m) if length_m > 0 else 0.0

    curve_check_step = max(2.0 * grid.res, 4.0)
    resampled = resample_polyline_2d(polyline, chainage_m, curve_check_step)
    total_chainage = chainage_m[-1] if chainage_m else 0.0
    curve_violations = 0
    min_curve_radius_actual = float("inf")
    radii = [float("inf")] * len(resampled)
    for i in range(1, len(resampled) - 1):
        radius = circumradius_2d(resampled[i - 1], resampled[i], resampled[i + 1])
        radii[i] = radius
        min_curve_radius_actual = min(min_curve_radius_actual, radius)
        if radius < min_curve_radius_m * 0.99:
            curve_violations += 1

    curve_warning_segments = []
    run_start = None
    for i in range(1, len(resampled)):
        violating = i < len(resampled) - 1 and radii[i] < min_curve_radius_m * 0.99
        if violating and run_start is None:
            run_start = i
        if (not violating) and run_start is not None:
            run_end = i - 1
            ch_s = min(run_start * curve_check_step, total_chainage)
            ch_e = min(run_end * curve_check_step, total_chainage)
            curve_warning_segments.append(
                {
                    "chainage_start_m": round(ch_s, 2),
                    "chainage_end_m": round(ch_e, 2),
                    "min_radius_m": round(min(radii[run_start : run_end + 1]), 2),
                    "required_radius_m": round(min_curve_radius_m, 2),
                    "polyline_start_index": int(np.searchsorted(chainage_m, ch_s)),
                    "polyline_end_index": int(np.searchsorted(chainage_m, ch_e)),
                }
            )
            run_start = None

    def _nearest_vertex(pt) -> int:
        best, best_d = 0, float("inf")
        for i, p in enumerate(polyline):
            d = (p[0] - pt[0]) ** 2 + (p[1] - pt[1]) ** 2
            if d < best_d:
                best, best_d = i, d
        return best

    segments = []
    prev_idx = 0
    for si, mark in enumerate(node_seg_marks):
        end_idx = _nearest_vertex(all_nodes[mark])
        s, e = prev_idx, max(end_idx, prev_idx)
        seg_max_grade = 0.0
        for i in range(s, e):
            x1, y1, z1 = polyline[i]
            x2, y2, z2 = polyline[i + 1]
            hd = math.hypot(x2 - x1, y2 - y1)
            if hd > 0.01:
                seg_max_grade = max(seg_max_grade, abs(z2 - z1) / hd)
        segments.append(
            {
                "index": si,
                "from": _label(si),
                "to": _label(si + 1),
                "point_start": s,
                "point_end": e,
                "chainage_start_m": round(chainage_m[s], 2),
                "chainage_end_m": round(chainage_m[e], 2),
                "length_m": round(chainage_m[e] - chainage_m[s], 2),
                "max_grade_pct": round(seg_max_grade * 100, 2),
            }
        )
        prev_idx = e

    tol_req = ROUTE_REQUIRED_POINT_TOLERANCE_M
    required_point_checks = []
    for idx, pt in enumerate(sequence):
        d = point_to_polyline_dist_2d(pt["x"], pt["y"], polyline)
        required_point_checks.append(
            {
                "point": _label(idx),
                "x": pt["x"],
                "y": pt["y"],
                "distance_m": round(d, 3),
                "snap_distance_m": 0.0,
                "point_on_valid_terrain": grid.valid_at(pt["x"], pt["y"]),
                "tolerance_m": tol_req,
                "within_tolerance": bool(d <= tol_req),
            }
        )
    required_points_ok = all(c["within_tolerance"] for c in required_point_checks)

    avoid_intrusions = circle_intrusions(polyline, ap_list, lambda i: f"AP{i + 1}")
    forbidden_intrusions = circle_intrusions(polyline, fp_list, lambda i: f"FP{i + 1}")

    constant_grade_segments = []
    for i in range(len(all_nodes) - 1):
        length = math.hypot(
            all_nodes[i + 1][0] - all_nodes[i][0], all_nodes[i + 1][1] - all_nodes[i][1]
        )
        if length > 0.01:
            constant_grade_segments.append(
                {
                    "index": i,
                    "length_m": round(length, 2),
                    "grade_pct": round((all_nodes[i + 1][2] - all_nodes[i][2]) / length * 100, 2),
                }
            )

    conditions_snapshot = {
        "filter": filter_key,
        "method": method,
        "smooth": smooth,
        "algorithm": "ridge_valley",
        "grade_class": options.get("grade_class", ROUTE_DEFAULT_GRADE_CLASS),
        "paved": bool(options.get("paved", False)),
        "max_uphill_grade_pct": round(max_uphill_grade * 100, 2),
        "max_downhill_grade_pct": round(max_downhill_grade * 100, 2),
        "min_uphill_grade_pct": round(min_uphill_grade * 100, 2),
        "min_downhill_grade_pct": round(min_downhill_grade * 100, 2),
        "grade_tolerance_pct": round(tol * 100, 2),
        "min_curve_radius_m": round(min_curve_radius_m, 2),
        "weights": options.get("weights") or {},
        "avoid_count": len(ap_list),
        "forbidden_count": len(fp_list),
    }

    return {
        "polyline": polyline,
        "chainage_m": [round(v, 3) for v in chainage_m],
        "segments": segments,
        "required_point_checks": required_point_checks,
        "required_points_ok": required_points_ok,
        "avoid_intrusions": avoid_intrusions,
        "forbidden_intrusions": forbidden_intrusions,
        "curve_warning_segments": curve_warning_segments,
        "avoid_retry_performed": False,
        "conditions_snapshot": conditions_snapshot,
        "constant_grade_segments": constant_grade_segments,
        "metrics": {
            "length_m": round(length_m, 2),
            "avg_grade_pct": round(avg_grade_pct * 100, 2),
            "max_grade_pct": round(max_grade_pct * 100, 2),
            "max_uphill_pct": round(max_uphill_pct * 100, 2),
            "max_downhill_pct": round(max_downhill_pct * 100, 2),
            "slope_violations": slope_violations,
            "curve_violations": curve_violations,
            "min_curve_radius_m": round(min_curve_radius_actual, 2)
            if math.isfinite(min_curve_radius_actual)
            else None,
            "min_curve_radius_limit_m": round(min_curve_radius_m, 2),
        },
    }