"""
Test module for the vector attitude estimation module.

Authors: Sebastián Rodríguez-Martínez
Contact: srodriguez@mbari.org
Last Update: 2024-03-25
"""

import numpy as np
import pytest

from navlib import (
    ahrs_complementary_filter,
    ahrs_correct_magfield,
    ahrs_hua_filter,
    ahrs_madgwick_filter,
    ahrs_mahony_filter,
    ahrs_raw_hdg,
    ahrs_raw_rp,
    ahrs_raw_rph,
    so3_integrator,
)
from navlib.nav import ahrs_complementary_filter as ahrs_complementary_filter_relative
from navlib.nav import ahrs_correct_magfield as ahrs_correct_magfield_relative
from navlib.nav import ahrs_hua_filter as ahrs_hua_mahony_filter_relative
from navlib.nav import ahrs_madgwick_filter as ahrs_madgwick_filter_relative
from navlib.nav import ahrs_mahony_filter as ahrs_mahony_filter_relative
from navlib.nav import ahrs_raw_hdg as ahrs_raw_hdg_relative
from navlib.nav import ahrs_raw_rp as ahrs_raw_rp_relative
from navlib.nav import ahrs_raw_rph as ahrs_raw_rph_relative
from navlib.nav import so3_integrator as so3_integrator_relative


def test_imports():
    # Checks that the absolute and relative imports work correctly
    assert (
        ahrs_complementary_filter_relative.__code__.co_code
        == ahrs_complementary_filter.__code__.co_code
    )
    assert (
        ahrs_correct_magfield_relative.__code__.co_code
        == ahrs_correct_magfield.__code__.co_code
    )
    assert (
        ahrs_hua_mahony_filter_relative.__code__.co_code
        == ahrs_hua_filter.__code__.co_code
    )
    assert (
        ahrs_madgwick_filter_relative.__code__.co_code
        == ahrs_madgwick_filter.__code__.co_code
    )
    assert (
        ahrs_mahony_filter_relative.__code__.co_code
        == ahrs_mahony_filter.__code__.co_code
    )
    assert ahrs_raw_hdg_relative.__code__.co_code == ahrs_raw_hdg.__code__.co_code
    assert ahrs_raw_rp_relative.__code__.co_code == ahrs_raw_rp.__code__.co_code
    assert ahrs_raw_rph_relative.__code__.co_code == ahrs_raw_rph.__code__.co_code
    assert so3_integrator_relative.__code__.co_code == so3_integrator.__code__.co_code


def test_so3_integrator():
    """
    Test the so3_integrator function.
    """
    # Test case 1: Valid input
    Rold = np.eye(3)
    Rdot = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 0]])
    dt = 0.1
    Rnew = so3_integrator(Rold, Rdot, dt)
    assert Rnew.shape == (3, 3)

    # Test case 2: List input
    Rnew = so3_integrator(Rold.tolist(), Rdot.tolist(), dt)
    assert Rnew.shape == (3, 3)

    # Test case 3: Invalid Rold type
    try:
        so3_integrator("invalid_type", Rdot, dt)
    except TypeError as e:
        assert str(e) == "Rold must be a numpy array or list"
    else:
        assert False

    # Test case 4: Invalid Rdot type
    try:
        so3_integrator(Rold, "invalid_type", dt)
    except TypeError as e:
        assert str(e) == "Rdot must be a numpy array or list"
    else:
        assert False

    # Test case 5: Invalid dt type
    try:
        so3_integrator(Rold, Rdot, "invalid_type")
    except TypeError as e:
        assert str(e) == "dt must be a float or integer"
    else:
        assert False

    # Test case 6: Invalid dt value
    try:
        so3_integrator(Rold, Rdot, -0.1)
    except ValueError as e:
        assert str(e) == "dt must be a positive value"
    else:
        assert False

    # Test case 7: Invalid Rold shape
    try:
        so3_integrator(np.array([1, 2, 3, 4]), Rdot, dt)
    except ValueError as e:
        assert str(e) == "Rold must be a 3x3 matrix"
    else:
        assert False

    # Test case 8: Invalid Rdot shape
    try:
        so3_integrator(Rold, np.array([1, 2, 3, 4]), dt)
    except ValueError as e:
        assert str(e) == "Rdot must be a 3x3 matrix"
    else:
        assert False


def test_ahrs_correct_magfield():
    """
    Test for the ahrs_correct_magfield function.
    """

    # Test 1: Invalid magnetic_field type (string)
    with pytest.raises(TypeError):
        ahrs_correct_magfield("invalid", [0, 0, 0], np.eye(3))

    # Test 2: Invalid hard_iron type (string)
    with pytest.raises(TypeError):
        ahrs_correct_magfield([1, 2, 3], "invalid", np.eye(3))

    # Test 3: Invalid soft_iron type (string)
    with pytest.raises(TypeError):
        ahrs_correct_magfield([1, 2, 3], [0, 0, 0], "invalid")

    # Test 4: Magnetic field not 2D array
    with pytest.raises(ValueError):
        ahrs_correct_magfield(np.array([1, 2, 3]), [0, 0, 0], np.eye(3))

    # Test 5: Magnetic field not 3xN or Nx3
    with pytest.raises(ValueError):
        ahrs_correct_magfield(np.array([[1, 2], [3, 4]]), [0, 0, 0], np.eye(3))

    # Test 6: Hard iron not a 1x3 array
    with pytest.raises(ValueError):
        ahrs_correct_magfield(
            np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]]), [0, 0], np.eye(3)
        )

    # Test 7: Soft iron not 3x3
    with pytest.raises(ValueError):
        ahrs_correct_magfield(
            np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]]), [0, 0, 0], np.eye(2)
        )

    # Test 8: Soft iron matrix not symmetric
    non_symmetric_matrix = np.array([[1, 0.1, 0], [0.2, 1, 0], [0, 0, 1]])
    with pytest.raises(ValueError):
        ahrs_correct_magfield(
            np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]]), [0, 0, 0], non_symmetric_matrix
        )

    # Test 9: Soft iron matrix not positive definite
    non_positive_definite_matrix = np.array([[0, 0, 0], [0, 0, 0], [0, 0, 0]])
    with pytest.raises(ValueError):
        ahrs_correct_magfield(
            np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]]),
            [0, 0, 0],
            non_positive_definite_matrix,
        )

    # Test 10: Valid input (basic functionality check)
    magnetic_field = np.array([[1, 0, -1], [2, 0, -2], [3, 0, -3]])
    hard_iron = np.array([0.5, 0.5, 0.5])
    soft_iron = np.eye(3)

    corrected_field = ahrs_correct_magfield(magnetic_field, hard_iron, soft_iron)

    assert isinstance(corrected_field, np.ndarray)
    assert corrected_field.shape == magnetic_field.shape

    # Test 11: Check the correctness of the output
    expected_output = magnetic_field - hard_iron
    assert np.allclose(corrected_field, expected_output)


