"""Inspect every executed SPIDER state, using CPU collision detection independently."""
import os
os.environ['OPENBLAS_NUM_THREADS']='1';os.environ['OMP_NUM_THREADS']='1'
from pathlib import Path
import numpy as np,mujoco,json
from scipy.spatial.transform import Rotation as R
ROOT=Path(__file__).resolve().parents[1];O=Path(os.environ.get('SPIDER_TASK_OUT',str(ROOT/'output/collision_fix_20260915')));T=O/'datasets/processed/current/l20/bimanual/boxes';m=mujoco.MjModel.from_xml_path(str(T/'scene_act.xml'));d=mujoco.MjData(m);dr=mujoco.MjData(m);a=np.load(T/'0/trajectory_mjwp_act.npz');ref=np.load(T/'0/trajectory_kinematic_act.npz');qref=ref['qpos'];fp=np.load(ROOT/'output/foundationpose_spider_20260915/foundationpose_objects.npz');q=a['qpos'].reshape(-1,66);times=a['time'].ravel();assert np.isfinite(q).all();pen=[];counts=[];activecounts=[];errors=[[],[]];angles=[[],[]]
for f,row in enumerate(q):
 d.qpos[:]=row;mujoco.mj_fwdPosition(m,d);i=min(int(round(times[f]/.0025)),len(qref)-1);dr.qpos[:]=qref[i];mujoco.mj_kinematics(m,dr);cs=[c for c in d.contact if {int(m.geom_contype[c.geom[0]]),int(m.geom_contype[c.geom[1]])}=={1,2}];pen.append(max([max(0,-c.dist) for c in cs],default=0)*1000);counts.append(sum(c.dist<.001 for c in cs));activecounts.append(sum(c.dist<c.includemargin for c in cs))
 for k,s in enumerate(['right','left']):
  site=m.site(s+'_object_track').id;obj=m.body(s+'_object').id;errors[k].append(np.linalg.norm(d.site_xpos[site]-dr.site_xpos[site])*1000);angles[k].append(R.from_matrix(d.xmat[obj].reshape(3,3)@dr.xmat[obj].reshape(3,3).T).magnitude()*180/np.pi)
ids=np.flatnonzero(m.jnt_limited);v=q[:,m.jnt_qposadr[ids]];limits=float(max(np.maximum(m.jnt_range[ids,0]-v,0).max(),np.maximum(v-m.jnt_range[ids,1],0).max()));report={'steps':len(q),'duration_s':float(times[-1]),'all_finite':bool(np.isfinite(q).all()),'max_penetration_mm':float(max(pen)),'median_step_max_penetration_mm':float(np.median(pen)),'steps_over_1mm':int(np.sum(np.array(pen)>1)),'steps_with_hand_object_contact':int(np.sum(np.array(counts)>0)),'steps_with_active_hand_object_constraint':int(np.sum(np.array(activecounts)>0)),'contact_count_definition':'within 1 mm; active constraints separately include configured contact activation distance','max_joint_limit_violation_rad':limits,'zero_object_actuator_gains':bool(not m.actuator_gainprm[-12:].any() and not m.actuator_biasprm[-12:].any())}
videoindex=np.minimum(np.round(times*30).astype(int),351)
for k,n in enumerate(['upper','lower']):
 mask=fp[n+'_valid'][videoindex];ee=np.array(errors[k]);aa=np.array(angles[k]);report[n]={'observed_centroid_error_mean_mm':float(ee[mask].mean()),'observed_centroid_error_max_mm':float(ee[mask].max()),'observed_rotation_error_mean_deg':float(aa[mask].mean())}
report['grasp_tracking_20mm_passed']=all(report[n]['observed_centroid_error_max_mm']<20 for n in ['upper','lower']);(O/'physics_validation.json').write_text(json.dumps(report,indent=2));np.savez_compressed(O/'physics_motion.npz',qpos=q,ctrl=a['ctrl'].reshape(-1,56),time=times,max_penetration_mm=pen,contact_counts=counts,active_contact_counts=activecounts,object_centroid_error_mm=np.array(errors).T,object_rotation_error_deg=np.array(angles).T);print(json.dumps(report,indent=2))