"""Tests for geofence boundary enforcer."""

import pytest
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point, Quaternion, Pose, PoseWithCovariance, TwistWithCovariance
import rclpy
from rclpy.time import Time

from saltybot_geofence.geofence_node import GeofenceNode


@pytest.fixture
def rclpy_fixture():
    rclpy.init()
    yield
    rclpy.shutdown()


@pytest.fixture
def node(rclpy_fixture):
    node = GeofenceNode()
    yield node
    node.destroy_node()


class TestInit:
    def test_node_initialization(self, node):
        assert node.enforce_boundary is False
        assert node.margin == 0.0
        assert node.inside_geofence is True


class TestVertexParsing:
    def test_parse_vertices_valid(self, node):
        vertices = node._parse_vertices([0.0, 0.0, 1.0, 0.0, 1.0, 1.0, 0.0, 1.0])
        assert len(vertices) == 4
        assert vertices[0] == (0.0, 0.0)
        assert vertices[1] == (1.0, 0.0)

    def test_parse_vertices_insufficient(self, node):
        vertices = node._parse_vertices([0.0, 0.0, 1.0])
        assert len(vertices) == 0


class TestPointInPolygon:
    def test_point_inside_square(self, node):
        vertices = [(0.0, 0.0), (2.0, 0.0), (2.0, 2.0), (0.0, 2.0)]
        assert node._point_in_polygon((1.0, 1.0), vertices) is True

    def test_point_outside_square(self, node):
        vertices = [(0.0, 0.0), (2.0, 0.0), (2.0, 2.0), (0.0, 2.0)]
        assert node._point_in_polygon((3.0, 1.0), vertices) is False

    def test_point_on_vertex(self, node):
        vertices = [(0.0, 0.0), (2.0, 0.0), (2.0, 2.0), (0.0, 2.0)]
        # Point on vertex behavior may vary (typically outside)
        result = node._point_in_polygon((0.0, 0.0), vertices)
        assert isinstance(result, bool)

    def test_point_on_edge(self, node):
        vertices = [(0.0, 0.0), (2.0, 0.0), (2.0, 2.0), (0.0, 2.0)]
        # Point on edge behavior (typically outside)
        result = node._point_in_polygon((1.0, 0.0), vertices)
        assert isinstance(result, bool)

    def test_triangle_inside(self, node):
        vertices = [(0.0, 0.0), (4.0, 0.0), (2.0, 3.0)]
        assert node._point_in_polygon((2.0, 1.0), vertices) is True

    def test_triangle_outside(self, node):
        vertices = [(0.0, 0.0), (4.0, 0.0), (2.0, 3.0)]
        assert node._point_in_polygon((5.0, 5.0), vertices) is False

    def test_concave_polygon(self, node):
        # L-shaped polygon
        vertices = [(0.0, 0.0), (3.0, 0.0), (3.0, 1.0), (1.0, 1.0), (1.0, 3.0), (0.0, 3.0)]
        assert node._point_in_polygon((0.5, 0.5), vertices) is True
        assert node._point_in_polygon((2.0, 2.0), vertices) is False

    def test_circle_approximation(self, node):
        # Octagon approximating circle
        import math
        vertices = []
        for i in range(8):
            angle = 2 * math.pi * i / 8
            vertices.append((math.cos(angle), math.sin(angle)))

        # Center should be inside
        assert node._point_in_polygon((0.0, 0.0), vertices) is True
        # Far outside should be outside
        assert node._point_in_polygon((10.0, 10.0), vertices) is False


class TestOdometryProcessing:
    def test_odometry_update_position(self, node):
        node.geofence_vertices = [(0.0, 0.0), (10.0, 0.0), (10.0, 10.0), (0.0, 10.0)]

        msg = Odometry()
        msg.pose.pose.position.x = 5.0
        msg.pose.pose.position.y = 5.0

        node._on_odometry(msg)

        assert node.robot_x == 5.0
        assert node.robot_y == 5.0

    def test_breach_detection_inside(self, node):
        node.geofence_vertices = [(0.0, 0.0), (10.0, 0.0), (10.0, 10.0), (0.0, 10.0)]

        msg = Odometry()
        msg.pose.pose.position.x = 5.0
        msg.pose.pose.position.y = 5.0

        node._on_odometry(msg)

        assert node.inside_geofence is True

    def test_breach_detection_outside(self, node):
        node.geofence_vertices = [(0.0, 0.0), (10.0, 0.0), (10.0, 10.0), (0.0, 10.0)]

        msg = Odometry()
        msg.pose.pose.position.x = 15.0
        msg.pose.pose.position.y = 5.0

        node._on_odometry(msg)

        assert node.inside_geofence is False

    def test_breach_flag_transition(self, node):
        node.geofence_vertices = [(0.0, 0.0), (10.0, 0.0), (10.0, 10.0), (0.0, 10.0)]
        assert node.breach_published is False

        # Move outside
        msg = Odometry()
        msg.pose.pose.position.x = 15.0
        msg.pose.pose.position.y = 5.0
        node._on_odometry(msg)

        assert node.breach_published is True

        # Move back inside
        msg.pose.pose.position.x = 5.0
        msg.pose.pose.position.y = 5.0
        node._on_odometry(msg)

        assert node.breach_published is False


class TestEdgeCases:
    def test_no_geofence_defined(self, node):
        node.geofence_vertices = []

        msg = Odometry()
        msg.pose.pose.position.x = 0.0
        msg.pose.pose.position.y = 0.0

        node._on_odometry(msg)

        # Should default to safe (inside)
        assert node.inside_geofence is True

    def test_very_small_polygon(self, node):
        vertices = [(0.0, 0.0), (0.01, 0.0), (0.01, 0.01), (0.0, 0.01)]
        assert node._point_in_polygon((0.005, 0.005), vertices) is True
        assert node._point_in_polygon((0.1, 0.1), vertices) is False

    def test_large_coordinates(self, node):
        vertices = [(0.0, 0.0), (1000.0, 0.0), (1000.0, 1000.0), (0.0, 1000.0)]
        assert node._point_in_polygon((500.0, 500.0), vertices) is True
        assert node._point_in_polygon((1500.0, 500.0), vertices) is False