def test_ahrs_raw_rp():
    """
    Test the ahrs_raw_rp function with enhanced flexible input handling.
    """
    # Test case 1: Valid 2D input (Nx3)
    acceleration = np.array([[0.0, 0.0, -9.81], [9.81, 0.0, 0.0], [0.0, 9.81, 0.0]])
    output = ahrs_raw_rp(acceleration)
    assert output.shape == (3, 2)
    assert isinstance(output, np.ndarray)

    # Test case 2: Valid 2D input (3xN)
    acceleration_3xn = acceleration.T  # Shape (3, 3)
    output_3xn = ahrs_raw_rp(acceleration_3xn)
    assert output_3xn.shape == (3, 2)
    # Note: For 3x3 arrays, results may differ due to ambiguous shape interpretation

    # Test case 3: Valid 2D input with unambiguous shapes
    acceleration_5x3 = np.array(
        [
            [0.0, 0.0, -9.81],
            [9.81, 0.0, 0.0],
            [0.0, 9.81, 0.0],
            [0.1, 0.2, -9.7],
            [-0.1, 0.3, -9.9],
        ]
    )
    acceleration_3x5 = acceleration_5x3.T
    output_5x3 = ahrs_raw_rp(acceleration_5x3)
    output_3x5 = ahrs_raw_rp(acceleration_3x5)
    assert output_5x3.shape == (5, 2)
    assert output_3x5.shape == (5, 2)
    assert np.allclose(output_5x3, output_3x5, atol=1e-12)

    # Test case 4: List input (2D)
    acceleration_list = [[0.0, 0.0, -9.81], [9.81, 0.0, 0.0], [0.0, 9.81, 0.0]]
    output_list = ahrs_raw_rp(acceleration_list)
    assert output_list.shape == (3, 2)
    assert np.allclose(output, output_list)

    # Test case 5: Single measurement - 1D array
    acceleration_1d = np.array([0.1, 0.2, -9.8])
    output_1d = ahrs_raw_rp(acceleration_1d)
    assert output_1d.shape == (1, 2)
    assert isinstance(output_1d, np.ndarray)

    # Test case 6: Single measurement - list
    acceleration_single_list = [0.1, 0.2, -9.8]
    output_single_list = ahrs_raw_rp(acceleration_single_list)
    assert output_single_list.shape == (1, 2)
    assert np.allclose(output_1d, output_single_list)

    # Test case 7: Single measurement - 1x3 array
    acceleration_1x3 = np.array([[0.1, 0.2, -9.8]])
    output_1x3 = ahrs_raw_rp(acceleration_1x3)
    assert output_1x3.shape == (1, 2)
    assert np.allclose(output_1d, output_1x3)

    # Test case 8: Single measurement - 3x1 array
    acceleration_3x1 = np.array([[0.1], [0.2], [-9.8]])
    output_3x1 = ahrs_raw_rp(acceleration_3x1)
    assert output_3x1.shape == (1, 2)
    assert np.allclose(output_1d, output_3x1)

    # Error test cases
    # Test case 9: Invalid acceleration type
    with pytest.raises(TypeError) as excinfo:
        ahrs_raw_rp("invalid_type")
    assert str(excinfo.value) == "Acceleration must be a numpy array or list"

    # Test case 10: Invalid 1D acceleration shape
    with pytest.raises(ValueError) as excinfo:
        ahrs_raw_rp(np.array([1, 2]))  # Only 2 elements
    assert (
        str(excinfo.value) == "For 1D arrays, acceleration must have exactly 3 elements"
    )

    # Test case 11: Invalid 2D acceleration shape
    with pytest.raises(ValueError) as excinfo:
        ahrs_raw_rp(np.array([[1, 2], [3, 4]]))  # 2x2, neither 3xN nor Nx3
    assert str(excinfo.value) == "For 2D arrays, acceleration must be 3xN or Nx3"

    # Test case 12: Invalid dimensions (3D array)
    with pytest.raises(ValueError) as excinfo:
        ahrs_raw_rp(np.array([[[1, 2, 3]]]))
    assert (
        str(excinfo.value)
        == "Acceleration must be a 1D array (3 elements) or 2D array (3xN or Nx3)"
    )

    # Test case 13: Mathematical correctness for known values
    # Test with standard gravity vector (gravity sensor measures upward acceleration)
    gravity_up = [0.0, 0.0, -9.81]  # Standard gravity pointing up
    rp_gravity = ahrs_raw_rp(gravity_up)
    # Roll and pitch should be close to zero for gravity pointing up
    assert abs(rp_gravity[0, 0]) < 1e-10  # Roll ≈ 0
    assert abs(rp_gravity[0, 1]) < 1e-10  # Pitch ≈ 0


def test_ahrs_raw_hdg():
    """
    Test the ahrs_raw_hdg function with enhanced flexible input handling.
    """
    # Test case 1: Valid 2D input without rph (Nx3)
    magnetic_field = np.array([[0.3, 0.4, 0.5], [0.3, 0.4, 0.5], [0.3, 0.4, 0.5]])
    output = ahrs_raw_hdg(magnetic_field)
    assert output.shape == (3, 1)
    assert isinstance(output, np.ndarray)

    # Test case 2: Valid 2D input with rph (Nx3)
    rph = np.array([[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]])
    output_rph = ahrs_raw_hdg(magnetic_field, rph)
    assert output_rph.shape == (3, 1)

    # Test case 3: Valid 2D input with unambiguous shapes
    magnetic_field_5x3 = np.array(
        [
            [0.3, 0.4, 0.5],
            [0.8, 0.1, 0.2],
            [1.0, 0.0, 0.3],
            [0.5, 0.6, 0.4],
            [0.9, 0.2, 0.1],
        ]
    )
    magnetic_field_3x5 = magnetic_field_5x3.T
    output_5x3 = ahrs_raw_hdg(magnetic_field_5x3)
    output_3x5 = ahrs_raw_hdg(magnetic_field_3x5)
    assert output_5x3.shape == (5, 1)
    assert output_3x5.shape == (5, 1)
    assert np.allclose(output_5x3, output_3x5, atol=1e-12)

    # Test case 4: List input (2D)
    magnetic_field_list = [[0.3, 0.4, 0.5], [0.3, 0.4, 0.5], [0.3, 0.4, 0.5]]
    output_list = ahrs_raw_hdg(magnetic_field_list)
    assert output_list.shape == (3, 1)
    assert np.allclose(output, output_list)

    # Test case 5: Single measurement - 1D array
    magnetic_field_1d = np.array([1.0, 0.1, 0.3])
    output_1d = ahrs_raw_hdg(magnetic_field_1d)
    assert output_1d.shape == (1, 1)
    assert isinstance(output_1d, np.ndarray)

    # Test case 6: Single measurement - list
    magnetic_field_single_list = [1.0, 0.1, 0.3]
    output_single_list = ahrs_raw_hdg(magnetic_field_single_list)
    assert output_single_list.shape == (1, 1)
    assert np.allclose(output_1d, output_single_list)

    # Test case 7: Single measurement - 1x3 array
    magnetic_field_1x3 = np.array([[1.0, 0.1, 0.3]])
    output_1x3 = ahrs_raw_hdg(magnetic_field_1x3)
    assert output_1x3.shape == (1, 1)
    assert np.allclose(output_1d, output_1x3)

    # Test case 8: Single measurement - 3x1 array
    magnetic_field_3x1 = np.array([[1.0], [0.1], [0.3]])
    output_3x1 = ahrs_raw_hdg(magnetic_field_3x1)
    assert output_3x1.shape == (1, 1)
    assert np.allclose(output_1d, output_3x1)

    # Test case 9: Single measurement with rph parameter
    rph_single = [0.1, 0.05, 0.0]
    output_with_rph = ahrs_raw_hdg(magnetic_field_single_list, rph_single)
    assert output_with_rph.shape == (1, 1)

    # Test case 10: Multiple measurements with rph parameter
    rph_multi = np.array(
        [
            [0.1, 0.05, 0.0],
            [0.2, 0.1, 0.0],
            [0.0, 0.02, 0.0],
            [-0.1, 0.03, 0.0],
            [0.15, -0.05, 0.0],
        ]
    )
    output_multi_rph = ahrs_raw_hdg(magnetic_field_5x3, rph_multi)
    assert output_multi_rph.shape == (5, 1)

    # Error test cases
    # Test case 11: Invalid magnetic_field type
    with pytest.raises(TypeError) as excinfo:
        ahrs_raw_hdg("invalid_type")
    assert str(excinfo.value) == "magnetic_field must be a numpy array or list"

    # Test case 12: Invalid rph type
    with pytest.raises(TypeError) as excinfo:
        ahrs_raw_hdg(magnetic_field, "invalid_type")
    assert str(excinfo.value) == "rph must be a numpy array or list"

    # Test case 13: Invalid 1D magnetic_field shape
    with pytest.raises(ValueError) as excinfo:
        ahrs_raw_hdg(np.array([1, 2]))  # Only 2 elements
    assert (
        str(excinfo.value)
        == "For 1D arrays, magnetic_field must have exactly 3 elements"
    )

    # Test case 14: Invalid 2D magnetic_field shape
    with pytest.raises(ValueError) as excinfo:
        ahrs_raw_hdg(np.array([[1, 2], [3, 4]]))  # 2x2, neither 3xN nor Nx3
    assert str(excinfo.value) == "For 2D arrays, magnetic_field must be 3xN or Nx3"

    # Test case 15: Invalid rph shape (1D)
    with pytest.raises(ValueError) as excinfo:
        ahrs_raw_hdg(magnetic_field_single_list, [1, 2])  # Only 2 elements
    assert str(excinfo.value) == "For 1D arrays, rph must have exactly 3 elements"

    # Test case 16: Invalid rph shape (2D)
    with pytest.raises(ValueError) as excinfo:
        ahrs_raw_hdg(magnetic_field, np.array([[1, 2], [3, 4]]))  # 2x2
    assert str(excinfo.value) == "For 2D arrays, rph must be 3xN or Nx3"

    # Test case 17: Mismatched measurement counts
    with pytest.raises(ValueError) as excinfo:
        mag_2_samples = np.array([[1.0, 0.1, 0.3], [0.9, 0.2, 0.4]])
        rph_3_samples = np.array([[0.1, 0.0, 0.0], [0.2, 0.0, 0.0], [0.0, 0.1, 0.0]])
        ahrs_raw_hdg(mag_2_samples, rph_3_samples)
    assert (
        str(excinfo.value)
        == "magnetic_field and rph must have the same number of measurements"
    )

    # Test case 18: Invalid dimensions (3D array)
    with pytest.raises(ValueError) as excinfo:
        ahrs_raw_hdg(np.array([[[1, 2, 3]]]))
    assert (
        str(excinfo.value)
        == "magnetic_field must be a 1D array (3 elements) or 2D array (3xN or Nx3)"
    )

    # Test case 19: Mathematical correctness for known values
    # Test heading calculation for standard magnetic field vectors
    mag_north = [1.0, 0.0, 0.0]  # Pointing north
    hdg_north = ahrs_raw_hdg(mag_north)
    assert abs(hdg_north[0, 0]) < 1e-10  # Heading ≈ 0 for north pointing

    mag_east = [0.0, 1.0, 0.0]  # Pointing east
    hdg_east = ahrs_raw_hdg(mag_east)
    assert (
        abs(hdg_east[0, 0] - (-np.pi / 2)) < 1e-10
    )  # Heading ≈ -90° for east pointing


