import numpy as np

from nusim.sim.obstacles import RES, obstacle_grid


def _cell(grid, x0, y0, x, y):
    return grid[int((y - y0) / RES), int((x - x0) / RES)]


def test_wall_blocks_but_curb_and_canopy_do_not():
    rng = np.random.default_rng(0)
    ground = np.c_[rng.uniform(-20, 20, (40000, 2)), rng.normal(0.0, 0.01, 40000)]
    # 벽: x = 5 m, 높이 0~3 m
    wall = np.c_[np.full(3000, 5.05), rng.uniform(-3, 3, 3000), rng.uniform(0, 3, 3000)]
    # 연석: x = -5 m, 높이 0.15 m — 차체에 걸리지 않는다 (주행가능영역 경계가 따로 맡는다)
    curb = np.c_[np.full(3000, -5.05), rng.uniform(-3, 3, 3000), np.full(3000, 0.15)]
    # 가로수 수관: 높이 4 m — 차가 밑으로 지나간다
    canopy = np.c_[rng.uniform(-1, 1, (3000, 2)) + [0, 10], np.full(3000, 4.0)]
    # 외톨이 잡음 점 무더기 한 칸
    noise = np.tile([[12.05, 12.05, 1.0]], (10, 1))
    grid, x0, y0 = obstacle_grid(np.vstack([ground, wall, curb, canopy, noise]), np.zeros(2), 20.0)
    assert _cell(grid, x0, y0, 5.05, 0.0) == 255
    assert _cell(grid, x0, y0, -5.05, 0.0) == 0
    assert _cell(grid, x0, y0, 0.0, 10.0) == 0
    assert _cell(grid, x0, y0, 12.05, 12.05) == 0  # 이웃 없는 칸은 지운다
    assert _cell(grid, x0, y0, 0.0, 0.0) == 0


def test_ground_follows_slope():
    """비탈(경사 10%)에서도 노면이 장애물이 되지 않는다 — 지면 높이를 칸마다 잰다."""
    rng = np.random.default_rng(1)
    xy = rng.uniform(-20, 20, (60000, 2))
    pts = np.c_[xy, 0.1 * xy[:, 0]]
    grid, _, _ = obstacle_grid(pts, np.zeros(2), 20.0)
    assert grid.sum() == 0


def test_smeared_road_and_canopy_not_obstacle():
    """긴 씬: 먼 sweep 의 노면 점이 아래로 번지고(지면이 낮게 잡히던 문제), 노면 위로 가로수 수관이 드리운다
    (가장 높은 점 기준이면 '솟은 구조물' 로 남던 문제) — scene-0402, 2026-10-09."""
    from nusim.sim.obstacles import TALL_BAND

    rng = np.random.default_rng(0)
    n = 200_000
    xy = rng.uniform(-10, 10, (n, 2))
    z = np.where(rng.random(n) < 0.7, rng.normal(-0.2, 0.01, n), rng.uniform(-0.65, -0.2, n))  # 노면 + 아래 번짐 30%
    road = np.c_[xy, z]
    canopy = np.c_[rng.uniform(-5, 5, (20_000, 2)), rng.uniform(6, 15, 20_000)]
    pole = np.c_[rng.normal(7, 0.03, (3000, 2)), rng.uniform(-0.2, 6, 3000)]
    grid, x0, y0, tall = obstacle_grid(np.vstack([road, canopy, pole]), np.zeros(2), 12.0, with_top=True)
    ix = lambda v: int((v - x0) / 0.2)
    assert grid[ix(-8):ix(5), ix(-8):ix(5)].sum() == 0  # 노면은 비어 있다
    assert not tall[ix(-4):ix(4), ix(-4):ix(4)].any()  # 수관 아래는 '솟은 구조물' 이 아니다
    assert grid[ix(7), ix(7)] and tall[ix(7), ix(7)]  # 기둥은 장애물이고 노면에서도 남는다
    assert TALL_BAND[1] <= 3.0