"""카메라 리그 수학. numpy 가 필요하므로 컨테이너에서 돈다."""

import math

import pytest

np = pytest.importorskip("numpy")

from nusim.data.index import CameraCalib, Frame
from nusim.data.rig import camera_pose, quat_to_matrix, rt_to_matrix


def test_identity_quaternion():
    assert np.allclose(quat_to_matrix((1.0, 0.0, 0.0, 0.0)), np.eye(3))


def test_quaternion_is_orthonormal():
    rng = np.random.default_rng(0)
    for _ in range(50):
        q = rng.normal(size=4)
        q /= np.linalg.norm(q)
        r = quat_to_matrix(tuple(q))
        assert np.allclose(r.T @ r, np.eye(3), atol=1e-12)
        assert abs(np.linalg.det(r) - 1.0) < 1e-12


def test_quaternion_90deg_about_z():
    # (w,x,y,z) = (cos45, 0, 0, sin45) → z축 +90도
    s = math.sqrt(0.5)
    r = quat_to_matrix((s, 0.0, 0.0, s))
    assert np.allclose(r @ np.array([1.0, 0, 0]), [0, 1, 0], atol=1e-12)


def test_degenerate_quaternion_returns_identity():
    # 정규화 불가능한 입력에 NaN 을 내놓는 대신 항등행렬로 안전하게 떨어져야 한다
    assert np.allclose(quat_to_matrix((0.0, 0.0, 0.0, 0.0)), np.eye(3))


def _pose():
    f = Frame(0, "x.jpg", (100.0, 200.0, 0.0), (math.sqrt(0.5), 0.0, 0.0, math.sqrt(0.5)), True)
    c = CameraCalib(
        translation=(1.7, 0.0, 1.5),
        rotation=(0.5, -0.5, 0.5, -0.5),
        intrinsic=((1266.4, 0.0, 816.3), (0.0, 1266.4, 491.5), (0.0, 0.0, 1.0)),
        width=1600, height=900,
    )
    return camera_pose(f, c, "CAM_FRONT")


def test_w2c_inverts_c2w():
    p = _pose()
    assert np.allclose(p.w2c @ p.c2w, np.eye(4), atol=1e-12)


def test_camera_is_offset_from_ego_origin():
    p = _pose()
    # ego 가 (100,200,0) 에 +90도로 서 있고 카메라가 전방 1.7 m, 높이 1.5 m 이므로
    # 전역 좌표에서는 ego 의 +y 방향으로 1.7 m 밀려 있어야 한다.
    assert np.allclose(p.position, [100.0, 201.7, 1.5], atol=1e-9)


def test_projection_is_vulkan_clip_space():
    p = _pose()
    proj = p.projection(znear=0.1, zfar=100.0)
    # znear 에 있는 점은 z_ndc=0, zfar 는 1 이어야 한다 (OpenGL 의 -1..1 이 아니다)
    for z, expect in ((0.1, 0.0), (100.0, 1.0)):
        v = proj @ np.array([0.0, 0.0, z, 1.0])
        assert abs(v[2] / v[3] - expect) < 1e-6


def test_projection_matches_pixel_projection():
    # 카메라 +y 는 '아래'(OpenCV)이고 Vulkan NDC 도 y 가 아래로 + 라 뒤집기가 없어야 한다.
    # 임의의 점을 K 로 픽셀에 투영한 결과와 NDC 가 정확히 일치하는지 본다.
    p = _pose()
    proj = p.projection()
    for pt in ([0.0, 1.0, 10.0], [3.0, -2.0, 25.0], [-5.0, 0.5, 7.0]):
        x, y, z = pt
        u = p.K[0, 0] * x / z + p.K[0, 2]
        v = p.K[1, 1] * y / z + p.K[1, 2]
        c = proj @ np.array([x, y, z, 1.0])
        ndc = c[:2] / c[3]
        assert np.allclose(ndc, [2 * u / p.width - 1, 2 * v / p.height - 1], atol=1e-9)
    # 아래쪽 점(+y)은 NDC 에서도 + 여야 한다
    c = proj @ np.array([0.0, 1.0, 10.0, 1.0])
    assert c[1] / c[3] > 0


def test_fov_matches_nuscenes_front_camera():
    h, v = _pose().fov_deg()
    assert 63.0 < h < 66.0   # nuScenes CAM_FRONT 은 약 64도
    assert 38.0 < v < 42.0


def test_rt_to_matrix():
    m = rt_to_matrix(np.eye(3), (1.0, 2.0, 3.0))
    assert np.allclose(m[:3, 3], [1, 2, 3])
    assert m[3, 3] == 1.0