import sys
from pathlib import Path
import numpy as np

# Add project root to sys.path
root_dir = Path(__file__).resolve().parent.parent
if str(root_dir) not in sys.path:
    sys.path.insert(0, str(root_dir))

import config
from utils.route_solver import (
    solve_optimal_route, turn_radius_from_grid,
    side_slope_magnitude, compute_gradients,
)

INSTANCE_DIR = root_dir / "instance"
PROJECT_ID = "upload_sample"
FILTER_KEY = "csf"
DTM_PATH = INSTANCE_DIR / PROJECT_ID / "terrain_models" / f"dtm_{FILTER_KEY}_smooth.npz"


def _valid_points_from_cost_surface(n=2):
    """Pick points that lie exactly on valid (buildable) cost-surface cells, so grid
    snap distance is ~0 — required for the 1m required-point checks (I-103/I-104)."""
    from utils.route_solver import _build_cost_surface
    td = INSTANCE_DIR / PROJECT_ID / "terrain_models"
    x_sub, y_sub, z_sub, valid_sub, *_ = _build_cost_surface(td, FILTER_KEY, "dtm", True)
    ys, xs = np.where(valid_sub)
    # Spread the chosen cells across the valid region.
    picks = np.linspace(0, len(ys) - 1, n, dtype=int)
    return [{"x": float(x_sub[xs[i]]), "y": float(y_sub[ys[i]]), "z": 0.0} for i in picks]


def _points_from_dtm():
    """Two valid (buildable) points across the terrain, derived from the real cost
    surface so grid snap distance is ~0 and the fixture tracks the sample data."""
    p = _valid_points_from_cost_surface(2)
    return {"bp": p[0], "ep": p[1], "cp": [], "ap": []}


def _solve(options, points=None):
    return solve_optimal_route(
        project_id=PROJECT_ID,
        filter_key=FILTER_KEY,
        smooth=True,
        points_data=points or _points_from_dtm(),
        options=options,
        instance_dir=INSTANCE_DIR,
    )


def test_side_slope_axis():
    """Cross-slope must use the gradient PERPENDICULAR to travel (I-101).

    On a plane tilted purely in +x (gx=s, gy=0):
      - N/S travel (perp = ±x) sees the full cross slope s,
      - E/W travel (along x) sees zero cross slope,
      - a 45° diagonal sees s/√2.
    """
    s = 0.3
    gx, gy = s, 0.0  # plane rises only in x
    # E/W travel: dc=±1, dr=0 -> perpendicular is y -> cross slope 0
    assert abs(side_slope_magnitude(0, 1, gx, gy) - 0.0) < 1e-9
    assert abs(side_slope_magnitude(0, -1, gx, gy) - 0.0) < 1e-9
    # N/S travel: dr=±1, dc=0 -> perpendicular is x -> full slope s
    assert abs(side_slope_magnitude(1, 0, gx, gy) - s) < 1e-9
    assert abs(side_slope_magnitude(-1, 0, gx, gy) - s) < 1e-9
    # Diagonal NE: perpendicular (-1,1)/√2 -> |gx*-1|/√2 = s/√2
    assert abs(side_slope_magnitude(1, 1, gx, gy) - s / np.sqrt(2)) < 1e-9
    # Symmetric check: plane tilted only in +y, E/W travel sees full slope
    assert abs(side_slope_magnitude(0, 1, 0.0, s) - s) < 1e-9
    assert abs(side_slope_magnitude(1, 0, 0.0, s) - 0.0) < 1e-9
    print("[ok] side slope uses perpendicular (cross) axis for all directions")


def test_compute_gradients_axis_order():
    """compute_gradients returns (dz_dx, dz_dy) aligned to columns(x)/rows(y)."""
    # z increases with x (columns): z[r,c] = 2*c, res=1 -> dz_dx=2, dz_dy=0
    z = np.tile(np.arange(5, dtype=float) * 2.0, (4, 1))  # shape (rows=4, cols=5)
    dz_dx, dz_dy = compute_gradients(z, 1.0)
    assert np.allclose(dz_dx, 2.0), dz_dx
    assert np.allclose(dz_dy, 0.0), dz_dy
    print("[ok] compute_gradients axis order correct (dz_dx along columns)")