def test_ahrs_raw_rph():
    """
    Test the ahrs_raw_rph function with enhanced flexible input handling.
    """
    # Test case 1: Valid 2D input (Nx3)
    magnetic_field = np.array([[1.0, 0.1, 0.3], [0.8, 0.2, 0.4], [1.2, -0.1, 0.2]])
    accelerometer = np.array([[0.1, 0.2, 9.8], [0.0, 0.1, 9.7], [-0.1, 0.3, 9.9]])
    output = ahrs_raw_rph(magnetic_field, accelerometer)
    assert output.shape == (3, 3)
    assert isinstance(output, np.ndarray)

    # Test case 2: Valid 2D input with unambiguous shapes
    magnetic_field_5x3 = np.array(
        [
            [1.0, 0.1, 0.3],
            [0.8, 0.2, 0.4],
            [1.2, -0.1, 0.2],
            [0.9, 0.0, 0.5],
            [1.1, 0.3, 0.1],
        ]
    )
    accelerometer_5x3 = np.array(
        [
            [0.1, 0.2, 9.8],
            [0.0, 0.1, 9.7],
            [-0.1, 0.3, 9.9],
            [0.2, -0.1, 9.6],
            [0.05, 0.25, 9.8],
        ]
    )
    magnetic_field_3x5 = magnetic_field_5x3.T
    accelerometer_3x5 = accelerometer_5x3.T

    output_5x3 = ahrs_raw_rph(magnetic_field_5x3, accelerometer_5x3)
    output_3x5 = ahrs_raw_rph(magnetic_field_3x5, accelerometer_3x5)
    assert output_5x3.shape == (5, 3)
    assert output_3x5.shape == (5, 3)
    assert np.allclose(output_5x3, output_3x5, atol=1e-12)

    # Test case 3: List input (2D)
    magnetic_field_list = [[1.0, 0.1, 0.3], [0.8, 0.2, 0.4], [1.2, -0.1, 0.2]]
    accelerometer_list = [[0.1, 0.2, 9.8], [0.0, 0.1, 9.7], [-0.1, 0.3, 9.9]]
    output_list = ahrs_raw_rph(magnetic_field_list, accelerometer_list)
    assert output_list.shape == (3, 3)
    assert np.allclose(output, output_list)

    # Test case 4: Single measurement - 1D arrays
    magnetic_field_1d = np.array([1.0, 0.1, 0.3])
    accelerometer_1d = np.array([0.1, 0.2, 9.8])
    output_1d = ahrs_raw_rph(magnetic_field_1d, accelerometer_1d)
    assert output_1d.shape == (1, 3)
    assert isinstance(output_1d, np.ndarray)

    # Test case 5: Single measurement - lists
    magnetic_field_single_list = [1.0, 0.1, 0.3]
    accelerometer_single_list = [0.1, 0.2, 9.8]
    output_single_list = ahrs_raw_rph(
        magnetic_field_single_list, accelerometer_single_list
    )
    assert output_single_list.shape == (1, 3)
    assert np.allclose(output_1d, output_single_list)

    # Test case 6: Single measurement - 1x3 arrays
    magnetic_field_1x3 = np.array([[1.0, 0.1, 0.3]])
    accelerometer_1x3 = np.array([[0.1, 0.2, 9.8]])
    output_1x3 = ahrs_raw_rph(magnetic_field_1x3, accelerometer_1x3)
    assert output_1x3.shape == (1, 3)
    assert np.allclose(output_1d, output_1x3)

    # Test case 7: Single measurement - 3x1 arrays
    magnetic_field_3x1 = np.array([[1.0], [0.1], [0.3]])
    accelerometer_3x1 = np.array([[0.1], [0.2], [9.8]])
    output_3x1 = ahrs_raw_rph(magnetic_field_3x1, accelerometer_3x1)
    assert output_3x1.shape == (1, 3)
    assert np.allclose(output_1d, output_3x1)

    # Test case 8: Mixed format inputs
    output_mixed1 = ahrs_raw_rph(magnetic_field_1d, accelerometer_single_list)
    output_mixed2 = ahrs_raw_rph(magnetic_field_single_list, accelerometer_1d)
    output_mixed3 = ahrs_raw_rph(magnetic_field_1x3, accelerometer_3x1)
    assert np.allclose(output_1d, output_mixed1)
    assert np.allclose(output_1d, output_mixed2)
    assert np.allclose(output_1d, output_mixed3)

    # Test case 9: Consistency with individual functions
    rp_individual = ahrs_raw_rp(accelerometer_single_list)
    # hdg_individual = ahrs_raw_hdg(magnetic_field_single_list)
    rph_combined = ahrs_raw_rph(magnetic_field_single_list, accelerometer_single_list)

    # Roll and pitch should match between individual and combined functions
    assert np.allclose(rp_individual, rph_combined[:, :2], atol=1e-12)

    # Heading computation in rph uses roll/pitch correction, so it should match corrected heading
    rph_for_hdg = np.concatenate([rp_individual, np.zeros((1, 1))], axis=1)
    hdg_corrected = ahrs_raw_hdg(magnetic_field_single_list, rph_for_hdg)
    assert np.allclose(hdg_corrected, rph_combined[:, [2]], atol=1e-12)

    # Error test cases
    # Test case 10: Invalid magnetic_field type
    with pytest.raises(TypeError) as excinfo:
        ahrs_raw_rph("invalid_type", accelerometer_single_list)
    assert str(excinfo.value) == "magnetic_field must be a numpy array or list"

    # Test case 11: Invalid accelerometer type
    with pytest.raises(TypeError) as excinfo:
        ahrs_raw_rph(magnetic_field_single_list, "invalid_type")
    assert str(excinfo.value) == "accelerometer must be a numpy array or list"

    # Test case 12: Invalid 1D magnetic_field shape
    with pytest.raises(ValueError) as excinfo:
        ahrs_raw_rph([1, 2], accelerometer_single_list)  # Only 2 elements
    assert (
        str(excinfo.value)
        == "For 1D arrays, magnetic_field must have exactly 3 elements"
    )

    # Test case 13: Invalid 1D accelerometer shape
    with pytest.raises(ValueError) as excinfo:
        ahrs_raw_rph(magnetic_field_single_list, [1, 2])  # Only 2 elements
    assert (
        str(excinfo.value)
        == "For 1D arrays, accelerometer must have exactly 3 elements"
    )

    # Test case 14: Invalid 2D magnetic_field shape
    with pytest.raises(ValueError) as excinfo:
        ahrs_raw_rph([[1, 2], [3, 4]], accelerometer_single_list)  # 2x2
    assert str(excinfo.value) == "For 2D arrays, magnetic_field must be 3xN or Nx3"

    # Test case 15: Invalid 2D accelerometer shape
    with pytest.raises(ValueError) as excinfo:
        ahrs_raw_rph(magnetic_field_single_list, [[1, 2], [3, 4]])  # 2x2
    assert str(excinfo.value) == "For 2D arrays, accelerometer must be 3xN or Nx3"

    # Test case 16: Mismatched measurement counts
    with pytest.raises(ValueError) as excinfo:
        mag_2_samples = [[1.0, 0.1, 0.3], [0.8, 0.2, 0.4]]
        acc_3_samples = [[0.1, 0.2, 9.8], [0.0, 0.1, 9.7], [-0.1, 0.3, 9.9]]
        ahrs_raw_rph(mag_2_samples, acc_3_samples)
    assert (
        str(excinfo.value)
        == "magnetic_field and accelerometer must have the same number of measurements"
    )

    # Test case 17: Invalid dimensions (3D arrays)
    with pytest.raises(ValueError) as excinfo:
        ahrs_raw_rph([[[1, 2, 3]]], accelerometer_single_list)
    assert (
        str(excinfo.value)
        == "magnetic_field must be a 1D array (3 elements) or 2D array (3xN or Nx3)"
    )

    # Test case 18: Mathematical correctness for known values
    # Test with standard vectors (gravity sensor measures upward acceleration)
    gravity_up = [
        0.0,
        0.0,
        -9.81,
    ]  # Gravity pointing up (standard accelerometer reading)
    mag_north = [1.0, 0.0, 0.0]  # Magnetic field pointing north

    rph_standard = ahrs_raw_rph(mag_north, gravity_up)
    # Roll and pitch should be close to zero for gravity pointing up
    assert abs(rph_standard[0, 0]) < 1e-10  # Roll ≈ 0
    assert abs(rph_standard[0, 1]) < 1e-10  # Pitch ≈ 0
    assert abs(rph_standard[0, 2]) < 1e-10  # Heading ≈ 0 for north pointing


