import numpy as np
from scipy.spatial.transform import Rotation as R
from vid2rig.motion import MotionClip
from vid2rig.retarget.resample import resample

def _clip(fps, rots, transl):
    F, J = rots.shape[0], rots.shape[1]
    return MotionClip(fps=fps, num_frames=F, source="smpl",
        joint_names=[f"j{i}" for i in range(J)], local_rotations=rots,
        root_translation=transl, root_rotation=rots[:, 0],
        rest_offsets=np.zeros((J, 3)), parents=[-1] + list(range(J - 1)), source_height_m=1.7)

def test_resample_doubles_frames_and_midpoint():
    q0 = R.from_rotvec([0, 0, 0.0]).as_quat(); q1 = R.from_rotvec([0, 0, np.pi / 2]).as_quat()
    rots = np.stack([q0, q1])[:, None, :]                       # [2,1,4]
    transl = np.array([[0, 0, 0.0], [2, 0, 0]])
    out = resample(_clip(30.0, rots, transl), 60.0)
    assert out.num_frames == 3 and out.fps == 60.0
    assert np.allclose(out.root_translation[1], [1, 0, 0], atol=1e-6)   # lerp midpoint
    mid = R.from_quat(out.local_rotations[1, 0]).as_rotvec()
    assert np.allclose(np.abs(mid), [0, 0, np.pi / 4], atol=1e-6)       # slerp midpoint
    assert np.allclose(out.local_rotations[:, 0], out.root_rotation)