"""Unit tests for zone_checker.py — pure geometry, no ROS2 required."""

import math
import pytest

from saltybot_uwb_geofence.zone_checker import (
    parse_flat_vertices,
    point_in_polygon,
    min_dist_to_boundary,
)


# ── parse_flat_vertices ───────────────────────────────────────────────────────

class TestParseFlatVertices:
    def test_square(self):
        verts = parse_flat_vertices([0.0, 0.0, 1.0, 0.0, 1.0, 1.0, 0.0, 1.0])
        assert verts == [(0.0, 0.0), (1.0, 0.0), (1.0, 1.0), (0.0, 1.0)]

    def test_triangle(self):
        verts = parse_flat_vertices([0.0, 0.0, 2.0, 0.0, 1.0, 2.0])
        assert len(verts) == 3

    def test_odd_length_raises(self):
        with pytest.raises(ValueError, match="even"):
            parse_flat_vertices([1.0, 2.0, 3.0])

    def test_too_few_points_raises(self):
        with pytest.raises(ValueError, match="3 vertices"):
            parse_flat_vertices([0.0, 0.0, 1.0, 1.0])


# ── point_in_polygon ──────────────────────────────────────────────────────────

UNIT_SQUARE = [(0.0, 0.0), (1.0, 0.0), (1.0, 1.0), (0.0, 1.0)]
TRIANGLE = [(0.0, 0.0), (4.0, 0.0), (2.0, 4.0)]


class TestPointInPolygon:
    # --- Unit square ---
    def test_centre_inside(self):
        assert point_in_polygon(0.5, 0.5, UNIT_SQUARE) is True

    def test_outside_right(self):
        assert point_in_polygon(1.5, 0.5, UNIT_SQUARE) is False

    def test_outside_left(self):
        assert point_in_polygon(-0.1, 0.5, UNIT_SQUARE) is False

    def test_outside_above(self):
        assert point_in_polygon(0.5, 1.5, UNIT_SQUARE) is False

    def test_outside_below(self):
        assert point_in_polygon(0.5, -0.1, UNIT_SQUARE) is False

    def test_far_outside(self):
        assert point_in_polygon(10.0, 10.0, UNIT_SQUARE) is False

    # --- Triangle ---
    def test_triangle_apex_region(self):
        assert point_in_polygon(2.0, 2.0, TRIANGLE) is True

    def test_triangle_base_centre(self):
        assert point_in_polygon(2.0, 0.5, TRIANGLE) is True

    def test_triangle_outside_top(self):
        assert point_in_polygon(2.0, 5.0, TRIANGLE) is False

    def test_triangle_outside_right(self):
        assert point_in_polygon(4.5, 0.5, TRIANGLE) is False

    # --- Degenerate ---
    def test_empty_polygon(self):
        assert point_in_polygon(0.5, 0.5, []) is False

    def test_two_vertex_polygon(self):
        assert point_in_polygon(0.5, 0.5, [(0.0, 0.0), (1.0, 1.0)]) is False

    # --- 10 × 8 room (matches emergency boundary in default config) ---
    def test_inside_room(self):
        room = [(-6.0, -5.0), (6.0, -5.0), (6.0, 5.0), (-6.0, 5.0)]
        assert point_in_polygon(0.0, 0.0, room) is True

    def test_outside_room(self):
        room = [(-6.0, -5.0), (6.0, -5.0), (6.0, 5.0), (-6.0, 5.0)]
        assert point_in_polygon(7.0, 0.0, room) is False


# ── min_dist_to_boundary ──────────────────────────────────────────────────────

class TestMinDistToBoundary:
    def test_centre_of_square(self):
        # Centre of unit square: distance to each edge = 0.5
        d = min_dist_to_boundary(0.5, 0.5, UNIT_SQUARE)
        assert abs(d - 0.5) < 1e-9

    def test_on_edge(self):
        d = min_dist_to_boundary(0.5, 0.0, UNIT_SQUARE)
        assert d < 1e-9

    def test_outside_corner(self):
        # Point at (1.5, 0.5): closest edge is the right edge at x=1
        d = min_dist_to_boundary(1.5, 0.5, UNIT_SQUARE)
        assert abs(d - 0.5) < 1e-9

    def test_outside_above(self):
        d = min_dist_to_boundary(0.5, 1.3, UNIT_SQUARE)
        assert abs(d - 0.3) < 1e-9

    def test_empty_polygon(self):
        assert min_dist_to_boundary(0.0, 0.0, []) == float("inf")

    def test_single_vertex(self):
        assert min_dist_to_boundary(1.0, 1.0, [(0.0, 0.0)]) == float("inf")


# ── ramp_toward & apply_speed_limit ──────────────────────────────────────────

from saltybot_uwb_geofence.zone_checker import ramp_toward, apply_speed_limit


class TestRampToward:
    def test_ramp_up(self):
        val = ramp_toward(0.0, 1.0, 0.1)
        assert abs(val - 0.1) < 1e-9

    def test_ramp_down(self):
        val = ramp_toward(1.0, 0.0, 0.1)
        assert abs(val - 0.9) < 1e-9

    def test_no_overshoot_up(self):
        val = ramp_toward(0.95, 1.0, 0.1)
        assert val == 1.0

    def test_no_overshoot_down(self):
        val = ramp_toward(0.05, 0.0, 0.1)
        assert val == 0.0

    def test_already_at_target(self):
        val = ramp_toward(0.5, 0.5, 0.1)
        assert val == 0.5


class TestApplySpeedLimit:
    def _call(self, vx: float, vy: float = 0.0, wz: float = 0.0,
              max_speed: float = 1.0, max_ang: float = 1.5):
        return apply_speed_limit(vx, vy, wz, max_speed, max_ang)

    def test_within_limit_unchanged(self):
        lx, ly, *_, wz = self._call(0.3, 0.0, 0.2)
        assert abs(lx - 0.3) < 1e-9
        assert abs(wz - 0.2) < 1e-9

    def test_linear_exceeds_limit(self):
        lx, ly, *_ = self._call(2.0, 0.0, 0.0)
        speed = math.hypot(lx, ly)
        assert speed <= 1.0 + 1e-9

    def test_direction_preserved_when_limited(self):
        lx, ly, *_ = self._call(3.0, 4.0, 0.0, max_speed=1.0)
        speed = math.hypot(lx, ly)
        assert abs(speed - 1.0) < 1e-9
        assert abs(lx / ly - 3.0 / 4.0) < 1e-6

    def test_angular_clamp_positive(self):
        *_, wz = self._call(0.0, 0.0, 2.0, max_ang=1.0)
        assert abs(wz - 1.0) < 1e-9

    def test_angular_clamp_negative(self):
        *_, wz = self._call(0.0, 0.0, -2.0, max_ang=1.0)
        assert abs(wz + 1.0) < 1e-9

    def test_zero_cmd_stays_zero(self):
        lx, ly, lz, ax, ay, wz = self._call(0.0, 0.0, 0.0)
        assert lx == 0.0
        assert ly == 0.0
        assert wz == 0.0
