"""Audit filtered/projected reference for table and hand-object penetration."""
from pathlib import Path
import json
import numpy as np
import mujoco

ROOT = Path(__file__).resolve().parents[1]
O = ROOT / "output/desktop_smooth_fix_20260915"
z = np.load(O / "reference_video_rate.npz")
m = mujoco.MjModel.from_xml_path(str(O / "scene_act.xml")); d = mujoco.MjData(m)
floor = next(i for i in range(m.ngeom) if "_floor" in (m.geom(i).name or ""))
floor_z = float(m.geom_pos[floor][2])
result = {"frames": len(z["qpos"]), "floor_z_m": floor_z, "max_table_penetration_mm": 0.0,
          "max_hand_object_penetration_mm": 0.0, "frames_table_over_0_5mm": 0,
          "frames_hand_object_over_0_5mm": 0}
for row in z["qpos"]:
    d.qpos[:] = row; mujoco.mj_forward(m, d)
    floor_min = {"right": 1.0, "left": 1.0}
    for g in range(m.ngeom):
        n = m.geom(g).name or ""
        if not n.startswith(("right_", "left_")) or m.geom_dataid[g] < 0:
            continue
        side = n.split("_")[0]; mid = int(m.geom_dataid[g]); a = int(m.mesh_vertadr[mid]); b = a + int(m.mesh_vertnum[mid])
        v = m.mesh_vert[a:b: max(1, (b-a)//128)]
        world = v @ d.geom_xmat[g].reshape(3,3).T + d.geom_xpos[g]
        floor_min[side] = min(floor_min[side], float(world[:,2].min()-floor_z))
    tp = max(0.0, -min(floor_min.values())) * 1000
    hp = 0.0
    for c in d.contact:
        a,b = int(c.geom[0]), int(c.geom[1]); ta,tb = int(m.geom_contype[a]),int(m.geom_contype[b])
        if {ta,tb} == {1,2}: hp = max(hp, max(0.0,-float(c.dist))*1000)
    result["max_table_penetration_mm"] = max(result["max_table_penetration_mm"],tp); result["max_hand_object_penetration_mm"] = max(result["max_hand_object_penetration_mm"],hp)
    result["frames_table_over_0_5mm"] += tp > .5; result["frames_hand_object_over_0_5mm"] += hp > .5
(O/"smoothing_table_validation.json").write_text(json.dumps(result,indent=2)); print(json.dumps(result,indent=2))