"""Independent full-rate collision audit and exact closed-red-mesh landmark probes."""
import os
os.environ.setdefault('MUJOCO_GL','osmesa')
from pathlib import Path
import json,numpy as np,mujoco,trimesh,open3d as o3d
from red_box_distance import signed_distance
ROOT=Path(__file__).resolve().parents[1];O=ROOT/'output/foundationpose_spider_20260915';T=O/'datasets/processed/current/l20/bimanual/boxes'
m=mujoco.MjModel.from_xml_path(str(T/'scene_act.xml'));d=mujoco.MjData(m);r=np.load(O/'comparison_motion.npz');q=r['spider_full_qpos'];pens=[];counts=[]
for row in q:
 d.qpos[:]=row;mujoco.mj_fwdPosition(m,d)
 selected=[c for c in d.contact if {int(m.geom_contype[c.geom[0]]),int(m.geom_contype[c.geom[1]])}=={1,2}]
 pens.append(max([max(0.,-c.dist) for c in selected],default=0)*1000);counts.append(len(selected))
mesh=trimesh.load(ROOT/'output/depth_ablation_red_20260916/red_box_closed.ply');assert mesh.is_watertight
sc=o3d.t.geometry.RaycastingScene();sc.add_triangles(o3d.t.geometry.TriangleMesh.from_legacy(o3d.geometry.TriangleMesh(o3d.utility.Vector3dVector(mesh.vertices),o3d.utility.Vector3iVector(mesh.faces))))
ids=[i for i in range(m.nsite) if 'landmark_' in m.site(i).name or '_hand_' in m.site(i).name];assert len(ids)==42
obj=m.body('right_object').id;report={}
for key in ['original','reference','spider']:
 points=[]
 for row in r[key]:
  d.qpos[:]=row;mujoco.mj_kinematics(m,d);points.append((d.site_xpos[ids]-d.xpos[obj])@d.xmat[obj].reshape(3,3))
 dist=signed_distance(sc,mesh,np.array(points).reshape(-1,3)).reshape(-1,len(ids));report[key]={'frames_with_landmark_inside_red_over_1mm':int((dist.min(1)<-.001).sum()),'max_red_landmark_inside_depth_mm':float(max(0.,-dist.min())*1000)}
report['full_rate_spider_collision']={'steps':len(q),'max_hand_object_penetration_mm':float(max(pens)),'median_step_max_penetration_mm':float(np.median(pens)),'hand_object_contact_steps':int((np.array(counts)>0).sum())}
report['scope']='42 kinematic landmarks are point probes, not complete hand mesh intersection. Exact red sign uses winding on topology-repaired watertight CAD; full-rate contacts use CoACD approximation.'
(O/'geometry_audit.json').write_text(json.dumps(report,indent=2));np.savez_compressed(O/'full_rate_contacts.npz',time=r['spider_full_time'],max_penetration_mm=pens,contact_counts=counts);print(json.dumps(report,indent=2))