# Path: auv_control_pi/utils.py
# def elapsed_micros(start_time_us):
# def micros():
# def clamp_angle(deg):
# def point_at_distance(distance, heading, current_point):
# def heading_to_point(point_a, point_b):
# def distance_to_point(point_a, point_b):
# def get_error_angle(target, heading):


from ..utils import get_error_angle, Point, heading_to_point, distance_to_point


def test_point():
    point = Point(10, 20)
    assert point.lat == 10
    assert point.lng == 20


def test_heading_to_point():
    start_point = Point(49, -120)
    end_point = Point(50, -120)
    heading = heading_to_point(start_point, end_point)
    assert heading == 0.0


def test_distance_to_point():
    start_point = Point(49, -120)
    end_point = Point(50, -120)
    distance = distance_to_point(start_point, end_point)
    assert isinstance(distance, float)


def test_get_error_angle():
    result = get_error_angle(target=10, heading=0)
    assert result == -10

    result = get_error_angle(target=0, heading=10)
    assert result == 10


# Path: auv_control_pi/utils.py
# def elapsed_micros(start_time_us):
# def micros():
# def clamp_angle(deg):
# def point_at_distance(distance, heading, current_point):
# def heading_to_point(point_a, point_b):
# def distance_to_point(point_a, point_b):
# def get_error_angle(target, heading):


# Path: auv_control_pi/tests/test_utils.py
from ..utils import get_error_angle, Point, heading_to_point, distance_to_point



def test_point():
    point = Point(10, 20)
    assert point.lat == 10
    assert point.lng == 20


def test_heading_to_point():
    start_point = Point(49, -120)
    end_point = Point(50, -120)
    heading = heading_to_point(start_point, end_point)
    assert heading == 0.0


def test_distance_to_point():
    start_point = Point(49, -120)
    end_point = Point(50, -120)
    distance = distance_to_point(start_point, end_point)
    assert isinstance(distance, float)


def test_get_error_angle():
    result = get_error_angle(target=10, heading=0)
    assert result == -10

    result = get_error_angle(target=0, heading=10)
    assert result == 10


    result = get_error_angle(target=10, heading=350)