"""src/vkgs/src/drive.hpp (헤더 전용) 의 충돌·후진 로직을 g++ 로 컴파일해 시험한다. 창을 띄우지 않는다."""

import os
import shutil
import subprocess
from pathlib import Path

import numpy as np
import pytest

ROOT = Path(__file__).resolve().parents[1]
# 뷰어 빌드와 같은 컴파일러 (conda gcc 12, $CXX). 시스템 g++ 9 는 C++20 을 모른다
CXX = os.environ.get("CXX") or "g++"


@pytest.mark.skipif(shutil.which(CXX) is None, reason="g++ 없음")
def test_drive_collision_and_reverse(tmp_path):
    res, n, x0 = 0.2, 200, -20.0
    g = np.zeros((n, n), np.uint8)
    xs = (np.arange(n) + 0.5) * res + x0
    g[:, (xs >= 10) & (xs < 10.4)] = 255  # x = 10 m 의 벽
    g[np.ix_((xs > 0.3) & (xs < 0.7), (xs > 0.9) & (xs < 1.3))] = 255  # 출발 자세 차체 밑의 기둥
    g[np.ix_((xs > 5.0) & (xs < 5.2), (xs > -10.0) & (xs < -9.8))] = 255  # 외톨이 한 칸 (x -10, y 5) — 차체 옆을 스친다
    pgm = tmp_path / "obst.pgm"
    pgm.write_bytes(f"P5\n# nusim obstacles {x0} {x0} {res}\n{n} {n}\n255\n".encode() + g.tobytes())
    exe = tmp_path / "t"
    subprocess.run([CXX, "-std=c++20", "-O1", "-I", str(ROOT / "src/vkgs/src"),
                    str(ROOT / "tests/cpp/test_drive.cpp"), "-o", str(exe)], check=True)
    r = subprocess.run([str(exe), str(pgm)], capture_output=True, text=True)
    assert r.returncode == 0, r.stdout + r.stderr


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