import os
import json
import math
import heapq
from pathlib import Path
from typing import Any, Dict, List, Tuple
import numpy as np

# Config references
import config

def get_neighbors(r: int, c: int, rows: int, cols: int):
    # 8-connected neighbors
    # (dr, dc, dir_idx)
    # DIRS: 0: N, 1: NE, 2: E, 3: SE, 4: S, 5: SW, 6: W, 7: NW
    neighbors = [
        (-1, 0, 4),   # S (moving down in rows)
        (-1, 1, 3),   # SE
        (0, 1, 2),    # E
        (1, 1, 1),    # NE
        (1, 0, 0),    # N (moving up in rows)
        (1, -1, 7),   # NW
        (0, -1, 6),   # W
        (-1, -1, 5)   # SW
    ]
    
    for dr, dc, d_idx in neighbors:
        nr, nc = r + dr, c + dc
        if 0 <= nr < rows and 0 <= nc < cols:
            yield nr, nc, d_idx

def compute_gradients(z_grid: np.ndarray, res_y: float, res_x: float = None) -> Tuple[np.ndarray, np.ndarray]:
    """Computes terrain gradients using the ACTUAL grid spacing on each axis.

    np.gradient returns [d/axis0, d/axis1] = [dz/dy (rows), dz/dx (cols)]; we return
    them as (dz_dx, dz_dy) so callers index by true x/y. Pass res_x when the row/column
    spacings differ (don't assume a fixed value).
    """
    if res_x is None:
        res_x = res_y
    dz_dy, dz_dx = np.gradient(z_grid, res_y, res_x)
    return dz_dx, dz_dy


def side_slope_magnitude(dr: int, dc: int, gx: float, gy: float) -> float:
    """Cross-slope: magnitude of the terrain gradient PERPENDICULAR to the travel
    direction. Travel is the grid step (dc along +x/cols, dr along +y/rows); the
    perpendicular of (dx, dy)=(dc, dr) is (-dr, dc). gx=dz/dx, gy=dz/dy.

    For E/W travel this returns |dz/dy|; for N/S travel |dz/dx| — i.e. the slope across
    the road, the cut/fill proxy — never the longitudinal slope along travel.
    """
    norm = math.hypot(dc, dr)
    if norm == 0:
        return 0.0
    px, py = -dr / norm, dc / norm
    return abs(gx * px + gy * py)

def _circumradius_2d(p0, p1, p2) -> float:
    """Horizontal-plane circumradius of the triangle through three consecutive
    polyline points — the discrete curve radius at the middle vertex. Returns inf for
    (near-)collinear points (a straight run has infinite radius)."""
    ax, ay = p0[0], p0[1]
    bx, by = p1[0], p1[1]
    cx, cy = p2[0], p2[1]
    a = math.hypot(bx - cx, by - cy)
    b = math.hypot(ax - cx, ay - cy)
    c = math.hypot(ax - bx, ay - by)
    if a < 0.01 or b < 0.01 or c < 0.01:
        return float("inf")
    # Twice the triangle area (cross product of two edge vectors).
    area2 = abs((bx - ax) * (cy - ay) - (cx - ax) * (by - ay))
    if area2 < 1e-9:
        return float("inf")
    return (a * b * c) / (2.0 * area2)


def _point_to_polyline_dist_2d(px: float, py: float, polyline) -> float:
    """Minimum horizontal distance from a point to a polyline (point-to-segment, not
    just to vertices) — used to verify required points (BP/CP/EP) are actually passed."""
    if not polyline:
        return float("inf")
    best = float("inf")
    for i in range(len(polyline) - 1):
        ax, ay = polyline[i][0], polyline[i][1]
        bx, by = polyline[i + 1][0], polyline[i + 1][1]
        dx, dy = bx - ax, by - ay
        seg2 = dx * dx + dy * dy
        if seg2 <= 1e-12:
            d = math.hypot(px - ax, py - ay)
        else:
            t = max(0.0, min(1.0, ((px - ax) * dx + (py - ay) * dy) / seg2))
            d = math.hypot(px - (ax + t * dx), py - (ay + t * dy))
        if d < best:
            best = d
    if len(polyline) == 1:
        best = math.hypot(px - polyline[0][0], py - polyline[0][1])
    return best


def _circle_intrusions(polyline, circles, labeler) -> List[Dict[str, Any]]:
    """For each circle (AP/FP: center + radius_m), report the route's minimum clearance
    to the center and the polyline length lying inside the circle (계획서 3.2/I-202)."""
    out = []
    for i, circ in enumerate(circles):
        cx, cy, radius = circ["x"], circ["y"], circ["radius_m"]
        clearance = _point_to_polyline_dist_2d(cx, cy, polyline)
        intruded_len = 0.0
        for j in range(len(polyline) - 1):
            ax, ay = polyline[j][0], polyline[j][1]
            bx, by = polyline[j + 1][0], polyline[j + 1][1]
            mx, my = 0.5 * (ax + bx), 0.5 * (ay + by)  # midpoint-inside test
            if math.hypot(mx - cx, my - cy) < radius:
                intruded_len += math.hypot(bx - ax, by - ay)
        out.append({
            "index": i,
            "label": labeler(i),
            "x": cx, "y": cy, "radius_m": radius,
            "min_clearance_m": round(clearance, 3),
            "intrusion_length_m": round(intruded_len, 3),
            "intrudes": bool(clearance < radius or intruded_len > 0.0),
        })
    return out


