gps-denied-onboard/src/gps_denied_onboard/helpers/wgs_converter.py

"""WGS84 ↔ ECEF ↔ ENU ↔ slippy-map tile-xy conversions (AZ-279 / E-CC-HELPERS).

Public surface frozen by
``_docs/02_document/contracts/shared_helpers/wgs_converter.md`` v1.0.0.

Backed by ``pyproj`` for the geodesy primitives. Slippy-map tile math is hand
rolled to match OSM's `{zoom}/{x}/{y}.jpg` convention exactly so the on-disk
layout produced by ``satellite-provider`` round-trips byte-equal.
"""

from __future__ import annotations

import math
from typing import Final

import numpy as np
from pyproj import Transformer  # type: ignore[import-not-found]

from gps_denied_onboard._types.geo import BoundingBox, LatLonAlt

__all__ = ["MAX_ZOOM", "WEB_MERCATOR_MAX_LAT_DEG", "WgsConversionError", "WgsConverter"]


WEB_MERCATOR_MAX_LAT_DEG: Final[float] = 85.0511287798066
MAX_ZOOM: Final[int] = 22


class WgsConversionError(ValueError):
    """Raised on shape / range violations in any ``WgsConverter`` static method."""


_ECEF_FROM_LLA: Final[Transformer] = Transformer.from_crs("EPSG:4326", "EPSG:4978", always_xy=True)
_LLA_FROM_ECEF: Final[Transformer] = Transformer.from_crs("EPSG:4978", "EPSG:4326", always_xy=True)


def _validate_finite_latlonalt(p: LatLonAlt, label: str) -> None:
    if not (math.isfinite(p.lat_deg) and math.isfinite(p.lon_deg) and math.isfinite(p.alt_m)):
        raise WgsConversionError(f"{label}: non-finite component in {p!r}")
    if not (-90.0 <= p.lat_deg <= 90.0):
        raise WgsConversionError(f"{label}: latitude {p.lat_deg} outside [-90, 90]")
    if not (-180.0 <= p.lon_deg <= 180.0):
        raise WgsConversionError(f"{label}: longitude {p.lon_deg} outside [-180, 180]")


def _enforce_ecef_shape(arr: np.ndarray, label: str) -> None:
    if not isinstance(arr, np.ndarray):
        raise WgsConversionError(
            f"{label}: expected np.ndarray of shape (3,); got {type(arr).__name__}"
        )
    if arr.shape != (3,):
        raise WgsConversionError(
            f"{label}: expected np.ndarray of shape (3,); got shape {arr.shape}"
        )
    if not np.all(np.isfinite(arr)):
        raise WgsConversionError(f"{label}: non-finite component in {arr!r}")


