"""Explicit opt-in: compiles Genesis kernels and needs an EGL-capable OpenGL stack."""
import json
import os
import subprocess
import sys

import numpy as np
from PIL import Image
import pytest
import yaml

from iiwa_genesis.config import ROOT


@pytest.mark.skipif(os.environ.get("IIWA_GENESIS_SMOKE") != "1", reason="set IIWA_GENESIS_SMOKE=1 to run real CPU/EGL tests")
@pytest.mark.parametrize("mode,names", [("without_gripper", ["overhead"]), ("with_gripper", ["overhead", "tool"])])
def test_cpu_rgbd_scene(tmp_path, mode, names):
    cfg = yaml.safe_load((ROOT / "configs/scene.yaml").read_text())
    cfg["scene"]["mode"] = mode
    cfg["output"]["directory"] = str(tmp_path / "frames")
    path = tmp_path / "scene.yaml"
    path.write_text(yaml.safe_dump(cfg))
    env = os.environ.copy()
    env.pop("PYTHONPATH", None)
    result = subprocess.run([sys.executable, str(ROOT / "run_scene.py"), "--config", str(path),
                             "--headless", "--backend", "cpu", "--steps", "4", "--save-frames"],
                            env=env, capture_output=True, text=True, timeout=600)
    assert result.returncode == 0, result.stdout + result.stderr
    captures = list((tmp_path / "frames").glob("*/*.npz"))
    assert len(captures) == len(names)
    for name in names:
        capture = next(p for p in captures if p.stem.startswith(name))
        data = np.load(capture)
        assert np.array(Image.open(capture.with_suffix(".png"))).shape == (480, 640, 3)
        assert data["depth"].shape == (480, 640)
        valid = data["valid"]
        assert np.isfinite(data["depth"]).all()
        assert (data["depth"][~valid] == 0).all()
        assert (data["depth"][valid] >= .01).all() and (data["depth"][valid] <= 6).all()
        meta = json.loads(capture.with_suffix(".json").read_text())
        np.testing.assert_allclose(np.array(meta["optical_from_world"]) @ meta["world_from_optical"], np.eye(4), atol=1e-6)
        if name == "overhead":
            assert valid.any()
        else:
            # The aperture must not sit inside the camera's 26 mm collision box.
            assert (data["depth"] > .05).any()