def _resample_polyline_2d(polyline, chainage_m, step_m: float):
    """Resamples a polyline at fixed arc-length intervals (horizontal plane) so curvature
    can be measured at a road-relevant scale rather than at grid resolution."""
    if len(polyline) < 2 or step_m <= 0:
        return [[p[0], p[1]] for p in polyline]
    total = chainage_m[-1]
    if total <= 0:
        return [[polyline[0][0], polyline[0][1]]]
    targets = np.arange(0.0, total + step_m * 0.5, step_m)
    out = []
    j = 0
    for t in targets:
        while j < len(chainage_m) - 2 and chainage_m[j + 1] < t:
            j += 1
        seg_len = chainage_m[j + 1] - chainage_m[j]
        frac = 0.0 if seg_len <= 1e-9 else (t - chainage_m[j]) / seg_len
        frac = min(max(frac, 0.0), 1.0)
        x = polyline[j][0] + frac * (polyline[j + 1][0] - polyline[j][0])
        y = polyline[j][1] + frac * (polyline[j + 1][1] - polyline[j][1])
        out.append([x, y])
    return out


def turn_radius_from_grid(diff: int, grid_res: float) -> float:
    """Approximates the curve radius (m) implied by a single-cell direction change
    on an 8-connected grid.

    diff is the minimal direction-index delta (0=straight, 1=45°, 2=90°). The turn
    happens over roughly one cell of travel, so we model the radius as the chord
    length (one grid step) divided by the turn angle in radians:
        R ≈ step_len / angle
    diff==0 -> straight (infinite radius).
    """
    if diff <= 0:
        return float("inf")
    angle_rad = math.radians(45.0 * diff)
    # Travel length over which the heading changes (~one cell, diagonal if 45°).
    step_len = grid_res * (math.sqrt(2) if diff == 1 else 1.0)
    return step_len / angle_rad


def single_segment_dijkstra(
    r_start: int, c_start: int,
    r_end: int, c_end: int,
    x_coords: np.ndarray,
    y_coords: np.ndarray,
    z_grid: np.ndarray,
    valid_mask: np.ndarray,
    dz_dx: np.ndarray,
    dz_dy: np.ndarray,
    ap_list: List[Dict[str, Any]],
    weights: Dict[str, float],
    max_grade: float,
    grid_res: float,
    min_curve_radius_m: float,
    max_uphill_grade: float = None,
    max_downhill_grade: float = None,
) -> List[Tuple[int, int]]:
    """Runs a direction-aware state-space Dijkstra search from start to end cell.

    Hard constraints:
      - 종단경사가 max_grade 를 초과하는 링크는 통행불가(∞).
      - 135도 이상의 급격한 방향전환(U-turn)은 통행불가.
    min_curve_radius_m 은 방향전환 비용에 반영되는 소프트 패널티로,
    값이 클수록 완만한(직선에 가까운) 경로를 유도한다(상세는 본문 주석 참조).
    """
    rows, cols = z_grid.shape
    
    # Weights
    w_dist = weights.get("dist", 1.0)
    w_grade = weights.get("grade", 2.0)
    w_side = weights.get("side", 1.5)
    w_curve = weights.get("curve", 0.5)
    w_avoid = weights.get("avoid", 10.0)

    # Separate uphill/downhill hard limits (계획서 6/I-303). Default to max_grade so the
    # behaviour matches the single-limit case when the caller doesn't split them.
    up_limit = max_uphill_grade if max_uphill_grade else max_grade
    down_limit = max_downhill_grade if max_downhill_grade else max_grade

    # State: (r, c, dir_idx)
    # dir_idx = incoming direction index (0..7)
    # Distance/Cost dictionary
    # dist[(r, c, d)] = min_cost
    dist = {}
    parent = {}
    
    # Priority Queue: (cost, r, c, dir_idx)
    pq = []
    
    # Initialize start state with all incoming directions
    for d in range(8):
        dist[(r_start, c_start, d)] = 0.0
        heapq.heappush(pq, (0.0, r_start, c_start, d))
        
    found_dest = False
    best_dest_state = None

    while pq:
        d_cost, r, c, d = heapq.heappop(pq)
        
        if d_cost > dist.get((r, c, d), float('inf')):
            continue
            
        if r == r_end and c == c_end:
            found_dest = True
            best_dest_state = (r, c, d)
            break
            
        for nr, nc, nd in get_neighbors(r, c, rows, cols):
            if not valid_mask[nr, nc]:
                continue
                
            # Curve Penalty & Radius Constraint
            # diff is the difference between incoming direction d and outgoing direction nd
            diff = abs(d - nd)
            diff = min(diff, 8 - diff)

            if diff >= 3:
                # Genuine U-turn/back-bend (>=135 degrees) is never buildable.
                continue

            # Curve-radius-aware soft penalty.
            #
            # On a coarse 8-connected grid a single 45 degree step implies a radius of
            # only ~grid_res, which is far tighter than any realistic minimum curve
            # radius. A gentle large-radius curve is instead realized over MANY cells
            # (plus the spline smoothing post-process), so a hard per-cell radius cut
            # would forbid essentially all turns. We therefore penalize direction
            # changes in proportion to how much gentler the route must be: the larger
            # the required minimum radius, the more each sharp grid turn costs, steering
            # Dijkstra toward straighter polylines that the smoother can round out.
            curve_penalty = 0.0
            if diff > 0:
                turn_radius = turn_radius_from_grid(diff, grid_res)
                # tightness >= 1; grows as the required radius exceeds what this single
                # grid turn represents. Capped so a 90 degree turn stays usable.
                tightness = min(min_curve_radius_m / turn_radius, 20.0)
                curve_penalty = w_curve * tightness * grid_res

            # Horizontal distance calculation
            is_diagonal = (nd % 2 != 0)
            h_dist = grid_res * math.sqrt(2) if is_diagonal else grid_res

            # Grade Calculation
            z_curr = z_grid[r, c]
            z_next = z_grid[nr, nc]
            dz = z_next - z_curr
            grade = abs(dz) / h_dist

            # Hard constraint with separate uphill/downhill limits (I-303). The grade
            # cost is normalised against whichever limit applies to this link.
            applicable_limit = up_limit if dz > 0 else down_limit
            if grade > applicable_limit:
                continue
            f_grade = (grade / applicable_limit) ** 2 * 10.0
            
            # Side (cross) slope = terrain gradient component PERPENDICULAR to travel.
            side_slope = side_slope_magnitude(
                nr - r, nc - c, dz_dx[nr, nc], dz_dy[nr, nc]
            )
            f_side = side_slope * 5.0
            
            # Avoidance points (AP) penalty
            nx_model = x_coords[nc]
            ny_model = y_coords[nr]
            avoid_penalty = 0.0
            for ap in ap_list:
                dist_to_ap = math.hypot(nx_model - ap["x"], ny_model - ap["y"])
                if dist_to_ap < ap["radius_m"]:
                    # Massive penalty to strongly discourage traversing AP
                    avoid_penalty += w_avoid * 1000.0 * h_dist
            
            # Calculate Link Cost
            link_cost = (
                w_dist * h_dist
                + w_grade * f_grade * h_dist
                + w_side * f_side * h_dist
                + curve_penalty
                + avoid_penalty
            )
            
            next_cost = d_cost + link_cost
            state_next = (nr, nc, nd)
            
            if next_cost < dist.get(state_next, float('inf')):
                dist[state_next] = next_cost
                parent[state_next] = (r, c, d)
                heapq.heappush(pq, (next_cost, nr, nc, nd))
                
    if not found_dest:
        return []
        
    # Reconstruct path
    path = []
    curr = best_dest_state
    while curr:
        path.append((curr[0], curr[1]))
        curr = parent.get(curr)
        
    return path[::-1]

