import tempfile
import unittest
from pathlib import Path

import numpy as np

from B04_wf1_Surface.B04_wf1_Surface_Engine_Pipeline import build_all_terrain_models
from B05_wf2_Route.B05_wf2_Route_Engine_Geometry import (
    circumradius_2d,
    point_to_polyline_dist_2d,
    single_segment_dijkstra,
)
from B05_wf2_Route.B05_wf2_Route_Engine_Solver import solve_optimal_route
from config.config_system import build_surface_model_config


class GeometryTest(unittest.TestCase):
    def test_circumradius_straight_line_is_inf(self) -> None:
        r = circumradius_2d([0, 0], [1, 0], [2, 0])
        self.assertEqual(r, float("inf"))

    def test_circumradius_right_angle(self) -> None:
        r = circumradius_2d([0, 0], [1, 0], [1, 1])
        self.assertTrue(np.isfinite(r) and r > 0)

    def test_point_to_polyline_distance(self) -> None:
        poly = [[0, 0], [10, 0]]
        self.assertAlmostEqual(point_to_polyline_dist_2d(5, 3, poly), 3.0, places=6)

    def test_dijkstra_on_flat_grid(self) -> None:
        coords = np.linspace(0.0, 10.0, 11)
        z = np.full((11, 11), 5.0)
        valid = np.ones((11, 11), dtype=bool)
        dz_dx = np.zeros((11, 11))
        dz_dy = np.zeros((11, 11))
        weights = {"dist": 1.0, "grade": 2.0, "side": 1.5, "curve": 0.5, "avoid": 10.0}
        path = single_segment_dijkstra(
            0,
            0,
            10,
            10,
            coords,
            coords,
            z,
            valid,
            dz_dx,
            dz_dy,
            [],
            weights,
            0.14,
            1.0,
            12.0,
        )
        self.assertTrue(path)
        self.assertEqual(path[0], (0, 0))
        self.assertEqual(path[-1], (10, 10))


class SolveOptimalRouteTest(unittest.TestCase):
    def _make_dtm(self, root: Path) -> None:
        models = root / "B04_wf1_Surface" / "models"
        coords = np.linspace(0.0, 60.0, 61)
        gx, gy = np.meshgrid(coords, coords)
        z = 5.0 + 0.03 * gx + 0.02 * gy
        xyz = np.column_stack([gx.ravel(), gy.ravel(), z.ravel()]).astype(np.float64)
        bounds = np.array([[0.0, 60.0], [0.0, 60.0], [float(z.min()), float(z.max())]])
        cfg = build_surface_model_config()
        cfg["source_filters"] = ["grid_min_z"]
        cfg["precompute"] = ["dtm"]
        build_all_terrain_models(
            {"xyz": xyz, "bounds": bounds},
            {"grid_min_z": np.ones(len(xyz), dtype=bool)},
            models,
            cfg,
        )

    def test_diagonal_route(self) -> None:
        with tempfile.TemporaryDirectory() as directory:
            root = Path(directory) / "proj"
            self._make_dtm(root)
            points = {
                "bp": {"x": 5.0, "y": 5.0},
                "ep": {"x": 55.0, "y": 55.0},
                "cp": [],
                "ap": [],
                "fp": [],
            }
            result = solve_optimal_route(root, "grid_min_z", False, points, {}, method="dtm")

            self.assertGreater(len(result["polyline"]), 2)
            self.assertTrue(result["required_points_ok"])
            self.assertEqual(len(result["segments"]), 1)
            # 대각선 50m×50m → 길이 ≈ 70.7m
            self.assertAlmostEqual(result["metrics"]["length_m"], 70.71, delta=5.0)

    def test_missing_bp_returns_empty(self) -> None:
        with tempfile.TemporaryDirectory() as directory:
            root = Path(directory) / "proj"
            self._make_dtm(root)
            points = {"bp": None, "ep": {"x": 55.0, "y": 55.0}, "cp": [], "ap": [], "fp": []}
            result = solve_optimal_route(root, "grid_min_z", False, points, {}, method="dtm")
            self.assertEqual(result["polyline"], [])
            self.assertFalse(result["required_points_ok"])