import numpy as np

from navlib import navdvl_dead_reckoning, navdvl_kf
from navlib.nav import navdvl_dead_reckoning as navdvl_dead_reckoning_relative
from navlib.nav import navdvl_kf as navdvl_kf_relative


def test_imports():
    # Checks that the absolute and relative imports work correctly
    assert (
        navdvl_dead_reckoning_relative.__code__.co_code
        == navdvl_dead_reckoning.__code__.co_code
    )
    assert navdvl_kf_relative.__code__.co_code == navdvl_kf.__code__.co_code


def test_navdvl_dead_reckoning():
    """
    Test the navdvl_dead_reckoning function.
    """
    # Test case 1: Valid input with time vector
    velocity = np.array([[1, 0, 0], [1, 0, 0], [1, 0, 0]])
    rph = np.array([[0, 0, 0], [0, 0, 0], [0, 0, 0]])
    time = np.array([0, 1, 2])
    initial_state = np.array([0, 0, 0])
    state = navdvl_dead_reckoning(velocity, rph, time=time, initial_state=initial_state)
    assert state.shape == (3, 10)
    # Check positions
    assert np.allclose(state[:, 1:4], np.array([[0, 0, 0], [1, 0, 0], [2, 0, 0]]))
    # Check rph
    assert np.allclose(state[:, 4:7], rph)
    # Check velocities
    assert np.allclose(state[:, 7:10], velocity)

    # Test case 2: Valid input with fixed time step
    dt = 1
    state = navdvl_dead_reckoning(velocity, rph, dt=dt, initial_state=initial_state)
    assert state.shape == (3, 10)
    assert np.allclose(state[:, 1:4], np.array([[0, 0, 0], [1, 0, 0], [2, 0, 0]]))
    assert np.allclose(state[:, 4:7], rph)
    assert np.allclose(state[:, 7:10], velocity)

    # Test case 3: Invalid velocity type
    try:
        navdvl_dead_reckoning("invalid_type", rph, time=time)
    except TypeError as e:
        assert str(e) == "Velocity must be a numpy array"
    else:
        assert False

    # Test case 4: Invalid rph type
    try:
        navdvl_dead_reckoning(velocity, "invalid_type", time=time)
    except TypeError as e:
        assert str(e) == "RPH must be a numpy array"
    else:
        assert False

    # Test case 5: Invalid time type
    try:
        navdvl_dead_reckoning(velocity, rph, time="invalid_type")
    except TypeError as e:
        assert str(e) == "Time must be a numpy array"
    else:
        assert False

    # Test case 6: Invalid initial_state type
    try:
        navdvl_dead_reckoning(velocity, rph, time=time, initial_state="invalid_type")
    except TypeError as e:
        assert str(e) == "Initial state must be a numpy array"
    else:
        assert False

    # Test case 7: Invalid dt type
    try:
        navdvl_dead_reckoning(velocity, rph, dt="invalid_type")
    except TypeError as e:
        assert str(e) == "dt must be a float or an integer"
    else:
        assert False

    # Test case 8: dt and time used together
    try:
        navdvl_dead_reckoning(velocity, rph, time=time, dt=dt)
    except ValueError as e:
        assert str(e) == "dt and time cannot be used together"
    else:
        assert False

    # Test case 9: dt less than or equal to zero
    try:
        navdvl_dead_reckoning(velocity, rph, dt=0)
    except ValueError as e:
        assert str(e) == "dt must be greater than zero"
    else:
        assert False

    # Test case 10: Invalid velocity_frame type
    try:
        navdvl_dead_reckoning(velocity, rph, time=time, velocity_frame=123)
    except TypeError as e:
        assert str(e) == "velocity_frame must be a string"
    else:
        assert False

    # Test case 11: Invalid velocity_frame value
    try:
        navdvl_dead_reckoning(velocity, rph, time=time, velocity_frame="invalid_frame")
    except ValueError as e:
        assert str(e) == "velocity_frame must be either 'body' or 'world'"
    else:
        assert False

    # Test case 12: Invalid velocity shape
    try:
        navdvl_dead_reckoning(np.array([1, 2, 3]), rph, time=time)
    except ValueError as e:
        assert str(e) == "Velocity must be a matrix with 3 columns"
    else:
        assert False

    # Test case 13: Invalid rph shape
    try:
        navdvl_dead_reckoning(velocity, np.array([1, 2, 3]), time=time)
    except ValueError as e:
        assert str(e) == "RPH must be a matrix with 3 columns"
    else:
        assert False

    # Test case 14: Invalid time shape
    try:
        navdvl_dead_reckoning(velocity, rph, time=np.array([[1, 2], [3, 4]]))
    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 15: Invalid initial_state shape
    try:
        navdvl_dead_reckoning(
            velocity, rph, time=time, initial_state=np.array([[1, 2], [3, 4]])
        )
    except ValueError as e:
        assert (
            str(e) == "The initial state must be a (3, ), (3, 1) or (1, 3) numpy array."
        )
    else:
        assert False

    # Test case 16: Initial state with incorrect number of elements
    try:
        navdvl_dead_reckoning(velocity, rph, time=time, initial_state=np.array([1, 2]))
    except ValueError as e:
        assert str(e) == "The initial state must have 3 elements."
    else:
        assert False


