import sys, os, tempfile
import numpy as np
import h5py
import pytest

sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))

def _make_test_h5(path, n_poses=100):
    T = np.tile(np.eye(4), (n_poses, 1, 1)).astype(np.float64)
    for i in range(n_poses):
        T[i, 0, 3] = float(i) * 0.1
    with h5py.File(path, "w") as f:
        pw = f.create_group("poses_world")
        pw.create_dataset("T_4x4", data=T)
        pw.create_dataset("t_ns", data=np.arange(n_poses, dtype=np.int64) * int(1e8))
        f.attrs["status"] = "aligned"

def test_load_world_poses_returns_n_transforms():
    from scripts.stitch import _load_world_poses
    with tempfile.NamedTemporaryFile(suffix=".h5", delete=False) as tmp:
        path = tmp.name
    try:
        _make_test_h5(path, n_poses=100)
        transforms = _load_world_poses(path, 4)
        assert len(transforms) == 4
        assert np.allclose(transforms[0], np.eye(4), atol=1e-6), "T_0 must be identity"
        assert not np.allclose(transforms[1], np.eye(4)), "T_1 must differ from identity"
        for T in transforms:
            assert T.shape == (4, 4)
    finally:
        os.unlink(path)

def test_load_world_poses_single_ply():
    from scripts.stitch import _load_world_poses
    with tempfile.NamedTemporaryFile(suffix=".h5", delete=False) as tmp:
        path = tmp.name
    try:
        _make_test_h5(path, n_poses=10)
        transforms = _load_world_poses(path, 1)
        assert len(transforms) == 1
        assert np.allclose(transforms[0], np.eye(4), atol=1e-6)
    finally:
        os.unlink(path)