def test_ahrs_complementary_filter():
    """
    Test the ahrs_complementary_filter function.
    """
    # Test case 1: Valid input without magnetic_field
    angular_rate = np.array([[0.1, 0.2, 0.3], [0.1, 0.2, 0.3], [0.1, 0.2, 0.3]])
    acceleration = np.array([[0.0, 0.0, -9.81], [0.0, 0.0, -9.81], [0.0, 0.0, -9.81]])
    time = np.array([0.0, 0.1, 0.2])
    output = ahrs_complementary_filter(angular_rate, acceleration, time)
    assert output.shape == (3, 3)

    # Test case 2: Valid input with magnetic_field
    magnetic_field = np.array([[0.3, 0.4, 0.5], [0.3, 0.4, 0.5], [0.3, 0.4, 0.5]])
    output = ahrs_complementary_filter(angular_rate, acceleration, time, magnetic_field)
    assert output.shape == (3, 3)

    # Test case 3: List input
    angular_rate = [[0.1, 0.2, 0.3], [0.1, 0.2, 0.3], [0.1, 0.2, 0.3]]
    acceleration = [[0.0, 0.0, -9.81], [0.0, 0.0, -9.81], [0.0, 0.0, -9.81]]
    time = [0.0, 0.1, 0.2]
    magnetic_field = [[0.3, 0.4, 0.5], [0.3, 0.4, 0.5], [0.3, 0.4, 0.5]]
    output = ahrs_complementary_filter(angular_rate, acceleration, time, magnetic_field)
    assert output.shape == (3, 3)

    # Test case 4: Invalid angular_rate type
    try:
        ahrs_complementary_filter("invalid_type", acceleration, time)
    except TypeError as e:
        assert str(e) == "The angular rate must be a numpy array or a list."
    else:
        assert False

    # Test case 5: Invalid acceleration type
    try:
        ahrs_complementary_filter(angular_rate, "invalid_type", time)
    except TypeError as e:
        assert str(e) == "The acceleration must be a numpy array or a list."
    else:
        assert False

    # Test case 6: Invalid time type
    try:
        ahrs_complementary_filter(angular_rate, acceleration, "invalid_type")
    except TypeError as e:
        assert str(e) == "The time must be a numpy array or a list."
    else:
        assert False

    # Test case 7: Invalid magnetic_field type
    try:
        ahrs_complementary_filter(angular_rate, acceleration, time, "invalid_type")
    except TypeError as e:
        assert str(e) == "The magnetic field must be a numpy array or a list."
    else:
        assert False

    # Test case 8: Invalid angular_rate shape
    try:
        ahrs_complementary_filter(np.array([1, 2, 3, 4]), acceleration, time)
    except ValueError as e:
        assert str(e) == "The angular rate must be a 3xN or Nx3 numpy array."
    else:
        assert False

    # Test case 9: Invalid acceleration shape
    try:
        ahrs_complementary_filter(angular_rate, np.array([1, 2, 3, 4]), time)
    except ValueError as e:
        assert str(e) == "The acceleration must be a 3xN or Nx3 numpy array."
    else:
        assert False

    # Test case 10: Invalid time shape
    try:
        ahrs_complementary_filter(
            angular_rate, acceleration, np.array([[0.0, 0.1], [0.2, 0.3]])
        )
    except ValueError as e:
        assert str(e) == "The time must be a (n, ), (n, 1) or (1, n) numpy array."
    else:
        assert False

    # Test case 11: Invalid magnetic_field shape
    try:
        ahrs_complementary_filter(
            angular_rate, acceleration, time, np.array([1, 2, 3, 4])
        )
    except ValueError as e:
        assert str(e) == "The magnetic field must be a 3xN or Nx3 numpy array."
    else:
        assert False

    # Test case 12: Invalid initial_heading type
    try:
        ahrs_complementary_filter(
            angular_rate,
            acceleration,
            time,
            magnetic_field,
            rph0="invalid_type",
        )
    except TypeError as e:
        assert (
            str(e)
            == "The initial roll, pitch, and heading must be a numpy array or a list."
        )
    else:
        assert False

    # Test case 13: Invalid gain type
    try:
        ahrs_complementary_filter(
            angular_rate, acceleration, time, magnetic_field, gain="invalid_type"
        )
    except TypeError as e:
        assert str(e) == "The gain must be a float or integer."
    else:
        assert False

    # Test case 14: Invalid gain value
    try:
        ahrs_complementary_filter(
            angular_rate, acceleration, time, magnetic_field, gain=1.5
        )
    except ValueError as e:
        assert str(e) == "The gain must be between 0 and 1."
    else:
        assert False


