"""Explicit offline smoothing ablation, preserving calibration and moving wrist."""
import json,shutil
import numpy as np
import mujoco
from scipy.ndimage import gaussian_filter1d
from scipy.spatial.transform import Rotation
from l20_calibrated import ROOT,Calibration
base=ROOT/'output/l20_2047635068/jitter_audit';out=base/'temporal';out.mkdir(exist_ok=True)
m=dict(np.load(base/'corrected/motion.npz'));cal=Calibration();names=list(m['joint_names'])
sigma=2.5
a=gaussian_filter1d(m['active_qpos'],sigma,axis=0)
q=[]
for row in a:
    d=dict(zip(cal.active,row))
    for n in cal.passive:d[n]=cal.passive_value(n,d[cal.curves[n][0]])[0]
    q.append([d[n] for n in names])
m['qpos']=np.array(q);m['active_qpos']=a
m['wrist_pos_unsmoothed']=m['wrist_pos'].copy();m['wrist_quat_unsmoothed_wxyz']=m['wrist_quat_wxyz'].copy()
m['wrist_pos']=gaussian_filter1d(m['wrist_pos'],sigma,axis=0)
rot=Rotation.from_quat(m['wrist_quat_wxyz'][:,[1,2,3,0]])
relative=(rot[0].inv()*rot).as_rotvec();assert np.linalg.norm(relative,axis=1).max()<np.pi/2
m['wrist_quat_wxyz']=(rot[0]*Rotation.from_rotvec(gaussian_filter1d(relative,sigma,axis=0))).as_quat()[:,[3,0,1,2]]
for n in ['l20_moving.xml','l20_right.xml']:shutil.copy2(base/'corrected'/n,out/n)
model=mujoco.MjModel.from_xml_path(str(out/'l20_moving.xml'));data=mujoco.MjData(model)
addr=[model.jnt_qposadr[model.joint(str(n)).id] for n in names];sites=[model.site(f'landmark_{i:02d}').id for i in range(21)]
actual=[]
for q in m['qpos']:
    data.qpos[addr]=q;mujoco.mj_forward(model,data);actual.append(data.site_xpos[sites].copy())
m['actual']=np.array(actual);m['command_u8']=np.array([cal.command(dict(zip(names,q))) for q in m['qpos']]);m['decoded_qpos']=np.array([cal.decode(c,names) for c in m['command_u8']])
m['offline_smoothing_sigma_frames']=sigma
np.savez_compressed(out/'motion.npz',**m)
np.savetxt(out/'trajectory.csv',np.c_[m['time'],m['wrist_pos'],m['wrist_quat_wxyz'],m['qpos']],delimiter=',',header=','.join(['time_s','wrist_x','wrist_y','wrist_z','qw','qx','qy','qz']+names),comments='')
limits=np.array([cal.urdf_limits[n] for n in names]);r=dict(frames=len(a),offline_sigma_frames=sigma,uses_future_frames=True,
tip_error_mean_mm=float(np.linalg.norm(m['actual'][:,[4,8,12,16,20]]-m['targets'][:,[4,8,12,16,20]],axis=-1).mean()*1000),
max_joint_step_deg=float(np.abs(np.diff(m['qpos'],axis=0)).max()*180/np.pi),joint_second_difference_rms_deg=float(np.sqrt(np.mean(np.diff(m['qpos'],n=2,axis=0)**2))*180/np.pi),
wrist_second_difference_rms_mm=float(np.sqrt(np.mean(np.diff(m['wrist_pos'],n=2,axis=0)**2))*1000),wrist_max_change_from_source_mm=float(np.linalg.norm(m['wrist_pos']-m['wrist_pos_unsmoothed'],axis=1).max()*1000),
wrist_range_m=np.ptp(m['wrist_pos'],axis=0).tolist(),limit_violations=int(np.sum((m['qpos']<limits[:,0]-1e-8)|(m['qpos']>limits[:,1]+1e-8))),
boundary='Offline hand-only smoothing ablation, not contact or hardware validation')
(out/'validation.json').write_text(json.dumps(r,indent=2));print(json.dumps(r,indent=2))