"""Audit saved SPIDER states; render source, kinematic reference, and physical rollout."""
import os
os.environ.setdefault('MUJOCO_GL','osmesa')
from pathlib import Path
import json,re,sys
import numpy as np,mujoco,cv2,imageio.v2 as imageio
from scipy.spatial.transform import Rotation as R
ROOT=Path(__file__).resolve().parents[1];OUT=ROOT/'output/foundationpose_spider_20260915';TASK=OUT/'datasets/processed/current/l20/bimanual/boxes';TRIAL=TASK/'0'
r=np.load(TRIAL/'trajectory_kinematic_act.npz');a=np.load(TRIAL/'trajectory_mjwp_act.npz');q=a['qpos'].reshape(-1,66);ctrl=a['ctrl'].reshape(-1,56);times=a['time'].ravel();ref=np.load(OUT/'reference_video_rate.npz');fp=np.load(OUT/'foundationpose_objects.npz')
assert np.isfinite(q).all(),'Nonfinite SPIDER result; cannot render as valid motion'
m=mujoco.MjModel.from_xml_path(str(TASK/'scene_act.xml'));d=mujoco.MjData(m);dr=mujoco.MjData(m);assert not m.actuator_gainprm[-12:].any() and not m.actuator_biasprm[-12:].any()
# Evaluate at video times, preserving actual step timestamps rather than nominal array indices.
ix=np.argmin(abs(times[:,None]-ref['time'][None,:]),axis=0);qv=q[ix];bv=np.load(OUT/'reference_before_contact_fit.npz')['qpos'];sid=[m.site(f'{s}_hand_{f}_track').id for s in ['right','left'] for f in ['thumb','index','middle','ring','pinky']]
def metrics(rows):
 pen=[];contacts=[];gaps=[];err=[[],[]];centererr=[[],[]];ang=[[],[]];lim=[];mim=[]
 for f,row in enumerate(rows):
  d.qpos[:]=row;mujoco.mj_forward(m,d);dr.qpos[:]=ref['qpos'][f];mujoco.mj_kinematics(m,dr)
  selected=[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 selected],default=0)*1000);contacts.append(len(selected))
  mask=ref['contact'][f].astype(bool);ds=np.linalg.norm(d.site_xpos[sid]-ref['contact_pos'][f],axis=1);gaps.extend(ds[mask]*1000)
  for k,s in enumerate(['right','left']):
   oid=m.body(s+'_object').id;cs=m.site(s+'_object_track').id;centererr[k].append(np.linalg.norm(d.site_xpos[cs]-dr.site_xpos[cs])*1000);err[k].append(np.linalg.norm(d.xpos[oid]-dr.xpos[oid])*1000);ang[k].append(R.from_matrix(d.xmat[oid].reshape(3,3)@dr.xmat[oid].reshape(3,3).T).magnitude()*180/np.pi)
  inds=np.flatnonzero(m.jnt_limited);v=row[m.jnt_qposadr[inds]];lim.append(max(np.maximum(m.jnt_range[inds,0]-v,0).max(),np.maximum(v-m.jnt_range[inds,1],0).max()))
  for j in range(m.neq):
   j1,j2=m.eq_obj1id[j],m.eq_obj2id[j];coef=m.eq_data[j,:5];expected=sum(coef[k]*row[m.jnt_qposadr[j2]]**k for k in range(5));mim.append(abs(row[m.jnt_qposadr[j1]]-expected))
 result=dict(finite=bool(np.isfinite(rows).all()),sampled_video_frames=len(rows),hand_object_contact_frames=int(np.sum(np.array(contacts)>0)),max_hand_object_penetration_mm=float(np.max(pen)),median_frame_max_penetration_mm=float(np.median(pen)),inferred_contact_target_gap_mean_mm=float(np.mean(gaps)) if gaps else None,max_joint_limit_violation_rad=float(max(lim)),max_mimic_residual_rad=float(max(mim)))
 for k,name in enumerate(['upper','lower']):
  mask=fp[name+'_valid'];e=np.array(err[k]);aa=np.array(ang[k]);result[name]=dict(observed_frames=int(mask.sum()),observed_position_error_mean_mm=float(e[mask].mean()),observed_position_error_max_mm=float(e[mask].max()),observed_rotation_error_mean_deg=float(aa[mask].mean()),all_frames_position_error_mean_mm=float(e.mean()),observed_centroid_error_mean_mm=float(np.array(centererr[k])[mask].mean()),observed_centroid_error_max_mm=float(np.array(centererr[k])[mask].max()))
 return result,np.array(pen),np.array(contacts)