def test_navdvl_kf():
    """
    Test the navdvl_kf function.
    """
    # Test case 1: Valid input with time vector
    velocity = np.array([[1, 0, 0], [1, 0, 0], [1, 0, 0]])
    rph = np.array([[0, 0, 0], [0, 0, 0], [0, 0, 0]])
    time = np.array([0, 1, 2])
    initial_state = np.array([0, 0, 0])
    state = navdvl_kf(velocity, rph, time=time, initial_state=initial_state)
    assert state.shape == (3, 10)
    # Check initial position matches initial_state
    assert np.allclose(state[0, 1:4], initial_state, atol=1e-6)
    # Check rph matches input
    assert np.allclose(state[:, 4:7], rph, atol=1e-6)
    # Check output is finite
    assert np.all(np.isfinite(state))

    # Test case 2: Valid input with fixed time step
    dt = 1
    state = navdvl_kf(velocity, rph, dt=dt, initial_state=initial_state)
    assert state.shape == (3, 10)
    assert np.allclose(state[0, 1:4], initial_state, atol=1e-6)
    assert np.allclose(state[:, 4:7], rph, atol=1e-6)
    assert np.all(np.isfinite(state))

    # The rest of the error tests remain unchanged, just assign to a single variable if needed

    # Test case 3: Invalid velocity type
    try:
        navdvl_kf("invalid_type", rph, time=time)
    except TypeError as e:
        assert str(e) == "Velocity must be a numpy array"
    else:
        assert False

    # Test case 4: Invalid rph type
    try:
        navdvl_kf(velocity, "invalid_type", time=time)
    except TypeError as e:
        assert str(e) == "RPH must be a numpy array"
    else:
        assert False

    # Test case 5: Invalid time type
    try:
        navdvl_kf(velocity, rph, time="invalid_type")
    except TypeError as e:
        assert str(e) == "Time must be a numpy array"
    else:
        assert False

    # Test case 6: Invalid initial_state type
    try:
        navdvl_kf(velocity, rph, time=time, initial_state="invalid_type")
    except TypeError as e:
        assert str(e) == "Initial state must be a numpy array"
    else:
        assert False

    # Test case 7: Invalid dt type
    try:
        navdvl_kf(velocity, rph, dt="invalid_type")
    except TypeError as e:
        assert str(e) == "dt must be a float or an integer"
    else:
        assert False

    # Test case 8: dt and time used together
    try:
        navdvl_kf(velocity, rph, time=time, dt=dt)
    except ValueError as e:
        assert str(e) == "dt and time cannot be used together"
    else:
        assert False

    # Test case 9: dt less than or equal to zero
    try:
        navdvl_kf(velocity, rph, dt=0)
    except ValueError as e:
        assert str(e) == "dt must be greater than zero"
    else:
        assert False

    # Test case 10: Invalid velocity_frame type
    try:
        navdvl_kf(velocity, rph, time=time, velocity_frame=123)
    except TypeError as e:
        assert str(e) == "velocity_frame must be a string"
    else:
        assert False

    # Test case 11: Invalid velocity_frame value
    try:
        navdvl_kf(velocity, rph, time=time, velocity_frame="invalid_frame")
    except ValueError as e:
        assert str(e) == "velocity_frame must be either 'body' or 'world'"
    else:
        assert False

    # Test case 12: Invalid velocity shape
    try:
        navdvl_kf(np.array([1, 2, 3]), rph, time=time)
    except ValueError as e:
        assert str(e) == "Velocity must be a matrix with 3 columns"
    else:
        assert False

    # Test case 13: Invalid rph shape
    try:
        navdvl_kf(velocity, np.array([1, 2, 3]), time=time)
    except ValueError as e:
        assert str(e) == "RPH must be a matrix with 3 columns"
    else:
        assert False

    # Test case 14: Invalid time shape
    try:
        navdvl_kf(velocity, rph, time=np.array([[1, 2], [3, 4]]))
    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 15: Invalid initial_state shape
    try:
        navdvl_kf(velocity, rph, time=time, initial_state=np.array([[1, 2], [3, 4]]))
    except ValueError as e:
        assert (
            str(e) == "The initial state must be a (3, ), (3, 1) or (1, 3) numpy array."
        )
    else:
        assert False

    # Test case 16: Initial state with incorrect number of elements
    try:
        navdvl_kf(velocity, rph, time=time, initial_state=np.array([1, 2]))
    except ValueError as e:
        assert str(e) == "The initial state must have 3 elements."
    else:
        assert False

    # Test case 17: Invalid gain type
    try:
        navdvl_kf(velocity, rph, time=time, k1="invalid")
    except TypeError as e:
        assert str(e) == "k1 must be an integer or a float"
    else:
        assert False

    # Test case 18: Invalid gain type
    try:
        navdvl_kf(velocity, rph, time=time, k2="invalid")
    except TypeError as e:
        assert str(e) == "k2 must be an integer or a float"
    else:
        assert False
