"""Unit test per umeyama_similarity (absolute orientation con scala)."""
from __future__ import annotations

import numpy as np
import pytest

from presa_mano.geometry import umeyama_similarity, rpy_deg_to_quaternion_xyzw, quat_xyzw_to_R


def _known_transform():
    # scala 1.5, rotazione nota, traslazione nota
    R = quat_xyzw_to_R(rpy_deg_to_quaternion_xyzw(10.0, -20.0, 35.0))
    s = 1.5
    t = np.array([0.3, -0.1, 0.2])
    return s, R, t


def test_recovers_known_similarity():
    s, R, t = _known_transform()
    rng = np.random.default_rng(0)
    src = rng.normal(size=(8, 3))
    dst = (s * (R @ src.T)).T + t

    s_hat, R_hat, t_hat, rmse = umeyama_similarity(src, dst)

    assert abs(s_hat - s) < 1e-6
    assert np.allclose(R_hat, R, atol=1e-6)
    assert np.allclose(t_hat, t, atol=1e-6)
    assert rmse < 1e-6


def test_forbids_reflection():
    rng = np.random.default_rng(1)
    src = rng.normal(size=(5, 3))
    dst = rng.normal(size=(5, 3))
    _, R_hat, _, _ = umeyama_similarity(src, dst)
    assert np.linalg.det(R_hat) > 0


def test_raises_on_too_few_points():
    with pytest.raises(ValueError):
        umeyama_similarity(np.zeros((2, 3)), np.zeros((2, 3)))


def test_raises_on_degenerate_points():
    src = np.ones((4, 3))
    dst = np.ones((4, 3)) * 2.0
    with pytest.raises(ValueError):
        umeyama_similarity(src, dst)