def _load_dtm_grid(terrain_dir: Path, filter_key: str, smooth: bool):
    """Loads the regular DTM grid for a filter (x, y, z, valid_mask).

    The DTM is always required because its valid_mask defines the buildable
    footprint reused as the valid region for every surface model.
    """
    suffix = "_smooth" if smooth else ""
    dtm_path = terrain_dir / f"dtm_{filter_key}{suffix}.npz"
    if not dtm_path.exists():
        dtm_path = terrain_dir / f"dtm_{filter_key}.npz"
    if not dtm_path.exists():
        raise FileNotFoundError(f"DTM 파일을 찾을 수 없습니다: dtm_{filter_key}{suffix}.npz")
    d = np.load(dtm_path)
    return (np.asarray(d["x"]), np.asarray(d["y"]),
            np.asarray(d["z"], dtype=np.float64), np.asarray(d["valid_mask"]))


def _sample_surface_on_grid(
    terrain_dir: Path, filter_key: str, method: str, smooth: bool,
    x_coords: np.ndarray, y_coords: np.ndarray, dtm_z: np.ndarray,
) -> np.ndarray:
    """Evaluates the CONFIRMED surface model's elevation on the DTM's regular grid.

    Path cost must reflect the terrain the user confirmed in stage 1, but Dijkstra
    needs a regular raster. So we reconstruct each model from its stage-1 npz and
    sample it onto the DTM grid. `dtm` is already a grid (used directly); the other
    representations are interpolated. Falls back to the DTM grid on any failure so a
    route can always be produced.
    """
    if method == "dtm":
        return dtm_z

    terrain_dir = Path(terrain_dir)
    xx, yy = np.meshgrid(x_coords, y_coords)
    query = np.column_stack([xx.ravel(), yy.ravel()])

    def _finalize(z_flat: np.ndarray) -> np.ndarray:
        z = np.asarray(z_flat, dtype=np.float64).reshape(len(y_coords), len(x_coords))
        # Fill any non-finite samples (outside hull) from the DTM so gradients stay clean.
        bad = ~np.isfinite(z)
        if bad.any():
            z[bad] = dtm_z[bad]
        return z

    try:
        suffix = "_smooth" if smooth else ""
        if method == "tin":
            from scipy.interpolate import LinearNDInterpolator
            path = terrain_dir / f"tin_{filter_key}{suffix}.npz"
            if not path.exists():
                path = terrain_dir / f"tin_{filter_key}.npz"
            d = np.load(path)
            verts = np.asarray(d["vertices"], dtype=np.float64)
            interp = LinearNDInterpolator(verts[:, :2], verts[:, 2])
            return _finalize(interp(query))

        if method == "nurbs":
            from scipy.interpolate import RectBivariateSpline
            d = np.load(terrain_dir / f"nurbs_{filter_key}.npz")
            cx = np.asarray(d["control_x"], dtype=np.float64)
            cy = np.asarray(d["control_y"], dtype=np.float64)
            cz = np.asarray(d["control_z"], dtype=np.float64)
            degree = int(d["degree"][0]) if "degree" in d else 3
            spline = RectBivariateSpline(
                cy, cx, cz,
                kx=min(degree, len(cy) - 1), ky=min(degree, len(cx) - 1),
            )
            # grid=True evaluation over the axes, then flatten in row-major order.
            return _finalize(spline(y_coords, x_coords).ravel())

        if method == "implicit":
            from scipy.interpolate import RBFInterpolator
            d = np.load(terrain_dir / f"implicit_{filter_key}.npz")
            centers = np.asarray(d["centers_xy"], dtype=np.float64)
            cz = np.asarray(d["center_z"], dtype=np.float64)
            smoothing = float(d["smoothing"][0]) if "smoothing" in d else 0.0
            interp = RBFInterpolator(
                centers, cz, neighbors=min(64, len(centers)),
                smoothing=smoothing, kernel="thin_plate_spline",
            )
            out = np.empty(len(query), dtype=np.float64)
            for s in range(0, len(query), 50_000):
                e = min(s + 50_000, len(query))
                out[s:e] = interp(query[s:e])
            return _finalize(out)

        if method == "meshfree":
            from scipy.interpolate import griddata
            path = terrain_dir / f"meshfree_{filter_key}.npz"
            d = np.load(path)
            pts = np.asarray(d["points"], dtype=np.float64)
            z = griddata(pts[:, :2], pts[:, 2], query, method="linear")
            return _finalize(z)
    except Exception as exc:  # pragma: no cover - defensive fallback
        print(f"[route_solver] '{method}' 표면 샘플링 실패 → DTM 표고로 대체합니다: {exc}")
        return dtm_z

    # Unknown method: safest is the DTM grid.
    return dtm_z


