"""
GPS Kalman Filter \u2014 rider-api

A 1D Kalman filter applied independently to latitude and longitude
to smooth noisy GPS coordinates from riders and delivery points.

Why Kalman for GPS?
- GPS readings contain measurement noise (\u00b15\u201315m typical, \u00b150m poor signal)
- Rider location pings can "jump" due to bad signal or device error
- Kalman filter gives an optimal estimate by balancing:
    (1) Previous predicted position (process model)
    (2) New GPS measurement (observation model)

Design:
- Separate filter instance per rider (stateful \u2014 preserves history)
- `CoordinateKalmanFilter` \u2014 single lat/lon smoother
- `GPSKalmanFilter`          \u2014 wraps two CoordinateKalmanFilters (lat + lon)
- `RiderKalmanRegistry`      \u2014 manages per-rider filter instances
- `smooth_coordinates()`     \u2014 stateless single-shot smoother for delivery coords

Usage:
    # Stateless (one-shot, no history \u2014 for delivery coords):
    smooth_lat, smooth_lon = smooth_coordinates(raw_lat, raw_lon)

    # Stateful (per-rider, preserves motion history):
    registry = RiderKalmanRegistry()
    lat, lon = registry.update(rider_id=1116, lat=11.0067, lon=76.9558)
"""

import logging
import time
from typing import Dict, Optional, Tuple

logger = logging.getLogger(__name__)


# \u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500
# CORE 1D KALMAN FILTER
# \u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500

class CoordinateKalmanFilter:
    """
    1-dimensional Kalman filter for a single GPS coordinate (lat or lon).

    State model: position only (constant position with random walk).

    Equations:
        Prediction:   x\u0302\u2096\u207b  = x\u0302\u2096\u208b\u2081          (no movement assumed between pings)
                      P\u0302\u2096\u207b   = P\u2096\u208b\u2081 + Q       (uncertainty grows over time)

        Update:       K\u2096    = P\u0302\u2096\u207b / (P\u0302\u2096\u207b + R)   (Kalman gain)
                      x\u0302\u2096    = x\u0302\u2096\u207b + K\u2096\u00b7(z\u2096 - x\u0302\u2096\u207b)  (weighted fusion)
                      P\u2096    = (1 - K\u2096)\u00b7P\u0302\u2096\u207b          (update uncertainty)

    Parameters:
        process_noise (Q): How much position can change between measurements.
                           Higher = filter trusts new measurements more (less smoothing).
        measurement_noise (R): GPS measurement uncertainty.
                               Higher = filter trusts history more (more smoothing).
    """

    def __init__(
        self,
        process_noise: float = 1e-4,
        measurement_noise: float = 0.01,
        initial_uncertainty: float = 1.0,
    ):
        self.Q = process_noise
        self.R = measurement_noise
        self._x: Optional[float] = None
        self._P: float = initial_uncertainty

    @property
    def initialized(self) -> bool:
        return self._x is not None

    def update(self, measurement: float) -> float:
        """Process one new measurement and return the filtered estimate."""
        if not self.initialized:
            self._x = measurement
            return self._x

        # Predict
        x_prior = self._x
        P_prior = self._P + self.Q

        # Update
        K = P_prior / (P_prior + self.R)
        self._x = x_prior + K * (measurement - x_prior)
        self._P = (1.0 - K) * P_prior

        return self._x

    def reset(self):
        self._x = None
        self._P = 1.0


# \u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500
# 2D GPS KALMAN FILTER (lat + lon)
# \u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500

class GPSKalmanFilter:
    """
    Two-dimensional GPS smoother using independent 1D Kalman filters
    for latitude and longitude.
    """

    def __init__(
        self,
        process_noise: float = 1e-4,
        measurement_noise: float = 0.01,
    ):
        self.lat_filter = CoordinateKalmanFilter(process_noise, measurement_noise)
        self.lon_filter = CoordinateKalmanFilter(process_noise, measurement_noise)
        self.last_updated: float = time.time()
        self.update_count: int = 0

    def update(self, lat: float, lon: float) -> Tuple[float, float]:
        """Feed a new GPS reading and get the smoothed (lat, lon)."""
        if not self._is_valid_coord(lat, lon):
            if self.lat_filter.initialized:
                return self.lat_filter._x, self.lon_filter._x
            return lat, lon

        smooth_lat = self.lat_filter.update(lat)
        smooth_lon = self.lon_filter.update(lon)
        self.last_updated = time.time()
        self.update_count += 1

        return smooth_lat, smooth_lon

    def get_estimate(self) -> Optional[Tuple[float, float]]:
        if self.lat_filter.initialized:
            return self.lat_filter._x, self.lon_filter._x
        return None

    def reset(self):
        self.lat_filter.reset()
        self.lon_filter.reset()
        self.update_count = 0

    @staticmethod
    def _is_valid_coord(lat: float, lon: float) -> bool:
        try:
            lat, lon = float(lat), float(lon)
            return (
                -90.0 <= lat <= 90.0
                and -180.0 <= lon <= 180.0
                and not (lat == 0.0 and lon == 0.0)
            )
        except (TypeError, ValueError):
            return False


# \u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500
# PER-RIDER FILTER REGISTRY
# \u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500