def test_cost_surface_is_reduced_grid():
    """The built cost surface must be the reduced ~2 m grid, not the full source grid
    (계획서 I-102: 원본 전체 meshgrid 없이 2m 비용면 생성)."""
    from utils.route_solver import _build_cost_surface, _load_dtm_grid
    td = INSTANCE_DIR / PROJECT_ID / "terrain_models"
    x_full, y_full, _, _ = _load_dtm_grid(td, FILTER_KEY, True)
    src_cells = len(x_full) * len(y_full)
    x_sub, y_sub, z_sub, valid_sub, dz_dx, dz_dy, grid_res = _build_cost_surface(td, FILTER_KEY, "dtm", True)
    route_cells = z_sub.shape[0] * z_sub.shape[1]
    assert z_sub.shape == (len(y_sub), len(x_sub))
    assert route_cells <= config.ROUTE_MAX_COST_CELLS
    assert route_cells < src_cells          # genuinely downsampled
    assert 1.5 <= grid_res <= 3.0           # actual spacing near the 2 m target
    print(f"[ok] cost surface reduced: src_cells={src_cells:,} -> route_cells={route_cells:,}, grid_res={grid_res:.2f}m")


def test_size_guard_blocks_oversized_grid():
    """Predicted grid beyond the cell limit is rejected before allocation."""
    from utils.route_solver import _build_cost_surface
    td = INSTANCE_DIR / PROJECT_ID / "terrain_models"
    saved = config.ROUTE_MAX_COST_CELLS
    config.ROUTE_MAX_COST_CELLS = 1000
    try:
        _build_cost_surface(td, FILTER_KEY, "dtm", True)
        raise AssertionError("size guard did not trigger")
    except ValueError:
        print("[ok] size guard rejects oversized cost surface")
    finally:
        config.ROUTE_MAX_COST_CELLS = saved


def test_turn_radius_monotonic():
    """Sharper turns (larger diff) imply a smaller radius; straight is infinite."""
    res = 2.0
    assert turn_radius_from_grid(0, res) == float("inf")
    r45 = turn_radius_from_grid(1, res)
    r90 = turn_radius_from_grid(2, res)
    assert r45 > r90 > 0
    print(f"[ok] turn radius: 45deg={r45:.2f}m, 90deg={r90:.2f}m")


def test_grade_hard_constraint_caps_max_grade():
    """The resolved max-grade is a hard constraint: the produced route must not exceed
    it (a small tolerance covers the spline smoothing post-process)."""
    limit = config.FOREST_ROAD_MAX_GRADE["work"]
    res = _solve({"grade_class": "work", "min_curve_radius_m": 8.0, "paved": False})
    assert res["polyline"], "경로가 생성되어야 합니다"
    assert res["metrics"]["max_grade_pct"] <= limit * 100 + 2.0, res["metrics"]
    print(f"[ok] grade hard-capped: max_grade={res['metrics']['max_grade_pct']}% "
          f"(limit {limit*100:.0f}%), length={res['metrics']['length_m']}m")


def test_grade_class_changes_feasibility():
    """Work roads (steeper limit) can cross terrain that trunk roads cannot, proving
    grade_class actually drives the grade limit."""
    pts = _points_from_dtm()
    work = _solve({"grade_class": "work", "min_curve_radius_m": 8.0, "paved": False}, pts)
    assert work["polyline"]
    try:
        _solve({"grade_class": "trunk", "min_curve_radius_m": 8.0, "paved": False}, pts)
        trunk_ok = True
    except ValueError:
        trunk_ok = False
    # On this steep sample, trunk (14%) should be the harder/failing case.
    print(f"[ok] grade_class drives limit: work=feasible, trunk_feasible={trunk_ok}")


def _count_turns(polyline):
    """Number of direction changes along a polyline (a straightness proxy)."""
    turns = 0
    for i in range(1, len(polyline) - 1):
        ax, ay = polyline[i][0] - polyline[i - 1][0], polyline[i][1] - polyline[i - 1][1]
        bx, by = polyline[i + 1][0] - polyline[i][0], polyline[i + 1][1] - polyline[i][1]
        # Cross product magnitude > tiny => heading changed.
        if abs(ax * by - ay * bx) > 1e-6:
            turns += 1
    return turns


