"""Pure geofence geometry -- haversine distance + bearings + the keep-in fence.

No I/O, no threads, no heavy deps (stdlib ``math`` only), so it unit-tests
off-robot. The :class:`GeoFence` holds a centre (lat/lon) and a radius, decides
when the dog has strayed too close to the edge, and computes the yaw needed to
home back toward the centre using GPS course-over-ground (no compass required).
"""

from __future__ import annotations

import math
from typing import Optional, Tuple

from config import GeoFenceConfig

_EARTH_RADIUS_M = 6_371_000.0


def haversine_m(lat1: float, lon1: float, lat2: float, lon2: float) -> float:
    """Great-circle distance between two lat/lon points, in metres."""
    p1 = math.radians(lat1)
    p2 = math.radians(lat2)
    dphi = math.radians(lat2 - lat1)
    dlam = math.radians(lon2 - lon1)
    a = (
        math.sin(dphi / 2.0) ** 2
        + math.cos(p1) * math.cos(p2) * math.sin(dlam / 2.0) ** 2
    )
    return 2.0 * _EARTH_RADIUS_M * math.asin(min(1.0, math.sqrt(a)))


def bearing_deg(lat1: float, lon1: float, lat2: float, lon2: float) -> float:
    """Initial bearing from point 1 to point 2, degrees clockwise from north [0,360)."""
    p1 = math.radians(lat1)
    p2 = math.radians(lat2)
    dlam = math.radians(lon2 - lon1)
    y = math.sin(dlam) * math.cos(p2)
    x = math.cos(p1) * math.sin(p2) - math.sin(p1) * math.cos(p2) * math.cos(dlam)
    return (math.degrees(math.atan2(y, x)) + 360.0) % 360.0


def normalize_deg(angle: float) -> float:
    """Wrap an angle to ``[-180, 180)`` degrees."""
    return (angle + 180.0) % 360.0 - 180.0


class GeoFence:
    """A circular keep-in fence around a centre point.

    Distances use the haversine metric; the homing yaw is derived from the GPS
    course-over-ground so no magnetometer/compass is needed.
    """

    def __init__(self, cfg: GeoFenceConfig) -> None:
        self.cfg = cfg
        self._center: Optional[Tuple[float, float]] = None
        if cfg.center_mode == "fixed" and (cfg.center_lat or cfg.center_lon):
            self._center = (cfg.center_lat, cfg.center_lon)

    # --- centre management -------------------------------------------------
    @property
    def has_center(self) -> bool:
        return self._center is not None

    @property
    def center(self) -> Optional[Tuple[float, float]]:
        return self._center

    def set_center(self, lat: float, lon: float) -> None:
        """Explicitly (re)set the fence centre (e.g. dashboard 'set here')."""
        self._center = (float(lat), float(lon))

    def maybe_capture_center(self, lat: float, lon: float) -> None:
        """In ``onstart`` mode, capture the first good fix as the centre."""
        if self._center is None and self.cfg.center_mode == "onstart":
            self._center = (float(lat), float(lon))

    # --- queries -----------------------------------------------------------
    def distance_from_center(self, lat: float, lon: float) -> Optional[float]:
        if self._center is None:
            return None
        return haversine_m(self._center[0], self._center[1], lat, lon)

    def distance_to_edge(self, lat: float, lon: float) -> Optional[float]:
        """Metres from the current position to the fence edge (negative = outside)."""
        d = self.distance_from_center(lat, lon)
        return None if d is None else (self.cfg.radius_m - d)

    def breached(self, lat: float, lon: float) -> bool:
        """True once within ``margin_m`` of the edge (or beyond) -> start homing."""
        d = self.distance_from_center(lat, lon)
        if d is None:
            return False
        return d >= (self.cfg.radius_m - self.cfg.margin_m)

    def cleared(self, lat: float, lon: float) -> bool:
        """True once safely back inside (hysteresis) -> may resume normal behaviour."""
        d = self.distance_from_center(lat, lon)
        if d is None:
            return True
        return d <= (self.cfg.radius_m - self.cfg.margin_m - self.cfg.release_m)

    def bearing_to_center(self, lat: float, lon: float) -> Optional[float]:
        if self._center is None:
            return None
        return bearing_deg(lat, lon, self._center[0], self._center[1])

    def homing_yaw(self, lat: float, lon: float, course_deg: Optional[float]) -> float:
        """Yaw rate (rad/s) to steer toward the centre given current GPS course.

        ``vyaw = homing_kp * heading_error_rad``, clamped, where ``heading_error``
        is ``normalize(bearing_to_centre - course)``. ``+`` yaw = turn left/CCW.
        When ``course_deg`` is unknown (too slow for a valid GPS track), returns
        a gentle constant turn so the dog moves and re-acquires a course.
        """
        target = self.bearing_to_center(lat, lon)
        if target is None:
            return 0.0
        if course_deg is None:
            # No reliable course yet: turn gently (one direction) to gain a track.
            return min(self.cfg.max_homing_yaw, 0.4)
        # Compass bearings are clockwise-from-north; a positive (clockwise) error
        # means "turn right" = negative yaw in the body convention, hence the sign.
        err_deg = normalize_deg(target - course_deg)
        vyaw = -math.radians(err_deg) * self.cfg.homing_kp
        return max(-self.cfg.max_homing_yaw, min(self.cfg.max_homing_yaw, vyaw))