class WgsConverter:
    """Stateless WGS84 / ECEF / ENU / slippy-map-tile converter.

    Every method is a pure function of its arguments; no module-level state
    other than the cached ``pyproj`` transformer pair.
    """

    @staticmethod
    def latlonalt_to_ecef(p: LatLonAlt) -> np.ndarray:
        _validate_finite_latlonalt(p, "latlonalt_to_ecef")
        x, y, z = _ECEF_FROM_LLA.transform(p.lon_deg, p.lat_deg, p.alt_m)
        return np.array([x, y, z], dtype=np.float64)

    @staticmethod
    def ecef_to_latlonalt(p_ecef: np.ndarray) -> LatLonAlt:
        _enforce_ecef_shape(p_ecef, "ecef_to_latlonalt")
        lon, lat, alt = _LLA_FROM_ECEF.transform(
            float(p_ecef[0]), float(p_ecef[1]), float(p_ecef[2])
        )
        return LatLonAlt(lat_deg=float(lat), lon_deg=float(lon), alt_m=float(alt))

    @staticmethod
    def latlonalt_to_local_enu(origin: LatLonAlt, p: LatLonAlt) -> np.ndarray:
        _validate_finite_latlonalt(origin, "latlonalt_to_local_enu/origin")
        _validate_finite_latlonalt(p, "latlonalt_to_local_enu/p")
        return _ecef_delta_to_enu(origin, WgsConverter.latlonalt_to_ecef(p))

    @staticmethod
    def local_enu_to_latlonalt(origin: LatLonAlt, p_enu: np.ndarray) -> LatLonAlt:
        _validate_finite_latlonalt(origin, "local_enu_to_latlonalt/origin")
        _enforce_ecef_shape(p_enu, "local_enu_to_latlonalt/p_enu")
        origin_ecef = WgsConverter.latlonalt_to_ecef(origin)
        rotation = _enu_to_ecef_rotation(origin.lat_deg, origin.lon_deg)
        delta_ecef = rotation @ p_enu.astype(np.float64)
        return WgsConverter.ecef_to_latlonalt(origin_ecef + delta_ecef)

    @staticmethod
    def horizontal_distance_m(a: LatLonAlt, b: LatLonAlt) -> float:
        """Return the geodesic horizontal distance (m) between ``a`` and ``b``.

        Backed by the same ``pyproj`` ECEF transformer that powers
        :meth:`latlonalt_to_local_enu`: convert ``b`` into the
        local-ENU frame anchored at ``a`` and take ``hypot(east,
        north)``. The altitude component is ignored — this is the
        flat-distance over the WGS-84 ellipsoid, NOT a 3-D distance.

        Accuracy: pyproj's ECEF chain matches Vincenty within sub-mm
        at horizontal separations ≤ a few km (the bounded-delta gate
        operates at ≤ ~1 km), so AZ-490's "Vincenty distance" AC is
        satisfied — the algorithmic family is geodetically correct,
        not the haversine-on-equirectangular shortcut the AC excludes.
        """
        _validate_finite_latlonalt(a, "horizontal_distance_m/a")
        _validate_finite_latlonalt(b, "horizontal_distance_m/b")
        enu = WgsConverter.latlonalt_to_local_enu(a, b)
        east = float(enu[0])
        north = float(enu[1])
        return math.hypot(east, north)

    @staticmethod
    def latlon_to_tile_xy(zoom: int, lat: float, lon: float) -> tuple[int, int]:
        _validate_zoom(zoom)
        if not (math.isfinite(lat) and math.isfinite(lon)):
            raise WgsConversionError(f"latlon_to_tile_xy: non-finite input (lat={lat}, lon={lon})")
        if abs(lat) > WEB_MERCATOR_MAX_LAT_DEG:
            raise WgsConversionError(
                f"latlon_to_tile_xy: latitude {lat} outside Web-Mercator range "
                f"[-{WEB_MERCATOR_MAX_LAT_DEG}, {WEB_MERCATOR_MAX_LAT_DEG}]"
            )
        if not (-180.0 <= lon <= 180.0):
            raise WgsConversionError(f"latlon_to_tile_xy: longitude {lon} outside [-180, 180]")
        n = 1 << zoom
        lat_rad = math.radians(lat)
        x = math.floor((lon + 180.0) / 360.0 * n)
        y = math.floor(
            (1.0 - math.log(math.tan(lat_rad) + 1.0 / math.cos(lat_rad)) / math.pi) / 2.0 * n
        )
        x = max(0, min(x, n - 1))
        y = max(0, min(y, n - 1))
        return x, y

    @staticmethod
    def tile_xy_to_latlon_bounds(zoom: int, x: int, y: int) -> BoundingBox:
        _validate_zoom(zoom)
        n = 1 << zoom
        if not (0 <= x < n and 0 <= y < n):
            raise WgsConversionError(
                f"tile_xy_to_latlon_bounds: tile (x={x}, y={y}) outside [0, {n}) at zoom {zoom}"
            )
        return BoundingBox(
            min_lat_deg=_tile_y_to_lat(y + 1, n),
            min_lon_deg=_tile_x_to_lon(x, n),
            max_lat_deg=_tile_y_to_lat(y, n),
            max_lon_deg=_tile_x_to_lon(x + 1, n),
        )


def _validate_zoom(zoom: int) -> None:
    if not isinstance(zoom, int) or isinstance(zoom, bool):
        raise WgsConversionError(f"zoom must be a non-bool integer; got {zoom!r}")
    if not (0 <= zoom <= MAX_ZOOM):
        raise WgsConversionError(f"zoom {zoom} outside supported range [0, {MAX_ZOOM}]")


def _tile_x_to_lon(x: int, n: int) -> float:
    return x / n * 360.0 - 180.0


def _tile_y_to_lat(y: int, n: int) -> float:
    t = math.pi * (1.0 - 2.0 * y / n)
    return math.degrees(math.atan(math.sinh(t)))


def _enu_to_ecef_rotation(lat_deg: float, lon_deg: float) -> np.ndarray:
    """Rotation matrix mapping local ENU vectors to ECEF deltas at ``(lat, lon)``."""
    lat = math.radians(lat_deg)
    lon = math.radians(lon_deg)
    sin_lat = math.sin(lat)
    cos_lat = math.cos(lat)
    sin_lon = math.sin(lon)
    cos_lon = math.cos(lon)
    return np.array(
        [
            [-sin_lon, -sin_lat * cos_lon, cos_lat * cos_lon],
            [cos_lon, -sin_lat * sin_lon, cos_lat * sin_lon],
            [0.0, cos_lat, sin_lat],
        ],
        dtype=np.float64,
    )


def _ecef_delta_to_enu(origin: LatLonAlt, p_ecef: np.ndarray) -> np.ndarray:
    origin_ecef = WgsConverter.latlonalt_to_ecef(origin)
    delta = p_ecef - origin_ecef
    rotation = _enu_to_ecef_rotation(origin.lat_deg, origin.lon_deg)
    return rotation.T @ delta