def test_required_points_passed_within_tolerance():
    """BP/CP/EP must be passed within ROUTE_REQUIRED_POINT_TOLERANCE_M after grid
    snap + smoothing, and reported in required_point_checks (I-103)."""
    p = _valid_points_from_cost_surface(3)
    pts = {
        "bp": p[0],
        "ep": p[2],
        "cp": [{"order": 1, **p[1]}],
        "ap": [],
    }
    r = _solve({"grade_class": "work", "min_curve_radius_m": 8.0, "paved": False}, pts)
    checks = r["required_point_checks"]
    assert len(checks) == 3  # BP, CP1, EP
    labels = [c["point"] for c in checks]
    assert labels == ["BP", "CP1", "EP"], labels
    tol = config.ROUTE_REQUIRED_POINT_TOLERANCE_M
    # Required vertices are pinned to the EXACT user (x,y), so the polyline passes
    # essentially through each point — well within tolerance.
    worst = max(c["distance_m"] for c in checks)
    assert worst <= tol, [c["distance_m"] for c in checks]
    assert r["required_points_ok"] is True
    print(f"[ok] required-point checks: {[(c['point'], c['distance_m']) for c in checks]}, "
          f"all_ok={r['required_points_ok']}")


def test_circle_intrusion_detector():
    """The intrusion detector reports clearance + inside-length correctly (I-202)."""
    from utils.route_solver import _circle_intrusions
    # A straight polyline along x=0..100 at y=0; circle centred at (50,0) r=10.
    poly = [[float(x), 0.0, 0.0] for x in range(0, 101, 2)]
    res = _circle_intrusions(poly, [{"x": 50.0, "y": 0.0, "radius_m": 10.0}], lambda i: f"AP{i+1}")
    a = res[0]
    assert a["intrudes"] is True
    assert abs(a["min_clearance_m"]) < 1e-6           # path passes through the centre
    assert 18.0 <= a["intrusion_length_m"] <= 22.0    # ~2*radius inside the circle
    # A path far from the circle does not intrude.
    far = _circle_intrusions(poly, [{"x": 50.0, "y": 50.0, "radius_m": 10.0}], lambda i: f"AP{i+1}")
    assert far[0]["intrudes"] is False
    print(f"[ok] intrusion detector: clearance={a['min_clearance_m']}m, inside={a['intrusion_length_m']}m")


