"""Opt-in contact regression using real exported meshes and the real table."""
import os
import subprocess
import sys

import pytest

from iiwa_genesis.config import ROOT


@pytest.mark.skipif(os.environ.get("IIWA_GENESIS_SMOKE") != "1",
                    reason="set IIWA_GENESIS_SMOKE=1 for real tool/table contact tests")
def test_all_hammers_fall_rest_and_move_on_table():
    script = r'''
import sys
import numpy as np
sys.path.insert(0, "src")
from iiwa_genesis.config import load_config
from iiwa_genesis.runtime import import_genesis
from iiwa_genesis.tools import add_table_tool

gs = import_genesis()
gs.init(backend=gs.cpu, seed=42, logging_level="warning")
try:
    for number in range(1, 6):
        cfg = load_config("configs/scene.yaml")
        cfg["tools"] = {"allowed": [f"hammer{number}"], "seed": 10}
        scene = gs.Scene(show_viewer=False, show_FPS=False,
                         sim_options=gs.options.SimOptions(dt=1/120, gravity=(0, 0, -9.81)))
        scene.add_entity(gs.morphs.URDF(file=str(cfg["table"]["urdf"]), fixed=True,
                                       merge_fixed_links=False, decimate=False))
        tool = add_table_tool(gs, scene, cfg)
        scene.build()
        assert len(scene.entities) == 2  # one table and exactly one tool
        assert tool.n_dofs == 6 and tool.n_links == 1 and tool.n_geoms > 1
        assert float(tool.get_mass()) > 0
        initial = tool.get_AABB().cpu().numpy()
        np.testing.assert_allclose(initial[0, 2], .805, atol=.002)
        assert initial[1, 2] < .87
        for _ in range(120):
            scene.step()
        rest = tool.get_AABB().cpu().numpy()
        assert np.isfinite(rest).all()
        assert .797 < rest[0, 2] < .803, (number, initial, rest)
        assert rest[0, 2] < initial[0, 2] - .001
        assert np.linalg.norm(tool.get_vel().cpu().numpy()) < .03
        # An imparted horizontal velocity moves the free rigid assembly; it
        # stays supported by the tabletop and friction slows it down.
        before = tool.get_pos().cpu().numpy().copy()
        tool.set_dofs_velocity([.3, 0, 0, 0, 0, 0])
        for _ in range(60):
            scene.step()
        after = tool.get_pos().cpu().numpy()
        assert np.linalg.norm(after[:2] - before[:2]) > .001, (number, before, after)
        assert .797 < float(tool.get_AABB()[0, 2]) < .803
        print(f"hammer{number}: contact, rest and dynamic movement OK", flush=True)
        scene.destroy()
finally:
    gs.destroy()
'''
    env = os.environ.copy()
    env.pop("PYTHONPATH", None)
    env.setdefault("PYOPENGL_PLATFORM", "egl")
    result = subprocess.run([sys.executable, "-c", script], cwd=ROOT,
                            env=env, capture_output=True, text=True, timeout=900)
    assert result.returncode == 0, result.stdout + result.stderr