def _source_npz_paths(terrain_dir: Path, filter_key: str, method: str, smooth: bool) -> List[Path]:
    """Source model files a cost surface depends on (DTM always; plus the method's npz).
    Only existing paths are returned, in a stable order, for signature hashing."""
    suffix = "_smooth" if smooth else ""
    candidates = [terrain_dir / f"dtm_{filter_key}{suffix}.npz",
                  terrain_dir / f"dtm_{filter_key}.npz"]
    if method != "dtm":
        candidates.append(terrain_dir / f"{method}_{filter_key}{suffix}.npz")
        candidates.append(terrain_dir / f"{method}_{filter_key}.npz")
    seen, out = set(), []
    for p in candidates:
        if p.exists() and p not in seen:
            seen.add(p)
            out.append(p)
    return out


def _cost_surface_signature(terrain_dir: Path, filter_key: str, method: str, smooth: bool) -> str:
    """A signature that changes whenever the cached cost surface must be rebuilt:
    grid resolution + the (name, mtime, size) of every source model file."""
    parts = [f"res={config.ROUTE_GRID_RES_M}", f"method={method}", f"smooth={smooth}"]
    for p in _source_npz_paths(terrain_dir, filter_key, method, smooth):
        st = p.stat()
        parts.append(f"{p.name}:{int(st.st_mtime)}:{st.st_size}")
    return "|".join(parts)


def _build_cost_surface(terrain_dir: Path, filter_key: str, method: str, smooth: bool):
    """Builds the downsampled cost surface (coords, elevation, footprint, gradients)
    for pathfinding.

    Memory strategy (계획서 I-102): the target ~2 m route grid is derived FIRST, then the
    confirmed model is sampled ONLY on those reduced coordinates — the full-resolution
    meshgrid/interpolation over the (potentially ~50M-cell) source grid is never built.
    The DTM's own z/valid_mask are downsampled by strided slicing and freed immediately.
    """
    import time
    t0 = time.time()

    x_full, y_full, dtm_z_full, valid_full = _load_dtm_grid(terrain_dir, filter_key, smooth)
    rows_full, cols_full = dtm_z_full.shape

    # 1. Reduced 2 m coordinate axes (actual spacing, not a hardcoded 2.0).
    target_res = config.ROUTE_GRID_RES_M
    src_res = (x_full[-1] - x_full[0]) / (len(x_full) - 1)
    step = max(1, int(round(target_res / src_res)))
    x_sub = np.ascontiguousarray(x_full[::step])
    y_sub = np.ascontiguousarray(y_full[::step])

    # 2. Size guard BEFORE allocating the 2 m grid.
    n_cells = len(x_sub) * len(y_sub)
    if n_cells > config.ROUTE_MAX_COST_CELLS:
        raise ValueError(
            f"경로 비용면 격자 셀 수({n_cells:,})가 한도({config.ROUTE_MAX_COST_CELLS:,})를 "
            f"초과합니다. config.ROUTE_GRID_RES_M({target_res} m)를 키우거나 영역을 줄이세요."
        )

    # 3. DTM elevation + footprint at 2 m via strided slicing, then free full-res arrays.
    dtm_z_sub = np.array(dtm_z_full[::step, ::step], dtype=np.float64)
    valid_sub = np.array(valid_full[::step, ::step])
    del dtm_z_full, valid_full

    # 4. Sample the CONFIRMED model on the reduced grid only (no full-res meshgrid).
    z_sub = _sample_surface_on_grid(
        terrain_dir, filter_key, method, smooth, x_sub, y_sub, dtm_z_sub
    )
    z_sub = np.asarray(z_sub, dtype=np.float64)

    # 5. Clean NaNs before caching so gradients (and the cache) stay finite.
    if not np.all(np.isfinite(z_sub)):
        finite = z_sub[np.isfinite(z_sub)]
        fill = float(finite.mean()) if finite.size else 0.0
        z_sub = np.where(np.isfinite(z_sub), z_sub, fill)

    # 6. Gradients use the ACTUAL subsampled spacing on each axis.
    res_y = (y_sub[-1] - y_sub[0]) / (len(y_sub) - 1) if len(y_sub) > 1 else target_res
    res_x = (x_sub[-1] - x_sub[0]) / (len(x_sub) - 1) if len(x_sub) > 1 else target_res
    dz_dx, dz_dy = compute_gradients(z_sub, res_y, res_x)

    # 7. One-line build log (계획서 I-102: 원본격자/경로격자/메모리/시간).
    route_bytes = z_sub.nbytes + valid_sub.nbytes + dz_dx.nbytes + dz_dy.nbytes
    print(
        f"[route_solver] cost surface built: src={rows_full}x{cols_full} "
        f"route={len(y_sub)}x{len(x_sub)} (res~{res_x:.2f}m) "
        f"mem~{route_bytes/1e6:.1f}MB time={time.time()-t0:.2f}s "
        f"[{method}/{filter_key}{'_smooth' if smooth else ''}]"
    )
    # Return the ACTUAL grid spacing as grid_res so distances/h_dist aren't hardcoded.
    grid_res = float(0.5 * (res_x + res_y))
    return x_sub, y_sub, z_sub, valid_sub, dz_dx, dz_dy, grid_res


