from __future__ import annotations

import importlib.util
import sys
import unittest
from pathlib import Path

import numpy as np


SCRIPT = Path(__file__).parents[1] / "triangulate_with_rtk_lrf_poses.py"
sys.path.insert(0, str(SCRIPT.parent))
SPEC = importlib.util.spec_from_file_location("rtk_lrf_pose_triangulation", SCRIPT)
MODULE = importlib.util.module_from_spec(SPEC)
assert SPEC.loader is not None
SPEC.loader.exec_module(MODULE)


class RtkLrfPoseTriangulationTests(unittest.TestCase):
    def test_upright_pose_looks_at_target(self) -> None:
        centre = np.asarray([10.0, 20.0, 30.0])
        target = np.asarray([30.0, 60.0, 10.0])
        pose = MODULE.upright_cam_from_world(centre, target)
        np.testing.assert_allclose(pose.inverse().translation, centre, atol=1e-10)
        forward = pose.rotation.matrix().T @ np.asarray([0.0, 0.0, 1.0])
        expected = (target - centre) / np.linalg.norm(target - centre)
        np.testing.assert_allclose(forward, expected, atol=1e-10)


if __name__ == "__main__":
    unittest.main()