report={};report['original_hands_new_objects'],_,_=metrics(bv);report['kinematic_contact_warmstart'],_,_=metrics(ref['qpos']);report['spider'],pen,counts=metrics(qv)
if (OUT/'independent_gpu_replay.npz').exists():
 replay=np.load(OUT/'independent_gpu_replay.npz')
 for key in replay.files:
  rows=replay[key][ix]
  report['independent_gpu_'+key]=metrics(rows)[0] if np.isfinite(rows).all() else {'finite':False}
 report['independent_replay_integrity']=json.loads((OUT/'independent_gpu_replay.json').read_text())
report.update(full_steps=len(q),duration_s=float(times[-1]),zero_object_assistance=True,validity_note='Video-depth consistency is not external pose ground truth; lower reference after176 is held and excluded as a new target; either hand may contact red.',collision_note='Penetration measured using approximate convex-decomposed collision meshes, not exact CAD signed distance.',contact_note='Masks are geometric hypotheses, not measured contact labels.',all_states_finite=bool(np.isfinite(q).all()))
log=(OUT/'spider_full.log').read_text();report['completion_marker']='SPIDER_RUN_COMPLETE' in log;report['nonfinite_candidate_warning_count']=len(re.findall('NaNs or infs in rews',log))
report['tracking_20mm_gate_passed']=all(report['spider'][n]['observed_position_error_max_mm']<20 for n in ['upper','lower'])
(OUT/'validation.json').write_text(json.dumps(report,indent=2));np.savez_compressed(OUT/'comparison_motion.npz',original=bv,reference=ref['qpos'],spider=qv,time=ref['time'],spider_full_qpos=q,spider_full_ctrl=ctrl,spider_full_time=times)
np.savetxt(OUT/'spider_trajectory.csv',np.c_[times,q,ctrl],delimiter=',',header=','.join(['time_s']+[m.joint(i).name for i in range(m.njnt)]+['ctrl_'+m.actuator(i).name for i in range(m.nu)]),comments='')
if '--skip-render' in sys.argv:
 print(json.dumps(report,indent=2));sys.exit(0)
m.vis.headlight.ambient[:]=.7;m.vis.quality.offsamples=1;renderer=mujoco.Renderer(m,height=360,width=640);opt=mujoco.MjvOption();opt.sitegroup[:]=0;opt.geomgroup[3:]=0;cid=m.camera('source_camera').id
cap=cv2.VideoCapture(str(ROOT/'docs/20260915_171525/color.mp4'));writer=imageio.get_writer(OUT/'original_foundationpose_spider.mp4',fps=30,codec='libx264',quality=8,macro_block_size=1)
for f in range(len(qv)):
 ok,img=cap.read();assert ok;panels=[cv2.cvtColor(cv2.resize(img,(640,360)),cv2.COLOR_BGR2RGB)]
 cam=ref['camera_world_from_cv'][f];m.cam_pos[cid]=cam[:3,3];quat=R.from_matrix(cam[:3,:3]@np.diag([1,-1,-1])).as_quat();m.cam_quat[cid]=quat[[3,0,1,2]]
 for row in [ref['qpos'][f],qv[f]]:
  d.qpos[:]=row;mujoco.mj_forward(m,d);renderer.update_scene(d,camera='source_camera',scene_option=opt);panels.append(renderer.render().copy())
 panel=np.concatenate(panels,1);cv2.rectangle(panel,(0,0),(1920,45),(20,25,30),-1)
 for j,label in enumerate(['Recorded RGB-D','FoundationPose + contact IK reference','Actual SPIDER: free objects, zero assistance']):cv2.putText(panel,label,(j*640+8,18),cv2.FONT_HERSHEY_SIMPLEX,.47,(255,255,255),1)
 label=f'frame {f}/351 | red observed={bool(fp["upper_valid"][f])}, blue observed={bool(fp["lower_valid"][f])} | SPIDER penetration={pen[f]:.1f}mm, contacts={counts[f]}'
 cv2.putText(panel,label,(8,38),cv2.FONT_HERSHEY_SIMPLEX,.45,(255,255,255),1);writer.append_data(panel)
 if f in [0,80,160,240,351]:cv2.imwrite(str(OUT/f'comparison_{f:04d}.jpg'),cv2.cvtColor(panel,cv2.COLOR_RGB2BGR))
writer.close();renderer.close();cap.release();print(json.dumps(report,indent=2),flush=True)