def _load_or_build_cost_surface(terrain_dir: Path, filter_key: str, method: str, smooth: bool):
    """Returns the cost surface, reusing the on-disk cache when its signature matches
    the current source models + grid resolution; otherwise rebuilds and re-caches.
    (계획서 6.1/6.3: cost_surface 캐시 + config/소스 서명 기반 재사용)"""
    cache_dir = terrain_dir.parent / "route_design"
    suffix = "_smooth" if smooth else ""
    cache_path = cache_dir / f"cost_surface_{filter_key}_{method}{suffix}.npz"
    signature = _cost_surface_signature(terrain_dir, filter_key, method, smooth)

    if cache_path.exists():
        try:
            cached = np.load(cache_path, allow_pickle=False)
            if str(cached["signature"]) == signature:
                return (cached["x"], cached["y"], cached["z"], cached["valid_mask"],
                        cached["dz_dx"], cached["dz_dy"], float(cached["target_res"][0]))
        except Exception as exc:  # corrupt/old cache -> rebuild
            print(f"[route_solver] 비용면 캐시 무시(재생성): {exc}")

    surface = _build_cost_surface(terrain_dir, filter_key, method, smooth)
    x_sub, y_sub, z_sub, valid_sub, dz_dx, dz_dy, target_res = surface
    try:
        cache_dir.mkdir(parents=True, exist_ok=True)
        np.savez_compressed(
            cache_path, x=x_sub, y=y_sub, z=z_sub, valid_mask=valid_sub,
            dz_dx=dz_dx, dz_dy=dz_dy,
            target_res=np.array([target_res], np.float64),
            signature=np.array(signature),
        )
    except Exception as exc:  # caching is best-effort; never fail the solve over it
        print(f"[route_solver] 비용면 캐시 저장 실패(무시): {exc}")
    return surface