class RiderKalmanRegistry:
    """
    Maintains per-rider Kalman filter instances across calls.
    Stale filters (> 30 min silence) are automatically reset.
    """

    def __init__(
        self,
        process_noise: float = 1e-4,
        measurement_noise: float = 0.01,
        stale_seconds: float = 1800.0,
    ):
        self._filters: Dict[str, GPSKalmanFilter] = {}
        self._process_noise = process_noise
        self._measurement_noise = measurement_noise
        self._stale_seconds = stale_seconds

    def _get_or_create(self, rider_id) -> GPSKalmanFilter:
        key = str(rider_id)
        now = time.time()
        if key in self._filters:
            f = self._filters[key]
            if now - f.last_updated > self._stale_seconds:
                f.reset()
            return f
        self._filters[key] = GPSKalmanFilter(
            process_noise=self._process_noise,
            measurement_noise=self._measurement_noise,
        )
        return self._filters[key]

    def update(self, rider_id, lat: float, lon: float) -> Tuple[float, float]:
        return self._get_or_create(rider_id).update(lat, lon)

    def get_estimate(self, rider_id) -> Optional[Tuple[float, float]]:
        key = str(rider_id)
        if key in self._filters:
            return self._filters[key].get_estimate()
        return None

    def reset_rider(self, rider_id):
        key = str(rider_id)
        if key in self._filters:
            self._filters[key].reset()

    def clear_all(self):
        self._filters.clear()


# \u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500
# GLOBAL REGISTRY (process-level singleton)
# \u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500

_global_registry = RiderKalmanRegistry()


def get_registry() -> RiderKalmanRegistry:
    """Get the process-level rider Kalman filter registry."""
    return _global_registry


# \u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500
# STATELESS COORDINATE SMOOTHER
# \u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500

def smooth_coordinates(
    lat: float,
    lon: float,
    *,
    prior_lat: Optional[float] = None,
    prior_lon: Optional[float] = None,
    process_noise: float = 1e-4,
    measurement_noise: float = 0.01,
) -> Tuple[float, float]:
    """
    Stateless single-shot GPS smoother.
    If a prior is provided, blends the new reading towards it.
    """
    f = GPSKalmanFilter(process_noise=process_noise, measurement_noise=measurement_noise)
    if prior_lat is not None and prior_lon is not None:
        try:
            _flat = float(prior_lat)
            _flon = float(prior_lon)
            if GPSKalmanFilter._is_valid_coord(_flat, _flon):
                f.update(_flat, _flon)
        except (TypeError, ValueError):
            pass
    return f.update(lat, lon)


# \u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500
# BATCH SMOOTHERS
# \u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500

def smooth_rider_locations(riders: list) -> list:
    """
    Apply Kalman smoothing to a list of rider dicts in-place using
    the global per-rider registry (history preserved across calls).

    Reads/writes: latitude, longitude (and currentlat/currentlong if present).
    Adds: _kalman_smoothed = True on each processed rider.
    """
    registry = get_registry()
    for rider in riders:
        try:
            rider_id = (
                rider.get("userid") or rider.get("riderid") or
                rider.get("id") or "unknown"
            )
            raw_lat = float(rider.get("latitude") or rider.get("currentlat") or 0)
            raw_lon = float(rider.get("longitude") or rider.get("currentlong") or 0)
            if raw_lat == 0.0 and raw_lon == 0.0:
                continue
            smooth_lat, smooth_lon = registry.update(rider_id, raw_lat, raw_lon)
            # Cast back to string for Go compatibility
            s_lat, s_lon = str(round(smooth_lat, 8)), str(round(smooth_lon, 8))
            rider["latitude"] = s_lat
            rider["longitude"] = s_lon
            if "currentlat" in rider:
                rider["currentlat"] = s_lat
            if "currentlong" in rider:
                rider["currentlong"] = s_lon
            rider["_kalman_smoothed"] = True
        except Exception as e:
            logger.debug(f"Kalman rider smoothing skipped: {e}")
    return riders


def smooth_order_coordinates(orders: list) -> list:
    """
    Validate and lightly normalise delivery coordinates in a list of order dicts.

    DESIGN NOTE — why we do NOT use Kalman filtering here:
    ─────────────────────────────────────────────────────
    Kalman filtering is a *temporal* smoother: it blends successive measurements
    from the same sensor over time. Delivery coordinates are a single static
    point (one measurement). Feeding the kitchen location as a "prior" would
    pull the customer's address toward the kitchen — exactly wrong.

    Per-customer GPS accuracy is handled upstream by the FAISS coordinate store
    (verified historical rider-confirmed delivery points). This function only
    normalises the coordinate fields to floats so downstream code never sees
    raw strings or None values.

    Modifies orders in-place. Returns the same list.
    """
    for order in orders:
        try:
            dlat_raw = order.get("deliverylat") or order.get("droplat")
            dlon_raw = order.get("deliverylong") or order.get("droplon")
            if dlat_raw is None or dlon_raw is None:
                continue
            dlat = float(dlat_raw)
            dlon = float(dlon_raw)
            if not GPSKalmanFilter._is_valid_coord(dlat, dlon):
                continue
            # Normalise to string (Go service expects string coordinates)
            s_lat = str(round(dlat, 8))
            s_lon = str(round(dlon, 8))
            order["deliverylat"] = s_lat
            order["deliverylong"] = s_lon
            if "droplat" in order:
                order["droplat"] = s_lat
            if "droplon" in order:
                order["droplon"] = s_lon
        except Exception as e:
            logger.debug(f"Coordinate normalisation skipped: {e}")
    return orders