def test_ahrs_mahony_filter():
    """
    Test the ahrs_mahony_filter function.
    """
    # Test case 1: Valid input
    angular_rate = np.array([[0.1, 0.2, 0.3], [0.1, 0.2, 0.3], [0.1, 0.2, 0.3]])
    acceleration = np.array([[0.0, 0.0, -9.81], [0.0, 0.0, -9.81], [0.0, 0.0, -9.81]])
    time = np.array([0.0, 0.1, 0.2])
    magnetic_field = np.array([[0.3, 0.4, 0.5], [0.3, 0.4, 0.5], [0.3, 0.4, 0.5]])
    reference_magnetic_field = np.array([0.3, 0.4, 0.5])
    rph0 = np.array([0.0, 0.0, 0.0])
    k1 = 1.0
    k2 = 1.0
    kp = 1.0
    ki = 1.0
    rph_filtered, gyro_biases = ahrs_mahony_filter(
        angular_rate,
        acceleration,
        time,
        magnetic_field,
        reference_magnetic_field,
        rph0,
        k1,
        k2,
        kp,
        ki,
    )
    assert rph_filtered.shape == (3, 3)
    assert gyro_biases.shape == (3, 3)

    # Test case 2: List input
    rph_filtered, gyro_biases = ahrs_mahony_filter(
        angular_rate.tolist(),
        acceleration.tolist(),
        time.tolist(),
        magnetic_field.tolist(),
        reference_magnetic_field.tolist(),
        rph0.tolist(),
        k1,
        k2,
        kp,
        ki,
    )
    assert rph_filtered.shape == (3, 3)
    assert gyro_biases.shape == (3, 3)

    # Test case 3: Invalid angular_rate type
    try:
        ahrs_mahony_filter(
            "invalid_type",
            acceleration,
            time,
            magnetic_field,
            reference_magnetic_field,
            rph0,
            k1,
            k2,
            kp,
            ki,
        )
    except TypeError as e:
        assert str(e) == "The angular rate must be a numpy array or a list."
    else:
        assert False

    # Test case 4: Invalid acceleration type
    try:
        ahrs_mahony_filter(
            angular_rate,
            "invalid_type",
            time,
            magnetic_field,
            reference_magnetic_field,
            rph0,
            k1,
            k2,
            kp,
            ki,
        )
    except TypeError as e:
        assert str(e) == "The acceleration must be a numpy array or a list."
    else:
        assert False

    # Test case 5: Invalid time type
    try:
        ahrs_mahony_filter(
            angular_rate,
            acceleration,
            "invalid_type",
            magnetic_field,
            reference_magnetic_field,
            rph0,
            k1,
            k2,
            kp,
            ki,
        )
    except TypeError as e:
        assert str(e) == "The time must be a numpy array or a list."
    else:
        assert False

    # Test case 6: Invalid magnetic_field type
    try:
        ahrs_mahony_filter(
            angular_rate,
            acceleration,
            time,
            "invalid_type",
            reference_magnetic_field,
            rph0,
            k1,
            k2,
            kp,
            ki,
        )
    except TypeError as e:
        assert str(e) == "The magnetic field must be a numpy array or a list."
    else:
        assert False

    # Test case 7: Invalid reference_magnetic_field type
    try:
        ahrs_mahony_filter(
            angular_rate,
            acceleration,
            time,
            magnetic_field,
            "invalid_type",
            rph0,
            k1,
            k2,
            kp,
            ki,
        )
    except TypeError as e:
        assert str(e) == "The reference magnetic field must be a numpy array or a list."
    else:
        assert False

    # Test case 8: Invalid angular_rate shape
    try:
        ahrs_mahony_filter(
            np.array([1, 2, 3, 4]),
            acceleration,
            time,
            magnetic_field,
            reference_magnetic_field,
            rph0,
            k1,
            k2,
            kp,
            ki,
        )
    except ValueError as e:
        assert str(e) == "The angular rate must be a 3xN or Nx3 numpy array."
    else:
        assert False

    # Test case 9: Invalid acceleration shape
    try:
        ahrs_mahony_filter(
            angular_rate,
            np.array([1, 2, 3, 4]),
            time,
            magnetic_field,
            reference_magnetic_field,
            rph0,
            k1,
            k2,
            kp,
            ki,
        )
    except ValueError as e:
        assert str(e) == "The acceleration must be a 3xN or Nx3 numpy array."
    else:
        assert False

    # Test case 10: Invalid time shape
    try:
        ahrs_mahony_filter(
            angular_rate,
            acceleration,
            np.array([[0, 1, 2], [0, 1, 2]]),
            magnetic_field,
            reference_magnetic_field,
            rph0,
            k1,
            k2,
            kp,
            ki,
        )
    except ValueError as e:
        assert str(e) == "The time must be a (n, ), (n, 1) or (1, n) numpy array."
    else:
        assert False

    # Test case 11: Invalid magnetic_field shape
    try:
        ahrs_mahony_filter(
            angular_rate,
            acceleration,
            time,
            np.array([1, 2, 3, 4]),
            reference_magnetic_field,
            rph0,
            k1,
            k2,
            kp,
            ki,
        )
    except ValueError as e:
        assert str(e) == "The magnetic field must be a 3xN or Nx3 numpy array."
    else:
        assert False

    # Test case 12: Invalid reference_magnetic_field shape
    try:
        ahrs_mahony_filter(
            angular_rate,
            acceleration,
            time,
            magnetic_field,
            np.array([1, 2, 3, 4]),
            rph0,
            k1,
            k2,
            kp,
            ki,
        )
    except ValueError as e:
        assert (
            str(e)
            == "The reference magnetic field must be a 1D numpy array with 3 elements."
        )
    else:
        assert False

    # Test case 13: Invalid rph0 type
    try:
        ahrs_mahony_filter(
            angular_rate,
            acceleration,
            time,
            magnetic_field,
            reference_magnetic_field,
            "invalid_type",
            k1,
            k2,
            kp,
            ki,
        )
    except TypeError as e:
        assert (
            str(e)
            == "The initial roll, pitch, and heading must be a numpy array or a list."
        )
    else:
        assert False

    # Test case 13: Invalid k1 type
    try:
        ahrs_mahony_filter(
            angular_rate,
            acceleration,
            time,
            magnetic_field,
            reference_magnetic_field,
            rph0,
            "invalid_type",
            k2,
            kp,
            ki,
        )
    except TypeError as e:
        assert str(e) == "The k1 gain must be a float or integer."
    else:
        assert False

    # Test case 14: Invalid k2 type
    try:
        ahrs_mahony_filter(
            angular_rate,
            acceleration,
            time,
            magnetic_field,
            reference_magnetic_field,
            rph0,
            k1,
            "invalid_type",
            kp,
            ki,
        )
    except TypeError as e:
        assert str(e) == "The k2 gain must be a float or integer."
    else:
        assert False

    # Test case 15: Invalid kp type
    try:
        ahrs_mahony_filter(
            angular_rate,
            acceleration,
            time,
            magnetic_field,
            reference_magnetic_field,
            rph0,
            k1,
            k2,
            "invalid_type",
            ki,
        )
    except TypeError as e:
        assert str(e) == "The kp gain must be a float or integer."
    else:
        assert False

    # Test case 16: Invalid ki type
    try:
        ahrs_mahony_filter(
            angular_rate,
            acceleration,
            time,
            magnetic_field,
            reference_magnetic_field,
            rph0,
            k1,
            k2,
            kp,
            "invalid_type",
        )
    except TypeError as e:
        assert str(e) == "The ki gain must be a float or integer."
    else:
        assert False

    # Test case 17: Magnetic field provided but no reference
    try:
        ahrs_mahony_filter(
            angular_rate,
            acceleration,
            time,
            magnetic_field,
            None,
            rph0,
            k1,
            k2,
            kp,
            ki,
        )
    except ValueError as e:
        assert (
            str(e)
            == "If magnetic field is provided, a reference magnetic field is required."
        )


