"""src/vkgs/src/sensors.hpp (레이캐스트 라이다·레이더) 를 g++ 로 컴파일해 시험한다. 창을 띄우지 않는다.
복셀 월드는 nusim.sim.voxels 로 만든다 — 파일 형식을 Python·C++ 양쪽이 같게 읽는지도 같이 본다."""

import os
import shutil
import subprocess
from pathlib import Path

import numpy as np
import pytest

from nusim.sim.voxels import read_voxels, write_grid

ROOT = Path(__file__).resolve().parents[1]
CXX = os.environ.get("CXX") or "g++"


def _world(path: Path):
    g = np.mgrid[-30:30:0.1, -30:30:0.1].reshape(2, -1).T
    ground = np.c_[g, np.full(len(g), 0.1)]  # 지면 칸 z 0 ~ 0.2
    yz = np.mgrid[-30:30:0.1, 0:6:0.1].reshape(2, -1).T
    wall = np.c_[np.full(len(yz), 20.1), yz]  # x = 20 ~ 20.2
    pts = np.vstack([ground, ground, wall, wall])  # 칸마다 2점 이상
    return write_grid(path, pts, np.array([-30.0, -30.0, -2.0]), np.array([30.0, 30.0, 10.0]))


def test_voxels_roundtrip(tmp_path):
    _world(tmp_path / "w.vox")
    occ, lo, res = read_voxels(tmp_path / "w.vox")
    assert not occ[int((5 - lo[0]) / res), int((0 - lo[1]) / res), int((0.1 - lo[2]) / res)]  # 지면은 높이 지도가 맡는다 (복셀 아님)
    assert occ[int((20.1 - lo[0]) / res), int((0 - lo[1]) / res), int((3 - lo[2]) / res)]  # 벽
    assert not occ[int((5 - lo[0]) / res), int((0 - lo[1]) / res), int((2 - lo[2]) / res)]  # 빈 공간


@pytest.mark.skipif(shutil.which(CXX) is None, reason="g++ 없음")
def test_sensors(tmp_path):
    _world(tmp_path / "w.vox")
    exe = tmp_path / "t"
    subprocess.run([CXX, "-std=c++20", "-O2", "-pthread", "-I", str(ROOT / "src/vkgs/src"),
                    str(ROOT / "tests/cpp/test_sensors.cpp"), "-o", str(exe)], check=True)
    r = subprocess.run([str(exe), str(tmp_path / "w.vox")], capture_output=True, text=True)
    assert r.returncode == 0, r.stdout + r.stderr


@pytest.mark.skipif(not (ROOT / "build" / "vkgs_voxcarve").exists(), reason="build/vkgs_voxcarve 없음 (cmake --build build)")
def test_free_space_carving(tmp_path):
    """빈 공간 깎기: 한 sweep 에만 찍힌 '지나간 차 조각' 은 다른 sweep 의 빔이 그 자리를 지나가므로 지워지고,
    매 sweep 찍히는 벽은 남는다 (nusim.sim.voxels.carve, src/vkgs/src/voxcarve.cpp)."""
    rng = np.random.default_rng(0)
    org, ends, sweep = [], [], []
    for s in range(20):  # 자차가 x 0 → 19 m 로 가며 20 sweep
        o = np.array([float(s), 0.0, 1.8])
        org.append(o)
        # 벽 (x = 30, 높이 0.5~3 m): 매 sweep
        wall = np.c_[np.full(400, 30.1), rng.uniform(-3, 3, 400), rng.uniform(0.5, 3, 400)]
        pts = [wall]
        if s == 3:  # 지나간 차 조각 (x 15, y 0, 높이 0.5 m) — 한 sweep 에만
            pts.append(np.c_[rng.normal(15, 0.03, 50), rng.normal(0, 0.03, 50), rng.normal(0.5, 0.03, 50)])
        p = np.vstack(pts)
        ends.append(p)
        sweep.append(np.full(len(p), s, np.int32))
    pts = np.vstack(ends)
    lo, hi = np.array([-5.0, -10.0, -2.0]), np.array([40.0, 10.0, 8.0])
    write_grid(tmp_path / "w.vox", pts, lo, hi, rays=(np.array(org), np.concatenate(sweep)))
    occ, lo2, res = read_voxels(tmp_path / "w.vox")
    at = lambda x, y, z: occ[int((x - lo2[0]) / res), int((y - lo2[1]) / res), int((z - lo2[2]) / res)]
    assert not at(15.0, 0.0, 0.5)  # 깎였다
    assert at(30.1, 0.0, 1.5)  # 벽은 남는다