gps-denied-onboard/tests/unit/test_az277_se3_utils.py

"""AC tests for AZ-277: SE3Utils helper module.

Verifies the `se3_utils` contract v1.0.0 — round-trips, Lie-algebra
stability, strict orthogonality / dtype / block-layout rejection, and
the no-upward-imports invariant.
"""

from __future__ import annotations

import ast
from pathlib import Path

import gtsam
import numpy as np
import pytest

from gps_denied_onboard.helpers import (
    SE3,
    Se3InvalidMatrixError,
    adjoint,
    exp_map,
    is_valid_rotation,
    log_map,
    matrix_to_se3,
    se3_to_matrix,
)

SEED = 12345
RNG = np.random.default_rng(SEED)


def _random_valid_T(rng: np.random.Generator) -> np.ndarray:
    """Produce a valid SE(3) 4x4 matrix from a random twist."""
    xi = rng.standard_normal(6) * 0.5
    pose = gtsam.Pose3.Expmap(xi.astype(np.float64))
    return np.ascontiguousarray(pose.matrix(), dtype=np.float64)


def test_ac1_matrix_se3_roundtrip_within_tolerance() -> None:
    # Arrange + Act + Assert
    for _ in range(20):
        T = _random_valid_T(RNG)
        pose = matrix_to_se3(T)
        recovered = se3_to_matrix(pose)
        assert np.allclose(recovered, T, atol=1e-9), (
            f"matrix_to_se3 -> se3_to_matrix round-trip diverged for T:\n{T}"
        )


def test_ac2_lie_algebra_roundtrip() -> None:
    # Arrange + Act + Assert
    for _ in range(20):
        xi = RNG.standard_normal(6).astype(np.float64)
        norm = float(np.linalg.norm(xi))
        if norm == 0.0:
            continue
        xi *= 1.0 / norm  # norm ~= 1.0
        recovered = log_map(exp_map(xi))
        assert np.allclose(recovered, xi, atol=1e-9), (
            f"log_map(exp_map(xi)) round-trip diverged for xi={xi}"
        )


def test_ac3_near_identity_exp_map_is_stable() -> None:
    # Arrange
    xi = np.full(6, 1e-12, dtype=np.float64)

    # Act
    pose = exp_map(xi)
    matrix = se3_to_matrix(pose)

    # Assert
    identity = np.eye(4, dtype=np.float64)
    assert np.allclose(matrix, identity, atol=1e-9), (
        f"near-identity exp_map must return identity within atol=1e-9; got\n{matrix}"
    )
    assert not np.any(np.isnan(matrix)), "near-identity exp_map must not produce NaNs"


def test_ac4_strict_orthogonality_rejection() -> None:
    # Arrange — drift R^T R away from I by 1e-3 in Frobenius norm
    T = _random_valid_T(RNG)
    T[:3, :3] += np.eye(3, dtype=np.float64) * 1e-3

    # Act + Assert
    with pytest.raises(Se3InvalidMatrixError) as excinfo:
        matrix_to_se3(T)
    msg = str(excinfo.value)
    assert "orthogonal" in msg.lower() or "R^T R" in msg, (
        "rejection message must explain the orthogonality drift"
    )


def test_ac5_mirror_rotation_rejected() -> None:
    # Arrange — flip the sign of one column to produce det(R) = -1
    T = _random_valid_T(RNG)
    T[:3, 0] = -T[:3, 0]

    # Act + Assert
    with pytest.raises(Se3InvalidMatrixError) as excinfo:
        matrix_to_se3(T)
    msg = str(excinfo.value)
    assert "determinant" in msg.lower() or "negative" in msg.lower()


def test_ac6_bottom_row_guard() -> None:
    # Arrange
    T = _random_valid_T(RNG)
    T[3] = np.array([0.0, 0.0, 0.0, 2.0], dtype=np.float64)

    # Act + Assert
    with pytest.raises(Se3InvalidMatrixError) as excinfo:
        matrix_to_se3(T)
    assert "bottom row" in str(excinfo.value).lower()


def test_ac7_float32_dtype_rejected() -> None:
    # Arrange
    T = _random_valid_T(RNG).astype(np.float32)

    # Act + Assert
    with pytest.raises(Se3InvalidMatrixError) as excinfo:
        matrix_to_se3(T)
    assert "dtype" in str(excinfo.value).lower() or "float" in str(excinfo.value).lower()


def test_ac8_determinism_byte_equal_outputs() -> None:
    # Arrange
    T = _random_valid_T(RNG)

    # Act
    pose_a = matrix_to_se3(T)
    pose_b = matrix_to_se3(T)
    matrix_a = se3_to_matrix(pose_a)
    matrix_b = se3_to_matrix(pose_b)

    # Assert
    assert np.array_equal(matrix_a, matrix_b), (
        "matrix_to_se3 -> se3_to_matrix must be byte-equal across calls"
    )


def test_ac9_no_upward_imports_to_components() -> None:
    """The helper module MUST NOT import from gps_denied_onboard.components.*."""
    # Arrange
    module_path = (
        Path(__file__).resolve().parents[2]
        / "src"
        / "gps_denied_onboard"
        / "helpers"
        / "se3_utils.py"
    )
    tree = ast.parse(module_path.read_text())
    bad_imports: list[str] = []

    # Act
    for node in ast.walk(tree):
        if isinstance(node, ast.ImportFrom):
            module = node.module or ""
            if module.startswith("gps_denied_onboard.components"):
                bad_imports.append(module)
        elif isinstance(node, ast.Import):
            for alias in node.names:
                if alias.name.startswith("gps_denied_onboard.components"):
                    bad_imports.append(alias.name)

    # Assert
    assert not bad_imports, (
        f"helpers.se3_utils must not import from components.*; found {bad_imports}"
    )


def test_is_valid_rotation_predicate() -> None:
    # Arrange
    R_good = np.eye(3, dtype=np.float64)
    R_drift = np.eye(3, dtype=np.float64) + 1e-2  # off-orthogonal
    R_mirror = np.diag([-1.0, 1.0, 1.0]).astype(np.float64)
    R_f32 = np.eye(3, dtype=np.float32)

    # Assert
    assert is_valid_rotation(R_good) is True
    assert is_valid_rotation(R_drift) is False
    assert is_valid_rotation(R_mirror) is False
    assert is_valid_rotation(R_f32) is False
    assert is_valid_rotation("not an array") is False  # type: ignore[arg-type]


def test_adjoint_shape() -> None:
    # Arrange
    T = _random_valid_T(RNG)
    pose = matrix_to_se3(T)

    # Act
    ad = adjoint(pose)

    # Assert
    assert ad.shape == (6, 6)
    assert ad.dtype == np.float64


def test_se3_is_gtsam_pose3_alias() -> None:
    # Assert
    assert SE3 is gtsam.Pose3