def test_ahrs_hua_mahony_filter():
    """
    Test the ahrs_hua_mahony_filter function.
    """
    # Test case 1: Valid input without magnetic_field and reference_magnetic_field
    angular_rate = np.array([[0.1, 0.2, 0.3], [0.1, 0.2, 0.3], [0.1, 0.2, 0.3]])
    acceleration = np.array([[0.0, 0.0, -9.81], [0.0, 0.0, -9.81], [0.0, 0.0, -9.81]])
    time = np.array([0.0, 0.1, 0.2])
    output, gyro_biases = ahrs_hua_filter(angular_rate, acceleration, time)
    assert output.shape == (3, 3)
    assert gyro_biases.shape == (3, 3)

    # Test case 2: Valid input with magnetic_field and reference_magnetic_field
    magnetic_field = np.array([[0.3, 0.4, 0.5], [0.3, 0.4, 0.5], [0.3, 0.4, 0.5]])
    reference_magnetic_field = np.array([0.3, 0.4, 0.5])
    output, gyro_biases = ahrs_hua_filter(
        angular_rate, acceleration, time, magnetic_field, reference_magnetic_field
    )
    assert output.shape == (3, 3)
    assert gyro_biases.shape == (3, 3)

    # Test case 3: List input
    angular_rate = [[0.1, 0.2, 0.3], [0.1, 0.2, 0.3], [0.1, 0.2, 0.3]]
    acceleration = [[0.0, 0.0, -9.81], [0.0, 0.0, -9.81], [0.0, 0.0, -9.81]]
    time = [0.0, 0.1, 0.2]
    magnetic_field = [[0.3, 0.4, 0.5], [0.3, 0.4, 0.5], [0.3, 0.4, 0.5]]
    reference_magnetic_field = [0.3, 0.4, 0.5]
    output, gyro_biases = ahrs_hua_filter(
        angular_rate, acceleration, time, magnetic_field, reference_magnetic_field
    )
    assert output.shape == (3, 3)
    assert gyro_biases.shape == (3, 3)

    # Test case 4: Invalid angular_rate type
    try:
        ahrs_hua_filter("invalid_type", acceleration, time)
    except TypeError as e:
        assert str(e) == "The angular rate must be a numpy array or a list."
    else:
        assert False

    # Test case 5: Invalid acceleration type
    try:
        ahrs_hua_filter(angular_rate, "invalid_type", time)
    except TypeError as e:
        assert str(e) == "The acceleration must be a numpy array or a list."
    else:
        assert False

    # Test case 6: Invalid time type
    try:
        ahrs_hua_filter(angular_rate, acceleration, "invalid_type")
    except TypeError as e:
        assert str(e) == "The time must be a numpy array or a list."
    else:
        assert False

    # Test case 7: Invalid magnetic_field type
    try:
        ahrs_hua_filter(angular_rate, acceleration, time, "invalid_type")
    except TypeError as e:
        assert str(e) == "The magnetic field must be a numpy array or a list."
    else:
        assert False

    # Test case 8: Invalid reference_magnetic_field type
    try:
        ahrs_hua_filter(
            angular_rate, acceleration, time, magnetic_field, "invalid_type"
        )
    except TypeError as e:
        assert str(e) == "The reference magnetic field must be a numpy array or a list."
    else:
        assert False

    # Test case 9: Invalid rph0 type
    try:
        ahrs_hua_filter(
            angular_rate,
            acceleration,
            time,
            magnetic_field,
            reference_magnetic_field,
            "invalid_type",
        )
    except TypeError as e:
        assert (
            str(e)
            == "The initial roll, pitch, and heading must be a numpy array or a list."
        )
    else:
        assert False

    # Test case 10: Invalid angular_rate shape
    try:
        ahrs_hua_filter(np.array([1, 2, 3, 4]), acceleration, time)
    except ValueError as e:
        assert str(e) == "The angular rate must be a 3xN or Nx3 numpy array."
    else:
        assert False

    # Test case 11: Invalid acceleration shape
    try:
        ahrs_hua_filter(angular_rate, np.array([1, 2, 3, 4]), time)
    except ValueError as e:
        assert str(e) == "The acceleration must be a 3xN or Nx3 numpy array."
    else:
        assert False

    # Test case 12: Invalid time shape
    try:
        ahrs_hua_filter(angular_rate, acceleration, np.array([[0.0, 0.1], [0.2, 0.3]]))
    except ValueError as e:
        assert str(e) == "The time must be a (n, ), (n, 1) or (1, n) numpy array."
    else:
        assert False

    # Test case 13: Invalid magnetic_field shape
    try:
        ahrs_hua_filter(angular_rate, acceleration, time, np.array([1, 2, 3, 4]))
    except ValueError as e:
        assert str(e) == "The magnetic field must be a 3xN or Nx3 numpy array."
    else:
        assert False

    # Test case 14: Invalid reference_magnetic_field shape
    try:
        ahrs_hua_filter(
            angular_rate, acceleration, time, magnetic_field, np.array([1, 2, 3, 4])
        )
    except ValueError as e:
        assert (
            str(e)
            == "The reference magnetic field must be a 1D numpy array with 3 elements."
        )
    else:
        assert False

    # Test case 15: Invalid rph0 shape
    try:
        ahrs_hua_filter(
            angular_rate,
            acceleration,
            time,
            magnetic_field,
            reference_magnetic_field,
            np.array([1, 2, 3, 4]),
        )
    except ValueError as e:
        assert (
            str(e)
            == "The initial roll, pitch, and heading must be a 1D numpy array with 3 elements."
        )
    else:
        assert False

    # Test case 16: Invalid k1 type
    try:
        ahrs_hua_filter(
            angular_rate,
            acceleration,
            time,
            magnetic_field,
            reference_magnetic_field,
            k1="invalid_type",
        )
    except TypeError as e:
        assert str(e) == "The k1 gain must be a float or integer."
    else:
        assert False

    # Test case 17: Invalid k2 type
    try:
        ahrs_hua_filter(
            angular_rate,
            acceleration,
            time,
            magnetic_field,
            reference_magnetic_field,
            k2="invalid_type",
        )
    except TypeError as e:
        assert str(e) == "The k2 gain must be a float or integer."
    else:
        assert False

    # Test case 18: Invalid k3 type
    try:
        ahrs_hua_filter(
            angular_rate,
            acceleration,
            time,
            magnetic_field,
            reference_magnetic_field,
            k3="invalid_type",
        )
    except TypeError as e:
        assert str(e) == "The k3 gain must be a float or integer."
    else:
        assert False

    # Test case 19: Invalid k4 type
    try:
        ahrs_hua_filter(
            angular_rate,
            acceleration,
            time,
            magnetic_field,
            reference_magnetic_field,
            k4="invalid_type",
        )
    except TypeError as e:
        assert str(e) == "The k4 gain must be a float or integer."
    else:
        assert False

    # Test case 20: Invalid kb type
    try:
        ahrs_hua_filter(
            angular_rate,
            acceleration,
            time,
            magnetic_field,
            reference_magnetic_field,
            kb="invalid_type",
        )
    except TypeError as e:
        assert str(e) == "The kb gain must be a float or integer."
    else:
        assert False

    # Test case 21: Invalid delta type
    try:
        ahrs_hua_filter(
            angular_rate,
            acceleration,
            time,
            magnetic_field,
            reference_magnetic_field,
            delta="invalid_type",
        )
    except TypeError as e:
        assert str(e) == "The delta gain must be a float or integer."
    else:
        assert False

    # Test case 22: Magnetic field provided but no reference
    try:
        ahrs_hua_filter(
            angular_rate,
            acceleration,
            time,
            magnetic_field,
            None,
        )
    except ValueError as e:
        assert (
            str(e)
            == "If magnetic field is provided, a reference magnetic field is required."
        )
    else:
        assert False


