"""closed-loop 서버 프로토콜(src/vkgs/src/serve.hpp)과 drive.hpp 제어 환산을 g++ 로 컴파일해 시험한다 (창·GPU 없이).
Python 쪽 판정(nusim.sim.geometry)이 C++ vehicle_contact 와 같은 점을 세는지도 대조한다."""

import os
import shutil
import subprocess
from pathlib import Path

import numpy as np
import pytest

from nusim.sim.closed_loop import format_command, parse_reply
from nusim.sim.geometry import ContactParams, VehicleGeom, World

ROOT = Path(__file__).resolve().parents[1]
CXX = os.environ.get("CXX") or "g++"
needs_cxx = pytest.mark.skipif(shutil.which(CXX) is None, reason="g++ 없음")


def _build(tmp_path):
    exe = tmp_path / "test_serve"
    subprocess.run([CXX, "-std=c++20", "-O1", "-I", str(ROOT / "src/vkgs/src"), str(ROOT / "tests/cpp/test_serve.cpp"),
                    "-o", str(exe)], check=True)
    return exe


@needs_cxx
def test_serve_protocol_and_control(tmp_path):
    r = subprocess.run([str(_build(tmp_path)), "self"], capture_output=True, text=True)
    assert r.returncode == 0, r.stdout + r.stderr


def test_python_protocol_roundtrip():
    st = parse_reply("ok t=0.500000 x=-9.5 obs=- cams=CAM_FRONT,CAM_BACK contact=1\n")
    assert st == {"t": 0.5, "x": -9.5, "obs": "", "cams": "CAM_FRONT,CAM_BACK", "contact": 1.0}
    with pytest.raises(RuntimeError, match="서버 오류"):
        parse_reply("err step: dt= 가 필요함")
    with pytest.raises(RuntimeError):
        parse_reply("")
    assert format_command("step", accel=1.25, delta=-0.1, dt=0.05, render=True, sensors=None) == \
        "step accel=1.25 delta=-0.1 dt=0.05 render=1"
    with pytest.raises(ValueError):
        format_command("actors", boxes="1 2")


@needs_cxx
def test_python_contact_matches_cpp(tmp_path):
    """같은 지도·자세에서 Python World 와 C++ vehicle_contact 가 같은 수의 점을 센다."""
    res, n, x0 = 0.2, 150, -15.0
    rng = np.random.default_rng(0)
    g = (rng.random((n, n)) < 0.03).astype(np.uint8) * 255  # 흩어진 칸
    g[60:64, :] = 255  # y ≈ -3 m 의 벽
    pgm = tmp_path / "obst.pgm"
    pgm.write_bytes(f"P5\n# nusim obstacles {x0} {x0} {res}\n{n} {n}\n255\n".encode() + g.tobytes())
    driv = tmp_path / "drivable.txt"
    # 바깥 사각형 + 가운데 구멍
    driv.write_text("# t\nP 2 4 -10 -10 10 -10 10 10 -10 10 4 2 2 4 2 4 4 2 4\n")
    poses = np.c_[rng.uniform(-9, 9, 60), rng.uniform(-9, 9, 60), rng.uniform(-np.pi, np.pi, 60)]
    r = subprocess.run([str(_build(tmp_path)), "contact", str(pgm), str(driv)], input="\n".join(
        f"{x:.4f} {y:.4f} {yaw:.4f}" for x, y, yaw in poses), capture_output=True, text=True, check=True)
    cpp = np.array([list(map(int, l.split())) for l in r.stdout.split("\n") if l.strip()])
    w = World.load(VehicleGeom(), ContactParams(curb=True, curb_tol=0.15, obst_tol=0.1, graze=2), pgm, driv)
    poses32 = poses.astype(np.float32).astype(np.float64)  # C++ 는 float 로 읽는다
    py_obst = [w.hard_contacts(*p) for p in poses32]
    py_road = [w.on_road(*p) for p in poses32]
    assert np.abs(cpp[:, 0] - np.array(py_obst)).max() <= 1, (cpp[:, 0], py_obst)  # float 경계 1점까지
    assert (cpp[:, 0] > 0).sum() > 5  # 실제로 닿는 자세가 섞여 있어야 시험이 된다
    assert ((cpp[:, 1] == 0) == np.array(py_road)).all()