def solve_optimal_route(
    project_id: str,
    filter_key: str,
    smooth: bool,
    points_data: Dict[str, Any],
    options: Dict[str, Any],
    instance_dir: Path,
    method: str = "dtm",
    _avoid_retry: bool = False,
) -> Dict[str, Any]:
    """Orchestrates multi-segment cost-surface pathfinding and returns the final
    coordinates and metrics.

    The cost surface uses the elevation of the CONFIRMED (filter, method) model from
    stage 1, sampled onto the DTM's regular grid (Dijkstra requires a raster). The
    DTM's valid_mask defines the buildable footprint.
    """
    terrain_dir = instance_dir / project_id / "terrain_models"

    # Cost surface (downsampled grid + footprint + gradients) for the confirmed
    # (filter, method, smooth) model, reusing the on-disk cache when valid.
    (x_coords_sub, y_coords_sub, z_grid_sub, valid_mask_sub,
     dz_dx, dz_dy, target_res) = _load_or_build_cost_surface(
        terrain_dir, filter_key, method, smooth
    )

    # Points
    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", [])

    # Forbidden zones (FP): bake into the footprint as a HARD constraint — cells inside
    # any FP circle become invalid so Dijkstra can never traverse them (계획서 3.3/I-203).
    if fp_list:
        valid_mask_sub = np.array(valid_mask_sub, copy=True)
        xx, yy = np.meshgrid(x_coords_sub, y_coords_sub)
        for fp in fp_list:
            inside = (xx - fp["x"]) ** 2 + (yy - fp["y"]) ** 2 < float(fp["radius_m"]) ** 2
            valid_mask_sub[inside] = False

    if not bp or not ep:
        return {
            "polyline": [],
            "chainage_m": [],
            "segments": [],
            "required_point_checks": [],
            "required_points_ok": False,
            "avoid_intrusions": [],
            "forbidden_intrusions": [],
            "curve_warning_segments": [],
            "avoid_retry_performed": False,
            "conditions_snapshot": {},
            "metrics": {
                "length_m": 0.0,
                "avg_grade_pct": 0.0,
                "max_grade_pct": 0.0,
                "slope_violations": 0,
                "curve_violations": 0,
                "min_curve_radius_m": None,
                "min_curve_radius_limit_m": 0.0,
            }
        }
        
    # Build target sequence of points: BP -> CP1 -> CP2 ... -> EP
    sequence = [bp] + cp_list + [ep]
    
    def _nearest_coord_index(coords: np.ndarray, value: float) -> int:
        """Return the genuinely nearest grid coordinate (searchsorted alone biases high)."""
        upper = int(np.clip(np.searchsorted(coords, value), 0, len(coords) - 1))
        lower = max(upper - 1, 0)
        return lower if abs(value - coords[lower]) <= abs(coords[upper] - value) else upper

    # Helper to find closest grid index, snapping to closest valid cell if necessary.
    # Also report whether the point's own nearest raster cell belongs to the valid
    # terrain footprint. Grid-centre distance itself is not a route pass error.
    def get_grid_indices(pt: Dict[str, float]) -> Tuple[int, int, bool]:
        c = _nearest_coord_index(x_coords_sub, pt["x"])
        r = _nearest_coord_index(y_coords_sub, pt["y"])
        in_bounds = (
            float(x_coords_sub[0]) <= pt["x"] <= float(x_coords_sub[-1])
            and float(y_coords_sub[0]) <= pt["y"] <= float(y_coords_sub[-1])
        )
        point_on_valid_terrain = in_bounds and bool(valid_mask_sub[r, c])

        if not point_on_valid_terrain:
            valid_ys, valid_xs = np.where(valid_mask_sub)
            if len(valid_ys) > 0:
                dists = (valid_ys - r) ** 2 + (valid_xs - c) ** 2
                best_idx = np.argmin(dists)
                r, c = int(valid_ys[best_idx]), int(valid_xs[best_idx])
        return r, c, point_on_valid_terrain

    # Snap record per required point: the valid cell it maps to, that cell's model
    # coordinate, and the horizontal distance from the user's point to that cell.
    # This drives both pinning (I-103) and the required-point tolerance check (I-104).
    tol_req = config.ROUTE_REQUIRED_POINT_TOLERANCE_M
    required_snap = []
    for pt in sequence:
        r, c, point_on_valid_terrain = get_grid_indices(pt)
        sx, sy = float(x_coords_sub[c]), float(y_coords_sub[r])
        snap_dist = math.hypot(pt["x"] - sx, pt["y"] - sy)
        required_snap.append({
            "r": r, "c": c, "snap_x": sx, "snap_y": sy,
            "snap_dist": snap_dist, "point_on_valid_terrain": point_on_valid_terrain,
        })

    # Weights and options.
    # `.get("weights")` may return None (key present but null), so fall back via `or`.
    weights = options.get("weights") or {
        "dist": config.ROUTE_W_DIST,
        "grade": config.ROUTE_W_GRADE,
        "side": config.ROUTE_W_SIDE,
        "curve": config.ROUTE_W_CURVE,
        "avoid": config.ROUTE_W_AVOID
    }
    paved = options.get("paved", False)

    # Resolve the longitudinal-grade hard limit from road grade class + pavement.
    grade_class = options.get("grade_class", config.ROUTE_DEFAULT_GRADE_CLASS)
    base_max_grade = config.FOREST_ROAD_MAX_GRADE.get(grade_class, config.ROUTE_MAX_GRADE)
    if paved:
        # Pavement raises the limit, but never below the unpaved base for that class.
        max_grade = max(base_max_grade, config.ROUTE_MAX_GRADE_PAVED)
    else:
        max_grade = base_max_grade

    # Minimum curve radius (m): user-provided, else the grade-class default.
    min_curve_radius_m = options.get("min_curve_radius_m")
    if not min_curve_radius_m or min_curve_radius_m <= 0:
        min_curve_radius_m = config.FOREST_ROAD_MIN_CURVE_R_M.get(grade_class, 12.0)

    # Separate uphill/downhill grade limits (계획서 6/I-303). Default to max_grade.
    def _grade_opt(key: str) -> float:
        v = options.get(key)
        return float(v) if (v is not None and float(v) > 0) else max_grade
    max_uphill_grade = _grade_opt("max_uphill_grade")
    max_downhill_grade = _grade_opt("max_downhill_grade")

    def _point_label(idx: int, pt: Dict[str, Any]) -> str:
        if idx == 0:
            return "BP"
        if idx == len(sequence) - 1:
            return "EP"
        return f"CP{pt.get('order', idx)}"

    full_path_grid = []
    # Record each segment's [start, end] index range into the final polyline so the
    # next stage (종·횡단 측점 생성) can split the route by BP→CP1→…→EP.
    segment_bounds: List[Dict[str, Any]] = []

    # Solve segments sequentially
    for i in range(len(sequence) - 1):
        pt_start = sequence[i]
        pt_end = sequence[i + 1]

        r_s, c_s, _ = get_grid_indices(pt_start)
        r_e, c_e, _ = get_grid_indices(pt_end)

        segment = single_segment_dijkstra(
            r_s, c_s, r_e, c_e,
            x_coords_sub, y_coords_sub, z_grid_sub, valid_mask_sub,
            dz_dx, dz_dy, ap_list, weights, max_grade, target_res,
            min_curve_radius_m, max_uphill_grade, max_downhill_grade
        )

        if not segment:
            fp_note = "·금지구역(FP)" if fp_list else ""
            raise ValueError(
                f"세그먼트 {i+1} ({_point_label(i, pt_start)} -> {_point_label(i+1, pt_end)}) "
                f"경로 탐색 실패: 종단경사 한계({max_grade*100:.0f}%)·최소곡선반지름"
                f"({min_curve_radius_m:.0f}m)·회피지역{fp_note} 제약으로 통과 경로가 없습니다."
            )

        # Concatenate paths (avoid duplicating the shared endpoint between segments).
        start_idx = max(len(full_path_grid) - 1, 0)
        if i > 0 and len(segment) > 0:
            full_path_grid.extend(segment[1:])
        else:
            full_path_grid.extend(segment)
        segment_bounds.append({
            "index": i,
            "from": _point_label(i, pt_start),
            "to": _point_label(i + 1, pt_end),
            "point_start": start_idx,
            "point_end": len(full_path_grid) - 1,
        })

    # Convert grid path back to model coordinates
    polyline = []
    for r, c in full_path_grid:
        x = float(x_coords_sub[c])
        y = float(y_coords_sub[r])
        z = float(z_grid_sub[r, c])
        polyline.append([x, y, z])
        
    # 2. Spline smoothing post-processing (체감 평활)
    # Moving-average smooth (preserves point count, so segment_bounds indices stay valid).
    # All three coordinates are averaged together: re-snapping Z to the nearest grid
    # cell instead would inject stair-step noise and spurious grade spikes that violate
    # the grade limit the search already enforced. Averaging Z keeps the longitudinal
    # profile consistent with the constrained path while still hugging the terrain.
    # Required-point vertices (BP, every CP junction, EP) are PINNED to the user's EXACT
    # (x, y) so the final polyline passes through them within tolerance, instead of
    # drifting (smoothing) or sitting on the snapped grid node up to ~grid_res/2 away
    # (계획서 I-103/8장: 시작·끝점 고정 + 1m 통과 보정). Z is sampled from the grid.
    def _grid_z(px: float, py: float) -> float:
        c_idx = _nearest_coord_index(x_coords_sub, px)
        r_idx = _nearest_coord_index(y_coords_sub, py)
        return float(z_grid_sub[r_idx, c_idx])

    def _pin_coord_for(seq_idx: int) -> List[float]:
        # A valid raster cell represents an area, not only its centre. Therefore pin
        # to the user's exact coordinate whenever that coordinate maps to valid terrain;
        # do not mistake centre-to-point grid distance for a route pass error.
        snap = required_snap[seq_idx]
        pt = sequence[seq_idx]
        if snap["point_on_valid_terrain"]:
            return [pt["x"], pt["y"], _grid_z(pt["x"], pt["y"])]
        return [snap["snap_x"], snap["snap_y"], _grid_z(snap["snap_x"], snap["snap_y"])]

    pin_coords: Dict[int, List[float]] = {}
    for sb in segment_bounds:
        pin_coords[sb["point_start"]] = _pin_coord_for(sb["index"])
        pin_coords[sb["point_end"]] = _pin_coord_for(sb["index"] + 1)

    if len(polyline) > 4:
        original = polyline
        smoothed_polyline = []
        window_size = 3
        padded = [original[0]] * (window_size // 2) + original + [original[-1]] * (window_size // 2)
        for i in range(len(original)):
            if i in pin_coords:
                smoothed_polyline.append(list(pin_coords[i]))  # exact required point
                continue
            window = padded[i : i + window_size]
            sx = sum(p[0] for p in window) / window_size
            sy = sum(p[1] for p in window) / window_size
            sz = sum(p[2] for p in window) / window_size
            smoothed_polyline.append([sx, sy, sz])
        polyline = smoothed_polyline
    else:
        # Short paths skip smoothing; still pin required points to exact coordinates.
        for i, coord in pin_coords.items():
            if 0 <= i < len(polyline):
                polyline[i] = list(coord)

    # 3. Metrics + chainage on the FINAL (returned) geometry.
    n = len(polyline)
    chainage_m = [0.0] * n          # cumulative horizontal distance per vertex (측점 산출용)
    length_m = 0.0
    slope_violations = 0
    max_grade_pct = 0.0
    grade_sums = 0.0

    max_uphill_pct = 0.0
    max_downhill_pct = 0.0
    for i in range(n - 1):
        x1, y1, z1 = polyline[i]
        x2, y2, z2 = polyline[i + 1]
        h_dist = math.hypot(x2 - x1, y2 - y1)
        chainage_m[i + 1] = chainage_m[i] + h_dist
        if h_dist > 0.01:
            dz = z2 - z1
            segment_slope = abs(dz) / h_dist
            length_m += h_dist
            grade_sums += segment_slope * h_dist
            max_grade_pct = max(max_grade_pct, segment_slope)
            # Separate uphill/downhill limits (I-303).
            applicable = max_uphill_grade if dz > 0 else max_downhill_grade
            if dz > 0:
                max_uphill_pct = max(max_uphill_pct, segment_slope)
            else:
                max_downhill_pct = max(max_downhill_pct, segment_slope)
            if segment_slope > applicable:
                slope_violations += 1

    avg_grade_pct = (grade_sums / length_m) if length_m > 0 else 0.0

    # Curve-radius check on the smoothed polyline (계획서 5.4: 평활 후 곡선반지름 점검).
    # Measured on the polyline RESAMPLED at a fixed arc-length step, not on raw grid
    # vertices: adjacent 2 m cells would otherwise report grid-discretization radii
    # (~1-3 m) rather than the actual design curvature. The step is a few cells so the
    # chord reflects real road bending.
    curve_check_step = max(2.0 * target_res, 4.0)
    resampled = _resample_polyline_2d(polyline, chainage_m, curve_check_step)
    total_chainage = chainage_m[-1] if chainage_m else 0.0

    def _poly_index_at_chainage(ch: float) -> int:
        # Nearest polyline vertex to a given cumulative distance.
        idx = int(np.searchsorted(chainage_m, ch)) if chainage_m else 0
        return int(min(max(idx, 0), max(len(polyline) - 1, 0)))

    curve_violations = 0
    min_curve_radius_actual = float("inf")
    # radii[i] is the curve radius at resampled point i (interior only).
    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:
            curve_violations += 1

    # Merge consecutive sub-radius samples into warning segments (계획서 5/I-301).
    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
        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": _poly_index_at_chainage(ch_s),
                "polyline_end_index": _poly_index_at_chainage(ch_e),
            })
            run_start = None

    # Per-segment length (uses chainage at the recorded boundary indices).
    segments = []
    for sb in segment_bounds:
        s, e = sb["point_start"], min(sb["point_end"], n - 1)
        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": sb["index"],
            "from": sb["from"],
            "to": sb["to"],
            "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),
        })

    # Required-point pass check (계획서 I-103/8장): distance from each BP/CP/EP to the
    # final polyline. When the user's point is on valid terrain this is ~0 (pinned);
    # when it sits farther than tolerance from valid terrain, the snap distance governs
    # and the point is flagged out-of-tolerance rather than silently moved.
    required_point_checks = []
    for idx, pt in enumerate(sequence):
        label = _point_label(idx, pt)
        poly_dist = _point_to_polyline_dist_2d(pt["x"], pt["y"], polyline)
        snap = required_snap[idx]
        snap_dist = snap["snap_dist"]
        # On valid terrain, the final route-to-point distance is the pass criterion.
        # Only an input outside the valid footprint uses distance to the snapped valid
        # cell, preventing an out-of-surface point from being silently accepted.
        dist = poly_dist if snap["point_on_valid_terrain"] else max(poly_dist, snap_dist)
        required_point_checks.append({
            "point": label,
            "x": pt["x"],
            "y": pt["y"],
            "distance_m": round(dist, 3),
            "snap_distance_m": round(snap_dist, 3),
            "point_on_valid_terrain": snap["point_on_valid_terrain"],
            "tolerance_m": tol_req,
            "within_tolerance": bool(dist <= tol_req),
        })
    required_points_ok = all(c["within_tolerance"] for c in required_point_checks)

    # AP intrusion post-check (계획서 3.2/I-202). AP is a soft-avoid region: if the route
    # still cut through one, retry ONCE with a boosted avoid weight before reporting.
    avoid_intrusions = _circle_intrusions(polyline, ap_list, lambda i: f"AP{i+1}")
    # FP is a hard constraint, so this should always report no intrusion — included as a
    # verifiable safety invariant (계획서 11.2 forbidden_intrusions).
    forbidden_intrusions = _circle_intrusions(polyline, fp_list, lambda i: f"FP{i+1}")
    allow_pass = bool(options.get("allow_avoid_pass_through", False))
    any_intrusion = any(a["intrudes"] for a in avoid_intrusions)
    if any_intrusion and not allow_pass and not _avoid_retry:
        boosted = dict(options)
        bw = dict(weights)
        bw["avoid"] = min(bw.get("avoid", config.ROUTE_W_AVOID) * 10.0, config.ROUTE_WEIGHT_MAX)
        boosted["weights"] = bw
        try:
            retried = solve_optimal_route(
                project_id, filter_key, smooth, points_data, boosted,
                instance_dir, method=method, _avoid_retry=True,
            )
            retried["avoid_retry_performed"] = True
            return retried
        except Exception:
            pass  # retry failed (e.g. isolation) -> fall through with the warning result

    # Snapshot of the conditions that produced this route (계획서 11.2/I-304).
    conditions_snapshot = {
        "filter": filter_key,
        "method": method,
        "smooth": smooth,
        "grade_class": grade_class,
        "paved": paved,
        "max_grade_pct": round(max_grade * 100, 2),
        "max_uphill_grade_pct": round(max_uphill_grade * 100, 2),
        "max_downhill_grade_pct": round(max_downhill_grade * 100, 2),
        "min_curve_radius_m": round(min_curve_radius_m, 2),
        "weights": weights,
        "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": _avoid_retry,
        "conditions_snapshot": conditions_snapshot,
        "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),
        },
    }