def test_ahrs_hua_filter_time_jump_handling():
    """
    Test the enhanced time jump handling in ahrs_hua_filter function.
    Tests the new parameters max_dt_factor_s and max_dt_reinit_s and their behavior.
    """
    # Basic test data
    angular_rate_base = np.array(
        [[0.01, 0.02, 0.03], [0.01, 0.02, 0.03], [0.01, 0.02, 0.03]]
    )
    acceleration_base = np.array(
        [[0.0, 0.0, -9.81], [0.0, 0.0, -9.81], [0.0, 0.0, -9.81]]
    )
    magnetic_field_base = np.array([[0.3, 0.4, 0.5], [0.3, 0.4, 0.5], [0.3, 0.4, 0.5]])
    reference_magnetic_field = np.array([0.3, 0.4, 0.5])

    # Test case 1: Normal operation with regular time steps
    time_normal = np.array([0.0, 0.1, 0.2])
    output_normal, gyro_biases_normal = ahrs_hua_filter(
        angular_rate_base,
        acceleration_base,
        time_normal,
        magnetic_field_base,
        reference_magnetic_field,
    )
    assert output_normal.shape == (3, 3)
    assert gyro_biases_normal.shape == (3, 3)

    # Test case 2: Negative time step (non-increasing time) - should hold state
    time_negative = np.array([0.0, 0.1, 0.05])  # Time goes backwards
    angular_rate_neg = np.array([[0.0, 0.0, 0.0], [0.1, 0.2, 0.3], [0.5, 0.6, 0.7]])
    acceleration_neg = np.array([[0.0, 0.0, -9.81], [0.1, 0.0, -9.8], [0.2, 0.0, -9.7]])
    magnetic_field_neg = np.array([[0.3, 0.4, 0.5], [0.4, 0.5, 0.6], [0.5, 0.6, 0.7]])

    output_neg, gyro_corrected_neg = ahrs_hua_filter(
        angular_rate_neg,
        acceleration_neg,
        time_negative,
        magnetic_field_neg,
        reference_magnetic_field,
    )

    # State at index 2 should be held (same as index 1) due to negative time step
    assert np.allclose(output_neg[2], output_neg[1], atol=1e-12)

    # The difference in returned "corrected" angular rates should equal the difference in input angular rates
    # because internal biases are held constant: (w2 - bias_held) - (w1 - bias_held) = w2 - w1
    expected_diff = angular_rate_neg[2] - angular_rate_neg[1]  # [0.4, 0.4, 0.4]
    actual_diff = gyro_corrected_neg[2] - gyro_corrected_neg[1]
    assert np.allclose(actual_diff, expected_diff, atol=1e-12)

    # Test case 3: Zero time step - should hold state
    time_zero = np.array([0.0, 0.1, 0.1])  # Same time as previous
    output_zero, gyro_corrected_zero = ahrs_hua_filter(
        angular_rate_neg,
        acceleration_neg,
        time_zero,
        magnetic_field_neg,
        reference_magnetic_field,
    )

    # State at index 2 should be held (same as index 1) due to zero time step
    assert np.allclose(output_zero[2], output_zero[1], atol=1e-12)
    # Same logic for corrected angular rates
    expected_diff_zero = angular_rate_neg[2] - angular_rate_neg[1]
    actual_diff_zero = gyro_corrected_zero[2] - gyro_corrected_zero[1]
    assert np.allclose(actual_diff_zero, expected_diff_zero, atol=1e-12)

    # Test case 4: Medium time gap (soft reset) - should hold state
    # We need to ensure the gap is larger than max_dt_factor_s * nominal_dt
    # With times [0.0, 0.1, 3.0], dt_array = [0.1, 2.9], nominal_dt = (0.1 + 2.9) / 2 = 1.5 (for two values, median is their average)
    # max_dt_soft = 1.0 * 1.5 = 1.5, so gap of 2.9 > 1.5 should trigger soft reset
    time_medium = np.array([0.0, 0.1, 3.0])  # Gap of 2.9s
    angular_rate_med = np.array([[0.0, 0.0, 0.0], [0.1, 0.2, 0.3], [0.5, 0.6, 0.7]])
    acceleration_med = np.array([[0.0, 0.0, -9.81], [0.1, 0.0, -9.8], [0.2, 0.0, -9.7]])
    magnetic_field_med = np.array([[0.3, 0.4, 0.5], [0.4, 0.5, 0.6], [0.5, 0.6, 0.7]])

    output_med, gyro_corrected_med = ahrs_hua_filter(
        angular_rate_med,
        acceleration_med,
        time_medium,
        magnetic_field_med,
        reference_magnetic_field,
        max_dt_factor_s=1.0,  # Lower threshold to ensure soft reset triggers
    )

    # State should be held due to medium time gap
    assert np.allclose(output_med[2], output_med[1], atol=1e-12)
    # Same logic for corrected angular rates - internal biases held constant
    expected_diff_med = angular_rate_med[2] - angular_rate_med[1]
    actual_diff_med = gyro_corrected_med[2] - gyro_corrected_med[1]
    assert np.allclose(actual_diff_med, expected_diff_med, atol=1e-12)

    # Test case 5: Very long time gap (hard reset) - should reinitialize
    time_long = np.array([0.0, 0.1, 70.0])  # Gap of 69.9s > 60.0s
    angular_rate_long = np.array([[0.0, 0.0, 0.0], [0.1, 0.2, 0.3], [0.5, 0.6, 0.7]])
    acceleration_long = np.array(
        [[0.0, 0.0, -9.81], [0.1, 0.0, -9.8], [0.2, 0.1, -9.7]]
    )
    magnetic_field_long = np.array([[0.3, 0.4, 0.5], [0.4, 0.5, 0.6], [0.6, 0.7, 0.8]])

    output_long, gyro_corrected_long = ahrs_hua_filter(
        angular_rate_long,
        acceleration_long,
        time_long,
        magnetic_field_long,
        reference_magnetic_field,
        max_dt_reinit_s=60.0,
    )

    # Test that state is reinitialized - it should be very different from held state
    # Compare with a case where the state would be held
    time_held_comparison = np.array([0.0, 0.1, 0.2])  # No time jump
    output_held, _ = ahrs_hua_filter(
        angular_rate_long,
        acceleration_long,
        time_held_comparison,
        magnetic_field_long,
        reference_magnetic_field,
    )
    # The attitude after reinit should be significantly different from normal progression
    attitude_diff = np.linalg.norm(output_long[2] - output_held[2])
    assert attitude_diff > 0.1  # Should be substantially different

    # Test case 6: Error case - all time differences are <= 0
    time_all_negative = np.array([1.0, 0.5, 0.0])

    with pytest.raises(ValueError) as excinfo:
        ahrs_hua_filter(
            angular_rate_base,
            acceleration_base,
            time_all_negative,
            magnetic_field_base,
            reference_magnetic_field,
        )
    assert (
        str(excinfo.value)
        == "Cannot infer sampling period: all time differences are <= 0."
    )

    # Test case 7: Validate max_dt_factor_s parameter type checking
    with pytest.raises(TypeError) as excinfo:
        ahrs_hua_filter(
            angular_rate_base,
            acceleration_base,
            time_normal,
            magnetic_field_base,
            reference_magnetic_field,
            max_dt_factor_s="invalid",
        )
    assert str(excinfo.value) == "The max_dt_factor_s must be a float or integer."

    # Test case 8: Validate max_dt_reinit_s parameter type checking
    with pytest.raises(TypeError) as excinfo:
        ahrs_hua_filter(
            angular_rate_base,
            acceleration_base,
            time_normal,
            magnetic_field_base,
            reference_magnetic_field,
            max_dt_reinit_s="invalid",
        )
    assert str(excinfo.value) == "The max_dt_reinit_s must be a float or integer."

    # Test case 9: Custom time jump parameters
    # Test that we can control the soft reset threshold
    time_custom = np.array(
        [0.0, 0.1, 0.25]
    )  # dt_array = [0.1, 0.15], nominal_dt = 0.125
    # With max_dt_factor_s=1.0, max_dt_soft = 1.0 * 0.125 = 0.125
    # Gap of 0.15 > 0.125, so should trigger soft reset
    output_custom_soft, _ = ahrs_hua_filter(
        angular_rate_med,
        acceleration_med,
        time_custom,
        magnetic_field_med,
        reference_magnetic_field,
        max_dt_factor_s=1.0,
    )

    # Should hold state due to soft reset
    assert np.allclose(output_custom_soft[2], output_custom_soft[1], atol=1e-12)

    # Test case 10: Custom reinit parameter with smaller threshold
    time_custom_reinit = np.array([0.0, 0.1, 30.0])  # Gap of 29.9s
    output_custom_reinit, _ = ahrs_hua_filter(
        angular_rate_long,
        acceleration_long,
        time_custom_reinit,
        magnetic_field_long,
        reference_magnetic_field,
        max_dt_reinit_s=25.0,  # Gap > 25.0s should trigger hard reset
    )

    # Should reinitialize due to smaller max_dt_reinit_s
    # Compare with normal progression to verify reinit occurred
    time_custom_normal = np.array([0.0, 0.1, 0.2])
    output_custom_normal, _ = ahrs_hua_filter(
        angular_rate_long,
        acceleration_long,
        time_custom_normal,
        magnetic_field_long,
        reference_magnetic_field,
    )
    attitude_diff_custom = np.linalg.norm(
        output_custom_reinit[2] - output_custom_normal[2]
    )
    assert attitude_diff_custom > 0.1  # Should be substantially different due to reinit


