geo.distance

import math
from typing import Tuple

# Why this does not call math.sin, math.cos or math.atan2: those come from the
# platform maths library, which may differ in the last bit between Python,
# JavaScript and Rust, and a value next to a rounding boundary can then round
# differently. The trigonometry below uses only operations IEEE 754 requires to
# be correctly rounded (+ - * / sqrt floor), in the same order as the other two
# implementations, so the unrounded result is the same double everywhere.

# IUGG mean radius R1 = (2a + b) / 3 of the GRS 80 ellipsoid (Moritz,
# "Geodetic Reference System 1980", Journal of Geodesy 74 (2000) 128-133).
EARTH_RADIUS_METRES = 6371008.8

PI = 3.141592653589793
HALF_PI = PI / 2
DEGREES = PI / 180

S3, S5, S7, S9, S11 = -1 / 6, 1 / 120, -1 / 5040, 1 / 362880, -1 / 39916800
S13, S15, S17 = 1 / 6227020800, -1 / 1307674368000, 1 / 355687428096000
C2, C4, C6, C8, C10 = -1 / 2, 1 / 24, -1 / 720, 1 / 40320, -1 / 3628800
C12, C14, C16, C18 = 1 / 479001600, -1 / 87178291200, 1 / 20922789888000, -1 / 6402373705728000
A3, A5, A7, A9, A11 = -1 / 3, 1 / 5, -1 / 7, 1 / 9, -1 / 11
A13, A15, A17, A19, A21 = 1 / 13, -1 / 15, 1 / 17, -1 / 19, 1 / 21


def _sin_small(r: float) -> float:
    s = r * r
    p = S17
    p = S15 + s * p
    p = S13 + s * p
    p = S11 + s * p
    p = S9 + s * p
    p = S7 + s * p
    p = S5 + s * p
    p = S3 + s * p
    return r + r * s * p


def _cos_small(r: float) -> float:
    s = r * r
    p = C18
    p = C16 + s * p
    p = C14 + s * p
    p = C12 + s * p
    p = C10 + s * p
    p = C8 + s * p
    p = C6 + s * p
    p = C4 + s * p
    p = C2 + s * p
    return 1 + s * p


def _sin_cos(x: float) -> Tuple[float, float]:
    k = math.floor(x / HALF_PI + 0.5)
    r = x - float(k) * HALF_PI
    sr = _sin_small(r)
    cr = _cos_small(r)
    quadrant = k % 4
    if quadrant == 0:
        return sr, cr
    if quadrant == 1:
        return cr, -sr
    if quadrant == 2:
        return -sr, -cr
    return -cr, sr


def _atan_unit(u: float) -> float:
    v = u
    v = v / (1 + math.sqrt(1 + v * v))
    v = v / (1 + math.sqrt(1 + v * v))
    v = v / (1 + math.sqrt(1 + v * v))
    s = v * v
    p = A21
    p = A19 + s * p
    p = A17 + s * p
    p = A15 + s * p
    p = A13 + s * p
    p = A11 + s * p
    p = A9 + s * p
    p = A7 + s * p
    p = A5 + s * p
    p = A3 + s * p
    return 8 * (v + v * s * p)


def _atan2_positive(y: float, x: float) -> float:
    if x == 0:
        return 0.0 if y == 0 else HALF_PI
    t = y / x
    return HALF_PI - _atan_unit(x / y) if t > 1 else _atan_unit(t)


def _round_to(x: float, scale: float) -> float:
    # Half away from zero on the binary64 value, not Python's round(), which
    # rounds half to even. + 0.0 turns -0.0 into 0.0.
    y = abs(x) * scale
    r = float(math.floor(y))
    if y - r >= 0.5:
        r += 1
    out = r / scale
    return (-out if x < 0 else out) + 0.0


def _is_number(value: object) -> bool:
    return isinstance(value, (int, float)) and not isinstance(value, bool) and math.isfinite(value)


def _check_latitude(value: float) -> None:
    if not _is_number(value):
        raise TypeError("latitude must be a finite number of degrees")
    if value < -90 or value > 90:
        raise ValueError("latitude must be between -90 and 90 degrees")


def _check_longitude(value: float) -> None:
    if not _is_number(value):
        raise TypeError("longitude must be a finite number of degrees")
    if value < -180 or value > 180:
        raise ValueError("longitude must be between -180 and 180 degrees")


def distance(from_lat: float, from_lng: float, to_lat: float, to_lng: float) -> float:
    """Great-circle distance in metres (haversine, IUGG mean radius), to the millimetre."""
    _check_latitude(from_lat)
    _check_longitude(from_lng)
    _check_latitude(to_lat)
    _check_longitude(to_lng)
    from_lat, from_lng, to_lat, to_lng = float(from_lat), float(from_lng), float(to_lat), float(to_lng)

    phi1 = from_lat * DEGREES
    phi2 = to_lat * DEGREES
    # A longitude difference of 359 degrees is a 1 degree step across the
    # antimeridian; sin^2 of the half-angle is the same either way.
    sin_half_lat = _sin_cos(((to_lat - from_lat) * DEGREES) / 2)[0]
    sin_half_lng = _sin_cos(((to_lng - from_lng) * DEGREES) / 2)[0]
    cos1 = _sin_cos(phi1)[1]
    cos2 = _sin_cos(phi2)[1]

    a = sin_half_lat * sin_half_lat + cos1 * cos2 * sin_half_lng * sin_half_lng
    if a < 0:
        a = 0.0
    if a > 1:
        a = 1.0
    # atan2 rather than asin(sqrt(a)): stays accurate for antipodal points too.
    c = 2 * _atan2_positive(math.sqrt(a), math.sqrt(1 - a))
    return _round_to(EARTH_RADIUS_METRES * c, 1000.0)