"""zone_checker.py — Pure-geometry helpers for UWB geofence zones.

No ROS2 dependencies; fully unit-testable.

Coordinate system: UWB local frame, metres, right-hand X-Y plane.
"""

from __future__ import annotations

import math
from typing import List, Tuple

Vertex = Tuple[float, float]
Polygon = List[Vertex]


def parse_flat_vertices(flat: List[float]) -> Polygon:
    """Convert flat [x1, y1, x2, y2, ...] list to [(x1,y1), (x2,y2), ...].

    Raises ValueError if fewer than 3 vertices are provided or the list
    length is odd.
    """
    if len(flat) % 2 != 0:
        raise ValueError(f"Flat vertex list must have even length, got {len(flat)}")
    if len(flat) < 6:
        raise ValueError(f"Need at least 3 vertices (6 values), got {len(flat)}")
    return [(flat[i], flat[i + 1]) for i in range(0, len(flat), 2)]


def point_in_polygon(x: float, y: float, polygon: Polygon) -> bool:
    """Ray-casting point-in-polygon test.

    Returns True if (x, y) is strictly inside or on the boundary of
    *polygon*.  Polygon vertices must be ordered (CW or CCW).
    """
    n = len(polygon)
    if n < 3:
        return False
    inside = False
    x1, y1 = polygon[-1]
    for x2, y2 in polygon:
        # Check if the ray from (x,y) rightward crosses edge (x1,y1)-(x2,y2)
        if (y1 > y) != (y2 > y):
            x_intersect = (x2 - x1) * (y - y1) / (y2 - y1) + x1
            if x <= x_intersect:
                inside = not inside
        x1, y1 = x2, y2
    return inside


def min_dist_to_boundary(x: float, y: float, polygon: Polygon) -> float:
    """Minimum distance from (x, y) to any edge of *polygon* (metres).

    Uses point-to-segment distance with clamped projection.
    """
    if len(polygon) < 2:
        return float("inf")
    min_d = float("inf")
    n = len(polygon)
    for i in range(n):
        ax, ay = polygon[i]
        bx, by = polygon[(i + 1) % n]
        dx, dy = bx - ax, by - ay
        seg_len_sq = dx * dx + dy * dy
        if seg_len_sq < 1e-12:
            d = math.hypot(x - ax, y - ay)
        else:
            t = max(0.0, min(1.0, ((x - ax) * dx + (y - ay) * dy) / seg_len_sq))
            px, py = ax + t * dx, ay + t * dy
            d = math.hypot(x - px, y - py)
        if d < min_d:
            min_d = d
    return min_d


# ── Velocity helpers (no ROS2 deps — also used by tests) ─────────────────────

def ramp_toward(current: float, target: float, step: float) -> float:
    """Move *current* toward *target* by at most *step*, never overshoot."""
    if current < target:
        return min(current + step, target)
    if current > target:
        return max(current - step, target)
    return current


def apply_speed_limit(linear_x: float, linear_y: float, angular_z: float,
                      max_speed: float, max_angular: float,
                      linear_z: float = 0.0,
                      angular_x: float = 0.0,
                      angular_y: float = 0.0
                      ) -> tuple:
    """Return (linear_x, linear_y, linear_z, angular_x, angular_y, angular_z)
    with linear speed and angular_z clamped to limits.
    """
    speed = math.hypot(linear_x, linear_y)
    if speed > max_speed and speed > 1e-6:
        scale = max_speed / speed
        lx = linear_x * scale
        ly = linear_y * scale
    else:
        lx, ly = linear_x, linear_y
    wz = max(-max_angular, min(max_angular, angular_z))
    return lx, ly, linear_z, angular_x, angular_y, wz
