"""GPS sources for the geofence.

The Go2 has no built-in GPS, so this reads an EXTERNAL receiver on the onboard
computer. Three sources, selected by ``cfg.gps.source``:

  * ``gpsd``   -- talk to a running ``gpsd`` over its JSON socket (no extra dep).
  * ``serial`` -- parse NMEA (GGA/RMC) straight off a serial receiver (pyserial).
  * ``mock``   -- a simulated receiver that integrates the commanded velocity into
                  a fake lat/lon, so the geofence + BOUNDARY behaviour can be
                  exercised entirely off-robot.

Each source runs a background thread and exposes the most recent good fix via
:meth:`GpsSource.latest` (which returns ``None`` when the fix is stale). Quality
gating (min sats / max HDOP) is applied before a fix is published.
"""

from __future__ import annotations

import json
import logging
import math
import socket
import threading
import time
from abc import ABC, abstractmethod
from dataclasses import dataclass
from typing import Optional

from config import GoWelcomeConfig, GpsConfig

logger = logging.getLogger(__name__)

_EARTH_RADIUS_M = 6_371_000.0


@dataclass
class GpsFix:
    """A single GPS reading. Angles in degrees, distances in metres/SI."""

    lat: float
    lon: float
    ts: float                      # time.monotonic() when received
    course_deg: Optional[float] = None   # course over ground, CW from north
    speed_mps: Optional[float] = None
    num_sats: Optional[int] = None
    hdop: Optional[float] = None


class GpsSource(ABC):
    """Background GPS reader exposing the latest (non-stale) fix."""

    @abstractmethod
    def start(self) -> None: ...

    @abstractmethod
    def stop(self) -> None: ...

    @abstractmethod
    def latest(self) -> Optional[GpsFix]:
        """Most recent good fix, or ``None`` if unavailable/stale."""

    def on_command(self, vx: float, vy: float, vyaw: float) -> None:
        """Hook so simulated sources can integrate the robot's motion. No-op
        for real receivers."""


class _ThreadGpsSource(GpsSource):
    """Shared thread + quality-gating + staleness machinery for real sources."""

    def __init__(self, cfg: GpsConfig) -> None:
        self.cfg = cfg
        self._fix: Optional[GpsFix] = None
        self._lock = threading.Lock()
        self._stop = threading.Event()
        self._thread: Optional[threading.Thread] = None

    def start(self) -> None:
        self._thread = threading.Thread(target=self._loop, name="GpsSource", daemon=True)
        self._thread.start()

    def stop(self) -> None:
        self._stop.set()

    def latest(self) -> Optional[GpsFix]:
        with self._lock:
            fix = self._fix
        if fix is None:
            return None
        if (time.monotonic() - fix.ts) > self.cfg.stale_after:
            return None
        return fix

    def _publish(self, fix: GpsFix) -> None:
        if fix.num_sats is not None and fix.num_sats < self.cfg.min_satellites:
            return
        if fix.hdop is not None and fix.hdop > self.cfg.max_hdop:
            return
        with self._lock:
            self._fix = fix

    @abstractmethod
    def _loop(self) -> None: ...


class GpsdSource(_ThreadGpsSource):
    """Read fixes from a running ``gpsd`` over its JSON protocol (stdlib socket)."""

    def _loop(self) -> None:
        while not self._stop.is_set():
            try:
                with socket.create_connection(
                    (self.cfg.gpsd_host, self.cfg.gpsd_port), timeout=5.0
                ) as sock:
                    sock.sendall(b'?WATCH={"enable":true,"json":true}\n')
                    buf = b""
                    sock.settimeout(2.0)
                    while not self._stop.is_set():
                        try:
                            chunk = sock.recv(4096)
                        except socket.timeout:
                            continue
                        if not chunk:
                            break
                        buf += chunk
                        while b"\n" in buf:
                            line, buf = buf.split(b"\n", 1)
                            self._handle_line(line)
            except Exception as exc:  # pragma: no cover - hardware path
                logger.debug("gpsd connection error: %s; retrying", exc)
                self._stop.wait(2.0)

    def _handle_line(self, line: bytes) -> None:
        try:
            obj = json.loads(line.decode("ascii", "replace"))
        except Exception:
            return
        if obj.get("class") != "TPV":
            return
        if obj.get("mode", 0) < 2:  # need at least a 2D fix
            return
        lat, lon = obj.get("lat"), obj.get("lon")
        if lat is None or lon is None:
            return
        self._publish(GpsFix(
            lat=float(lat), lon=float(lon), ts=time.monotonic(),
            course_deg=_opt_float(obj.get("track")),
            speed_mps=_opt_float(obj.get("speed")),
            num_sats=None,  # gpsd reports sats in SKY reports; not gated here
            hdop=_opt_float(obj.get("hdop")),
        ))


class SerialNmeaSource(_ThreadGpsSource):
    """Parse NMEA GGA/RMC off a serial GPS receiver (lazy pyserial)."""

    def _loop(self) -> None:
        try:
            import serial  # type: ignore
        except ImportError:  # pragma: no cover - off-robot
            logger.error("SerialNmeaSource needs pyserial: pip install pyserial")
            return
        while not self._stop.is_set():
            try:
                with serial.Serial(self.cfg.serial_port, self.cfg.serial_baud, timeout=1.0) as ser:
                    last_course = None
                    last_speed = None
                    while not self._stop.is_set():
                        raw = ser.readline().decode("ascii", "replace").strip()
                        if not raw or not raw.startswith("$"):
                            continue
                        kind = raw[3:6]
                        if kind == "RMC":
                            c, s = _parse_rmc(raw)
                            if c is not None:
                                last_course = c
                            if s is not None:
                                last_speed = s
                        elif kind == "GGA":
                            parsed = _parse_gga(raw)
                            if parsed is not None:
                                lat, lon, sats, hdop = parsed
                                self._publish(GpsFix(
                                    lat=lat, lon=lon, ts=time.monotonic(),
                                    course_deg=last_course, speed_mps=last_speed,
                                    num_sats=sats, hdop=hdop,
                                ))
            except Exception as exc:  # pragma: no cover - hardware path
                logger.debug("serial GPS error: %s; retrying", exc)
                self._stop.wait(2.0)