def test_ahrs_madgwick_filter():
    """
    Test the ahrs_madgwick_filter function.
    """
    # Test case 1: Valid input without magnetic field
    angular_rate = np.array([[0.1, 0.2, 0.3], [0.1, 0.2, 0.3], [0.1, 0.2, 0.3]])
    acceleration = np.array([[0.0, 0.0, -9.81], [0.0, 0.0, -9.81], [0.0, 0.0, -9.81]])
    time = np.array([0.0, 0.1, 0.2])
    rph, angrate, flags = ahrs_madgwick_filter(angular_rate, acceleration, time)
    assert rph.shape == (3, 3)
    assert angrate.shape == (3, 3)
    # Flags should be a dictionary with each key containing an array of length 3
    for key in flags:
        assert flags[key].shape[0] == 3

    # Test case 2: Valid input with magnetic_field provided
    magnetic_field = np.array([[0.3, 0.4, 0.5], [0.3, 0.4, 0.5], [0.3, 0.4, 0.5]])
    rph, angrate, flags = ahrs_madgwick_filter(
        angular_rate, acceleration, time, magnetic_field
    )
    assert rph.shape == (3, 3)
    assert angrate.shape == (3, 3)
    for key in flags:
        assert flags[key].shape[0] == 3

    # Test case 3: List input for all data
    angular_rate_list = [[0.1, 0.2, 0.3], [0.1, 0.2, 0.3], [0.1, 0.2, 0.3]]
    acceleration_list = [[0.0, 0.0, -9.81], [0.0, 0.0, -9.81], [0.0, 0.0, -9.81]]
    time_list = [0.0, 0.1, 0.2]
    magnetic_field_list = [[0.3, 0.4, 0.5], [0.3, 0.4, 0.5], [0.3, 0.4, 0.5]]
    rph, angrate, flags = ahrs_madgwick_filter(
        angular_rate_list, acceleration_list, time_list, magnetic_field_list
    )
    assert rph.shape == (3, 3)
    assert angrate.shape == (3, 3)
    for key in flags:
        assert flags[key].shape[0] == 3

    # Test case 4: Invalid angular_rate type
    with pytest.raises(TypeError) as excinfo:
        ahrs_madgwick_filter("invalid", acceleration, time)
    assert str(excinfo.value) == "The angular rate must be a numpy array or a list."

    # Test case 5: Invalid acceleration type
    with pytest.raises(TypeError) as excinfo:
        ahrs_madgwick_filter(angular_rate, "invalid", time)
    assert str(excinfo.value) == "The acceleration must be a numpy array or a list."

    # Test case 6: Invalid time type
    with pytest.raises(TypeError) as excinfo:
        ahrs_madgwick_filter(angular_rate, acceleration, "invalid")
    assert str(excinfo.value) == "The time must be a numpy array or a list."

    # Test case 7: Invalid magnetic_field type, if provided
    with pytest.raises(TypeError) as excinfo:
        ahrs_madgwick_filter(angular_rate, acceleration, time, "invalid")
    assert str(excinfo.value) == "The magnetic field must be a numpy array or a list."

    # Test case 8: Invalid rph0 type, if provided
    with pytest.raises(TypeError) as excinfo:
        ahrs_madgwick_filter(
            angular_rate, acceleration, time, magnetic_field, "invalid"
        )
    assert (
        str(excinfo.value)
        == "The initial roll, pitch, and heading must be a numpy array or a list."
    )

    # Test case 9: Invalid gain type, if provided
    with pytest.raises(TypeError) as excinfo:
        ahrs_madgwick_filter(
            angular_rate, acceleration, time, magnetic_field, gain="invalid"
        )
    assert str(excinfo.value) == "The gain must be a float or integer."

    with pytest.raises(ValueError) as excinfo:
        ahrs_madgwick_filter(angular_rate, acceleration, time, magnetic_field, gain=1.5)
    assert str(excinfo.value) == "The gain must be between 0 and 1."

    # Test case 10: Invalid type for gyro_range, if provided
    with pytest.raises(TypeError) as excinfo:
        ahrs_madgwick_filter(
            angular_rate, acceleration, time, magnetic_field, gyro_range="invalid"
        )
    assert str(excinfo.value) == "The gyro range must be a float or integer."
    with pytest.raises(ValueError) as excinfo:
        ahrs_madgwick_filter(
            angular_rate, acceleration, time, magnetic_field, gyro_range=0
        )
    assert str(excinfo.value) == "The gyro range must be greater than 0."
    with pytest.raises(ValueError) as excinfo:
        ahrs_madgwick_filter(
            angular_rate, acceleration, time, magnetic_field, gyro_range=-1
        )
    assert str(excinfo.value) == "The gyro range must be greater than 0."

    # Test case 11: Invalid type for accel_rejection, if provided
    with pytest.raises(TypeError) as excinfo:
        ahrs_madgwick_filter(
            angular_rate, acceleration, time, magnetic_field, accel_rejection="invalid"
        )
    assert str(excinfo.value) == "The accel rejection must be a float or integer."
    with pytest.raises(ValueError) as excinfo:
        ahrs_madgwick_filter(
            angular_rate, acceleration, time, magnetic_field, accel_rejection=-1
        )
    assert (
        str(excinfo.value) == "The accel rejection must be greater than or equal to 0."
    )

    # Test case 12: Invalid type for magnetic_rejection, if provided
    with pytest.raises(TypeError) as excinfo:
        ahrs_madgwick_filter(
            angular_rate,
            acceleration,
            time,
            magnetic_field,
            magnetic_rejection="invalid",
        )
    assert str(excinfo.value) == "The magnetic rejection must be a float or integer."
    with pytest.raises(ValueError) as excinfo:
        ahrs_madgwick_filter(
            angular_rate, acceleration, time, magnetic_field, magnetic_rejection=-1
        )
    assert (
        str(excinfo.value)
        == "The magnetic rejection must be greater than or equal to 0."
    )

    # Test case 13: Invalid type for recovery trigger, if provided
    with pytest.raises(TypeError) as excinfo:
        ahrs_madgwick_filter(
            angular_rate, acceleration, time, magnetic_field, recovery_trigger="invalid"
        )
    assert str(excinfo.value) == "The recovery trigger must be a float or integer."
    with pytest.raises(ValueError) as excinfo:
        ahrs_madgwick_filter(
            angular_rate, acceleration, time, magnetic_field, recovery_trigger=-1
        )
    assert (
        str(excinfo.value) == "The recovery trigger must be greater than or equal to 0."
    )

    # Test case 14: Invalid dimensions for acceleration (e.g. not 3xN or Nx3)
    with pytest.raises(ValueError) as excinfo:
        ahrs_madgwick_filter(angular_rate, np.array([1, 2, 3, 4]), time)
    assert str(excinfo.value) == "The acceleration must be a 3xN or Nx3 numpy array."

    # Test case 15: Invalid dimensions for angular_rate
    with pytest.raises(ValueError) as excinfo:
        ahrs_madgwick_filter(np.array([1, 2, 3, 4]), acceleration, time)
    assert str(excinfo.value) == "The angular rate must be a 3xN or Nx3 numpy array."

    # Test case 16: Invalid time shape
    with pytest.raises(ValueError) as excinfo:
        ahrs_madgwick_filter(
            angular_rate, acceleration, np.array([[0.0, 0.1], [0.2, 0.3]])
        )
    assert (
        str(excinfo.value) == "The time must be a (n, ), (n, 1) or (1, n) numpy array."
    )

    # Test case 17: Invalid magnetic_field shape
    with pytest.raises(ValueError) as excinfo:
        ahrs_madgwick_filter(angular_rate, acceleration, time, np.array([1, 2, 3, 4]))
    assert str(excinfo.value) == "The magnetic field must be a 3xN or Nx3 numpy array."

    # Test case 18: Invalid rph0 shape
    with pytest.raises(ValueError) as excinfo:
        ahrs_madgwick_filter(
            angular_rate, acceleration, time, magnetic_field, np.array([1, 2, 3, 4])
        )
    assert (
        str(excinfo.value)
        == "The initial roll, pitch, and heading must be a 1D numpy array with 3 elements."
    )
    with pytest.raises(ValueError) as excinfo:
        ahrs_madgwick_filter(
            angular_rate, acceleration, time, magnetic_field, np.array([[1, 2], [3, 4]])
        )
    assert (
        str(excinfo.value)
        == "The initial roll, pitch, and heading must be a 1D numpy array with 3 elements."
    )
