"""
Test module for the AHRS calibration module.

Authors: Sebastián Rodríguez-Martínez
Contact: srodriguez@mbari.org
Last Update: 2025-04-15
"""

import numpy as np
import pytest

from navlib import cal_ahrs_so3
from navlib.cal import cal_ahrs_so3 as cal_ahrs_so3_relative


def test_import():
    # Test if both import methods work
    assert cal_ahrs_so3_relative.__code__.co_code == cal_ahrs_so3.__code__.co_code


def test_cal_ahrs_so3_identity():
    """
    Test that if both AHRS sensor data are identical, the calibration
    returns the identity matrix.
    """
    # Create a simple valid test input where both time arrays are identical
    times = np.array([1, 2, 3, 4])
    # Create a N x 3 array of zero angles (roll, pitch, yaw) for both sensors.
    # Assuming rph2rot( [0,0,0] ) returns the identity matrix.
    rph = np.zeros((4, 3))
    # For simplicity, disable filtering (set low_pass_filter to False)
    rot = cal_ahrs_so3(times, times, rph, rph, low_pass_filter=False)
    np.testing.assert_allclose(rot, np.eye(3), atol=1e-5)


def test_cal_ahrs_so3_invalid_type():
    """
    Test that passing an incorrect type for one of the inputs
    raises a TypeError.
    """
    times = [1, 2, 3, 4]  # acceptable (will be converted) if list
    rph = [[0, 0, 0]] * 4  # acceptable if convertible
    # Here, intentionally pass an invalid type for low_pass_filter.
    with pytest.raises(TypeError):
        cal_ahrs_so3(times, times, rph, rph, low_pass_filter="False")

    # Also, passing a non-array (non-list) for time_ahrs_1 should trigger a TypeError.
    with pytest.raises(TypeError):
        cal_ahrs_so3("invalid time", times, rph, rph, low_pass_filter=False)


def test_cal_ahrs_so3_dimension_error():
    """
    Test that passing a time array with invalid dimensions raises a ValueError.
    """
    # Create a 2D time array that does not fit (should be 1D or squeezeable to 1D)
    times_invalid = np.array([[1, 2, 3, 4], [1, 2, 3, 4]])
    rph = np.zeros((4, 3))
    with pytest.raises(ValueError):
        cal_ahrs_so3(times_invalid, times_invalid, rph, rph, low_pass_filter=False)


def test_cal_ahrs_so3_measurement_mismatch():
    """
    Test that a mismatch in the number of time measurements and rph measurements
    raises a ValueError.
    """
    times = np.array([1, 2, 3, 4])
    # Create an rph array with a different number of measurements (3 instead of 4)
    rph_mismatch = np.zeros((3, 3))
    with pytest.raises(ValueError):
        cal_ahrs_so3(times, times, rph_mismatch, rph_mismatch, low_pass_filter=False)