class MockGpsSource(GpsSource):
    """Simulated receiver: integrates the commanded velocity into a fake fix.

    Lets the geofence + BOUNDARY homing be demonstrated/tested with no hardware.
    Heading is tracked CW-from-north so it matches a real GPS course-over-ground.
    """

    def __init__(self, cfg: GpsConfig, rate_hz: float = 10.0) -> None:
        self.cfg = cfg
        self._lat0 = cfg.mock_start_lat
        self._lon0 = cfg.mock_start_lon
        self._north_m = 0.0
        self._east_m = 0.0
        self._heading_cw = 0.0      # degrees, CW from north
        self._vx = 0.0
        self._vyaw = 0.0
        self._period = 1.0 / max(1.0, rate_hz)
        self._lock = threading.Lock()
        self._stop = threading.Event()
        self._thread: Optional[threading.Thread] = None
        self._fix: Optional[GpsFix] = None

    def start(self) -> None:
        self._thread = threading.Thread(target=self._loop, name="MockGps", daemon=True)
        self._thread.start()

    def stop(self) -> None:
        self._stop.set()

    def on_command(self, vx: float, vy: float, vyaw: float) -> None:
        with self._lock:
            self._vx = vx
            self._vyaw = vyaw

    def latest(self) -> Optional[GpsFix]:
        with self._lock:
            return self._fix

    def _loop(self) -> None:
        dt = self._period
        while not self._stop.is_set():
            with self._lock:
                vx, vyaw = self._vx, self._vyaw
            # +vyaw is CCW (left); CW-from-north heading decreases on a left turn.
            self._heading_cw = (self._heading_cw - math.degrees(vyaw) * dt) % 360.0
            h = math.radians(self._heading_cw)
            self._north_m += vx * math.cos(h) * dt
            self._east_m += vx * math.sin(h) * dt
            lat = self._lat0 + math.degrees(self._north_m / _EARTH_RADIUS_M)
            lon = self._lon0 + math.degrees(
                self._east_m / (_EARTH_RADIUS_M * math.cos(math.radians(self._lat0)))
            )
            speed = abs(vx)
            fix = GpsFix(
                lat=lat, lon=lon, ts=time.monotonic(),
                course_deg=self._heading_cw if speed >= 0.05 else None,
                speed_mps=speed, num_sats=12, hdop=0.8,
            )
            with self._lock:
                self._fix = fix
            self._stop.wait(dt)


# --------------------------------------------------------------------------- #
# Helpers
# --------------------------------------------------------------------------- #
def _opt_float(v) -> Optional[float]:
    try:
        return float(v)
    except (TypeError, ValueError):
        return None


def _nmea_deg(value: str, hemi: str) -> Optional[float]:
    """Convert an NMEA ddmm.mmmm + hemisphere field to signed decimal degrees."""
    if not value:
        return None
    try:
        dot = value.index(".")
        deg = int(value[: dot - 2])
        minutes = float(value[dot - 2 :])
        dec = deg + minutes / 60.0
        if hemi in ("S", "W"):
            dec = -dec
        return dec
    except (ValueError, IndexError):
        return None


def _parse_gga(sentence: str):
    """Return ``(lat, lon, num_sats, hdop)`` from a GGA sentence, or ``None``."""
    sentence = sentence.split("*", 1)[0]   # drop the trailing *checksum
    f = sentence.split(",")
    if len(f) < 10:
        return None
    if f[6] in ("", "0"):   # fix quality 0 == no fix
        return None
    lat = _nmea_deg(f[2], f[3])
    lon = _nmea_deg(f[4], f[5])
    if lat is None or lon is None:
        return None
    sats = int(f[7]) if f[7].isdigit() else None
    hdop = _opt_float(f[8])
    return lat, lon, sats, hdop


def _parse_rmc(sentence: str):
    """Return ``(course_deg, speed_mps)`` from an RMC sentence (or ``None``s)."""
    sentence = sentence.split("*", 1)[0]   # drop the trailing *checksum
    f = sentence.split(",")
    if len(f) < 9 or f[2] != "A":   # status A == valid
        return None, None
    speed = _opt_float(f[7])         # knots
    course = _opt_float(f[8])
    return course, (speed * 0.514444 if speed is not None else None)


def build_gps_source(cfg: GoWelcomeConfig) -> Optional[GpsSource]:
    """Construct the GPS source named by ``cfg.gps`` (or ``None`` if disabled).

    ``auto`` probes gpsd first, then falls back to a serial NMEA receiver.
    """
    g = cfg.gps
    if not g.enabled:
        return None
    src = (g.source or "auto").strip().lower()

    if src == "mock" or cfg.mock:
        logger.info("GPS: using simulated MockGpsSource")
        return MockGpsSource(g)
    if src == "gpsd":
        return GpsdSource(g)
    if src == "serial":
        return SerialNmeaSource(g)

    # auto: prefer gpsd if it answers, else serial.
    try:
        with socket.create_connection((g.gpsd_host, g.gpsd_port), timeout=0.5):
            logger.info("GPS: auto-detected gpsd at %s:%d", g.gpsd_host, g.gpsd_port)
            return GpsdSource(g)
    except OSError:
        logger.info("GPS: gpsd not found; using serial NMEA on %s", g.serial_port)
        return SerialNmeaSource(g)