def test_ap_on_route_is_cleared():
    """Placing an AP on the baseline route pushes the route outside the AP circle
    (the strong soft-avoid keeps clearance >= radius) (I-202)."""
    p = _valid_points_from_cost_surface(2)
    bp, ep = p[0], p[1]
    opts = {"grade_class": "work", "min_curve_radius_m": 6.0, "paved": False}
    base = _solve(opts, {"bp": bp, "ep": ep, "cp": [], "ap": []})
    mid = base["polyline"][len(base["polyline"]) // 2]
    radius = 30.0
    pts = {"bp": bp, "ep": ep, "cp": [], "ap": [{"x": mid[0], "y": mid[1], "z": 0.0, "radius_m": radius}]}
    r = _solve(opts, pts)
    ai = r["avoid_intrusions"][0]
    # Route now clears the AP centre by at least ~the radius (it was on the path before).
    assert ai["min_clearance_m"] >= radius - 2.5, ai  # allow one grid cell of slack
    print(f"[ok] AP on route cleared: clearance={ai['min_clearance_m']}m (radius={radius}m), "
          f"intrudes={ai['intrudes']}")


def test_separate_uphill_downhill_limits():
    """max_uphill_grade is a hard constraint on z-increasing links only; the resulting
    route's max uphill grade must respect it, and conditions_snapshot records it (I-303)."""
    pts = _points_from_dtm()
    # Find a feasible uphill limit (terrain may need a fairly high one corner-to-corner).
    r, up_limit = None, None
    for cand in (0.15, 0.22, 0.30, 0.40):
        try:
            r = _solve({"grade_class": "work", "min_curve_radius_m": 6.0, "paved": False,
                        "max_uphill_grade": cand, "max_downhill_grade": 0.40}, pts)
            up_limit = cand
            break
        except ValueError:
            continue
    assert r is not None, "어떤 오르막 한계에서도 경로가 생성되지 않았습니다"
    # Uphill grade respects the chosen hard limit (small smoothing tolerance).
    assert r["metrics"]["max_uphill_pct"] <= up_limit * 100 + 2.5, (up_limit, r["metrics"])
    assert r["conditions_snapshot"]["max_uphill_grade_pct"] == round(up_limit * 100, 2)
    print(f"[ok] uphill/downhill split: max_uphill={r['metrics']['max_uphill_pct']}% "
          f"(limit {up_limit*100:.0f}%), max_downhill={r['metrics']['max_downhill_pct']}%")


def test_curve_warning_segments_merge():
    """Sub-radius samples merge into contiguous warning segments with chainage ranges
    and polyline indices; count of segments <= count of violations (I-301)."""
    pts = _points_from_dtm()
    # Large required radius on a jagged grid route -> guaranteed warnings.
    r = _solve({"grade_class": "work", "min_curve_radius_m": 50.0, "paved": False}, pts)
    segs = r["curve_warning_segments"]
    viol = r["metrics"]["curve_violations"]
    if viol == 0:
        print("[ok] curve warnings: none on this route (acceptable)")
        return
    assert len(segs) >= 1 and len(segs) <= viol
    for s in segs:
        assert s["chainage_end_m"] >= s["chainage_start_m"]
        assert s["min_radius_m"] < s["required_radius_m"]
        assert 0 <= s["polyline_start_index"] <= s["polyline_end_index"] < len(r["polyline"])
    print(f"[ok] curve warning segments: {len(segs)} merged from {viol} violations; "
          f"first={segs[0]['chainage_start_m']}-{segs[0]['chainage_end_m']}m "
          f"minR={segs[0]['min_radius_m']}m")


def test_fp_is_hard_constraint():
    """FP forbidden zones are a HARD constraint: the route never enters one, so the
    polyline keeps clearance >= radius and forbidden_intrusions is all clear (I-203)."""
    p = _valid_points_from_cost_surface(2)
    bp, ep = p[0], p[1]
    opts = {"grade_class": "work", "min_curve_radius_m": 6.0, "paved": False}
    base = _solve(opts, {"bp": bp, "ep": ep, "cp": [], "ap": []})
    mid = base["polyline"][len(base["polyline"]) // 2]
    radius = 30.0
    pts = {"bp": bp, "ep": ep, "cp": [], "ap": [],
           "fp": [{"x": mid[0], "y": mid[1], "z": 0.0, "radius_m": radius}]}
    r = _solve(opts, pts)
    fi = r["forbidden_intrusions"][0]
    assert fi["intrudes"] is False, fi              # hard constraint: never inside
    assert fi["intrusion_length_m"] == 0.0, fi
    assert fi["min_clearance_m"] >= radius - 2.5, fi  # outside the circle (cell slack)
    print(f"[ok] FP hard constraint: clearance={fi['min_clearance_m']}m (radius={radius}m), "
          f"intrudes={fi['intrudes']}")


def test_curve_radius_is_wired_and_steers_straighter():
    """min_curve_radius_m is consumed by the solver: a larger required radius must not
    produce a *more* twisty route than a small one (soft penalty; both stay feasible).
    2D length is NOT used here because a straighter route can be shorter."""
    pts = _points_from_dtm()
    gentle = _solve({"grade_class": "work", "min_curve_radius_m": 30.0, "paved": False}, pts)
    tight = _solve({"grade_class": "work", "min_curve_radius_m": 5.0, "paved": False}, pts)
    assert gentle["polyline"] and tight["polyline"]
    g_turns, t_turns = _count_turns(gentle["polyline"]), _count_turns(tight["polyline"])
    assert g_turns <= t_turns, f"gentle should be no twistier: {g_turns} vs {t_turns}"
    print(f"[ok] curve radius steers straighter: turns tight(5m)={t_turns}, "
          f"gentle(30m)={g_turns}")


if __name__ == "__main__":
    # Unit tests need no data.
    test_side_slope_axis()
    test_compute_gradients_axis_order()
    test_turn_radius_monotonic()

    if not DTM_PATH.exists():
        print("샘플 DTM 캐시가 없어 통합 테스트를 건너뜁니다.")
        sys.exit(0)

    test_cost_surface_is_reduced_grid()
    test_size_guard_blocks_oversized_grid()
    test_grade_hard_constraint_caps_max_grade()
    test_grade_class_changes_feasibility()
    test_required_points_passed_within_tolerance()
    test_circle_intrusion_detector()
    test_ap_on_route_is_cleared()
    test_fp_is_hard_constraint()
    test_separate_uphill_downhill_limits()
    test_curve_warning_segments_merge()
    test_curve_radius_is_wired_and_steers_straighter()
    print("\n[PASS] 모든 테스트 통과")