from pathlib import Path
import json,numpy as np
from scipy.ndimage import gaussian_filter1d
from scipy.spatial.transform import Rotation
ROOT=Path(__file__).resolve().parents[1];B=ROOT/'output/20260915_171525_dynhamr';a=dict(np.load(B/'object_refit_tracked/object_poses.npz'));report={}
for n in ['upper','lower']:
 center=np.array([0, .00415 if n=='upper' else -.03335, .2185 if n=='upper' else .21975])
 r=Rotation.from_quat(a[n+'_quaternion_xyzw']);c=r.apply(np.tile(center,(352,1)))+a[n+'_position_world'];cs=gaussian_filter1d(c,2,axis=0)
 rs=[]
 for t in range(352):
  ix=np.arange(max(0,t-6),min(352,t+7));rs.append(r[ix].mean(weights=np.exp(-.5*((ix-t)/2)**2)).as_quat())
 rr=Rotation.from_quat(rs)
 # Resolve visible-face ambiguity using the source video: flat red face and circular blue feature.
 rr=rr*Rotation.from_matrix(np.diag([-1.,-1.,1.]))
 a[n+'_position_world']=cs-rr.apply(np.tile(center,(352,1)));a[n+'_quaternion_xyzw']=rr.as_quat()
 angles=np.rad2deg((rr[:-1].inv()*rr[1:]).magnitude());steps=np.linalg.norm(np.diff(cs,axis=0),axis=1)*1000
 report[n]={'max_center_step_mm':float(steps.max()),'max_rotation_step_deg':float(angles.max()),'center_smoothing_max_change_mm':float(np.linalg.norm(cs-c,axis=1).max()*1000)}
 assert np.isfinite(cs).all() and angles.max()<20 and steps.max()<30
# Video inspection: lower part becomes occluded during placement (frames 180-214),
# and the common CAD assembly is visible from frame 215. This is an explicit assumption.
from scipy.spatial.transform import Slerp
hold_p=a['lower_position_world'][176].copy();hold_q=a['lower_quaternion_xyzw'][176].copy()
a['lower_position_world'][177:190]=hold_p;a['lower_quaternion_xyzw'][177:190]=hold_q
for t in range(190,215):
 alpha=(t-189)/26
 a['lower_position_world'][t]=(1-alpha)*hold_p+alpha*a['upper_position_world'][t]
 a['lower_quaternion_xyzw'][t]=Slerp([0,1],Rotation.from_quat([hold_q,a['upper_quaternion_xyzw'][t]]))([alpha]).as_quat()[0]
a['lower_position_world'][215:]=a['upper_position_world'][215:]
a['lower_quaternion_xyzw'][215:]=a['upper_quaternion_xyzw'][215:]
a['lower_keyframe_valid'][177:]=False
a['lower_assembly_assumed']=np.arange(352)>=215
a['lower_occluded_transition_assumed']=(np.arange(352)>=177)&(np.arange(352)<215)
report['assembly_assumption']={'from_frame':215,'transition_frames':[177,214],'basis':'Visual inspection and shared CAD assembly coordinates; not independent measured lower poses.'}
np.savez_compressed(B/'object_aligned/object_poses.npz',**a)
(B/'object_aligned/smoothing_validation.json').write_text(json.dumps(report,indent=2));print(report)