"""B05 경로 설계 기하 유틸리티 및 단일 구간 Dijkstra.

8-연결 격자에서 종단경사·측사면·곡률을 비용으로 반영하는 방향 인식
상태공간 Dijkstra와, 곡률/거리/교차 계산 등 순수 기하 헬퍼를 제공한다.
config에 독립적이며 모든 파라미터는 호출측에서 주입한다.
"""

import heapq
import math
from typing import Any

import numpy as np


def get_neighbors(r: int, c: int, rows: int, cols: int):
    """8-연결 이웃을 (nr, nc, dir_idx)로 순회한다. (방향 인덱스 0..7)"""
    neighbors = [
        (-1, 0, 4),  # S
        (-1, 1, 3),  # SE
        (0, 1, 2),  # E
        (1, 1, 1),  # NE
        (1, 0, 0),  # N
        (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 = None
) -> tuple[np.ndarray, np.ndarray]:
    """실제 격자 간격으로 지형 기울기를 계산해 (dz_dx, dz_dy)를 반환한다."""
    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:
    """진행 방향에 수직인 지형 기울기(측사면=절토/성토 프록시)의 크기."""
    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:
    """연속 세 점을 지나는 수평면 외접원 반지름(중간 정점의 이산 곡률 반경)."""
    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")
    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:
    """점에서 폴리라인까지 최소 수평거리(점-선분 거리)."""
    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]]:
    """각 원(AP/FP)에 대해 경로의 최소 이격거리와 원 내부 폴리라인 길이를 보고한다."""
    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)
            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):
    """폴리라인을 고정 호장 간격으로 재샘플한다(도로 스케일 곡률 측정용)."""
    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:
    """8-연결 격자의 단일 방향전환이 함의하는 곡선 반경(m) 근사."""
    if diff <= 0:
        return float("inf")
    angle_rad = math.radians(45.0 * diff)
    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 = None,
    max_downhill_grade: float | None = None,
) -> list[tuple[int, int]]:
    """시작→끝 셀 방향 인식 상태공간 Dijkstra 탐색.

    하드 제약: 종단경사 초과 링크·135도 이상 급전환은 통행불가.
    min_curve_radius_m은 방향전환 소프트 패널티로 반영된다.
    """
    rows, cols = z_grid.shape
    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)

    up_limit = max_uphill_grade if max_uphill_grade else max_grade
    down_limit = max_downhill_grade if max_downhill_grade else max_grade

    dist: dict[tuple[int, int, int], float] = {}
    parent: dict[tuple[int, int, int], tuple[int, int, int]] = {}
    pq: list[tuple[float, int, int, int]] = []

    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

            diff = abs(d - nd)
            diff = min(diff, 8 - diff)
            if diff >= 3:
                continue  # U턴/급전환은 통행불가

            curve_penalty = 0.0
            if diff > 0:
                turn_radius = turn_radius_from_grid(diff, grid_res)
                tightness = min(min_curve_radius_m / turn_radius, 20.0)
                curve_penalty = w_curve * tightness * grid_res

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

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

            applicable_limit = up_limit if dz > 0 else down_limit
            if grade > applicable_limit:
                continue
            f_grade = (grade / applicable_limit) ** 2 * 10.0

            side_slope = side_slope_magnitude(nr - r, nc - c, dz_dx[nr, nc], dz_dy[nr, nc])
            f_side = side_slope * 5.0

            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"]:
                    avoid_penalty += w_avoid * 1000.0 * h_dist

            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 []

    path = []
    curr = best_dest_state
    while curr:
        path.append((curr[0], curr[1]))
        curr = parent.get(curr)
    return path[::-1]