"""Independently check both URDF constraints and export common-world trajectories."""
from pathlib import Path
import json,xml.etree.ElementTree as E,argparse
import cv2,mujoco,numpy as np
from scipy.spatial.transform import Rotation
ROOT=Path(__file__).resolve().parents[1]
parser=argparse.ArgumentParser();parser.add_argument('--base',type=Path,default=ROOT/'output/20260915_171525');args=parser.parse_args()
BASE=args.base.resolve();OUT=BASE/'replay'
motion=np.load(OUT/'motion.npz');q=motion['qpos'];model=mujoco.MjModel.from_xml_path(str(OUT/'scene.xml'));data=mujoco.MjData(model)
bundle=dict(time=motion['time'],fps=motion['fps'],camera_world_from_cv=motion['camera_world_from_cv'])
reports={};samples={side:[] for side in ['right','left']}
for side in ['right','left']:
    prefix='' if side=='right' else 'left_';hand=np.load(BASE/f'l20_{side}_stable/motion.npz')
    names=hand['joint_names'].tolist();addrs=[model.jnt_qposadr[model.joint(prefix+n).id] for n in names]
    angles=q[:,addrs];assert np.allclose(angles,hand['qpos'],atol=1e-12)
    root=E.parse(ROOT/f'third_party/l20_assets/L20/{side.upper()}/linkerhand_g20_{side}.urdf').getroot()
    violation=0.;mimic=0.;fk=0.;positions=[];quats=[]
    Rshared=np.load(BASE/'l20_right_stable/motion.npz')['scene_rotation'];tshared=np.load(BASE/'l20_right_stable/motion.npz')['scene_translation']
    for j in root.findall('joint'):
        if j.get('type')=='fixed':continue
        col=angles[:,names.index(j.get('name'))];lim=j.find('limit')
        violation=max(violation,float(np.maximum(float(lim.get('lower'))-col,0).max()),float(np.maximum(col-float(lim.get('upper')),0).max()))
        mi=j.find('mimic')
        if mi is not None:
            mimic=max(mimic,float(np.abs(col-float(mi.get('multiplier','1'))*angles[:,names.index(mi.get('joint'))]-float(mi.get('offset','0'))).max()))
    for i,row in enumerate(q):
        data.qpos[:]=row;mujoco.mj_forward(model,data)
        pos=data.xpos[model.body(prefix+'hand_base_link').id].copy();quat=data.xquat[model.body(prefix+'hand_base_link').id].copy()
        positions.append(pos);quats.append(quat)
        localR=Rotation.from_quat(hand['wrist_quat_wxyz'][i,[1,2,3,0]]).as_matrix()
        expectedR=Rshared@hand['scene_rotation'].T@localR
        expectedp=Rshared@hand['scene_rotation'].T@(hand['wrist_pos'][i]-hand['scene_translation'])+tshared
        expected=hand['actual'][i]@expectedR.T+expectedp
        actual=np.array([data.site_xpos[model.site(prefix+f'landmark_{k:02d}').id] for k in range(21)])
        fk=max(fk,float(np.abs(actual-expected).max()))
    positions=np.asarray(positions);quats=np.asarray(quats)
    for i in range(1,len(quats)):
        if quats[i]@quats[i-1]<0:quats[i]*=-1
    bundle.update({side+'_joint_names':np.asarray(names),side+'_joint_position':angles,
                   side+'_wrist_position':positions,side+'_wrist_quaternion_wxyz':quats,
                   side+'_valid':hand['detection_valid']})
    np.savetxt(OUT/f'{side}_hand_trajectory.csv',np.c_[motion['time'],positions,quats,angles],delimiter=',',
               header=','.join(['time_s','wrist_x','wrist_y','wrist_z','qw','qx','qy','qz']+names),comments='')
    assert violation<1e-8 and mimic<1e-10 and fk<1e-5
    reports[side]=dict(frames=len(q),valid_frames=int(hand['detection_valid'].sum()),urdf_limit_violation_rad=violation,mimic_error_rad=mimic,independent_world_fk_max_error_m=fk)
for name in ['upper','lower']:
    addr=model.jnt_qposadr[model.joint(name+'_free').id]
    bundle[name+'_position']=q[:,addr:addr+3];bundle[name+'_quaternion_wxyz']=q[:,addr+3:addr+7]
    fit=np.load(BASE/'object_poses.npz')
    observed=np.zeros(len(q),dtype=bool)
    observed[fit[name+'_keyframes'][fit[name+'_keyframe_valid']]]=True
    bundle[name+'_accepted_fit_keyframe']=observed
    bundle[name+'_interpolated_or_held']=~observed
    for suffix in ['assembly_assumed','occluded_transition_assumed']:
        key=name+'_'+suffix
        if key in fit:bundle[key]=fit[key]
bundle['world_convention']='Common right-hand replay scene transform applied to first-camera RGBD odometry world; display axes, gravity not measured.'
bundle['valid_semantics']='Per-hand source validity; see human_joints source metadata. Object poses are partial-surface estimates; not contact labels.'
assert np.isfinite(q).all() and np.all(np.diff(motion['time'])>0)
np.savez_compressed(OUT/'demonstrations_bimanual.npz',**bundle)
cap=cv2.VideoCapture(str(OUT/'original_vs_l20_bimanual.mp4'));n=0
while True:
    ok,im=cap.read()
    if not ok:break
    assert im.shape==(360,1280,3);n+=1
cap.release();assert n==len(q)
reports.update(video_decoded_frames=n,time_last_s=float(motion['time'][-1]),finite=True,
               scope='Dual-hand kinematic retargeting with separate RGBD CAD pose estimates; no contact or dynamics success validation.')
(OUT/'validation.json').write_text(json.dumps(reports,indent=2));print(json.dumps(reports,indent=2))