geo.distance

/**
 * Great-circle distance between two points, by the haversine formula, on a
 * sphere of the IUGG mean Earth radius, in metres rounded to millimetres.
 *
 * Why this does not call Math.sin, Math.cos or Math.atan2: those come from
 * each platform's maths library, which is allowed to differ in the last bit
 * between JavaScript, Python and Rust. Rounding afterwards does not fully hide
 * that, because a value that lands next to a rounding boundary can go either
 * way. The trigonometry below uses only operations IEEE 754 requires to be
 * correctly rounded (+ - * / sqrt floor), in a fixed order, so the unrounded
 * result is the same double in every language, and the rounding is then only
 * presentation.
 */

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

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

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

// sin and cos on |r| <= pi/4, where the series converge to well below an ulp.
function sinSmall(r: number): number {
  const s = r * r;
  let 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;
}

function cosSmall(r: number): number {
  const s = r * r;
  let 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;
}

// Reduce to a quarter turn k and a remainder |r| <= pi/4.
function sinCos(x: number): [number, number] {
  const k = Math.floor(x / HALF_PI + 0.5);
  const r = x - k * HALF_PI;
  const sr = sinSmall(r);
  const cr = cosSmall(r);
  const quadrant = ((k % 4) + 4) % 4;
  if (quadrant === 0) return [sr, cr];
  if (quadrant === 1) return [cr, -sr];
  if (quadrant === 2) return [-sr, -cr];
  return [-cr, sr];
}

// atan for 0 <= u <= 1: halve the angle three times (tan(t/2) = u / (1 + sqrt(1 + u^2)))
// so the series runs on |u| <= tan(pi/32), then scale back by 8.
function atanUnit(u: number): number {
  let 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));
  const s = v * v;
  let 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);
}

// atan2 for y >= 0 and x >= 0, which is all haversine needs.
function atan2Positive(y: number, x: number): number {
  if (x === 0) return y === 0 ? 0 : HALF_PI;
  const t = y / x;
  return t > 1 ? HALF_PI - atanUnit(x / y) : atanUnit(t);
}

// Half away from zero on the binary64 value; + 0 turns -0 into 0.
function roundTo(x: number, scale: number): number {
  const y = Math.abs(x) * scale;
  let r = Math.floor(y);
  if (y - r >= 0.5) r += 1;
  const out = r / scale;
  return (x < 0 ? -out : out) + 0;
}

function checkLatitude(value: number): void {
  if (typeof value !== "number" || !Number.isFinite(value)) {
    throw new TypeError("latitude must be a finite number of degrees");
  }
  if (value < -90 || value > 90) throw new RangeError("latitude must be between -90 and 90 degrees");
}

function checkLongitude(value: number): void {
  if (typeof value !== "number" || !Number.isFinite(value)) {
    throw new TypeError("longitude must be a finite number of degrees");
  }
  if (value < -180 || value > 180) throw new RangeError("longitude must be between -180 and 180 degrees");
}

export function distance(fromLat: number, fromLng: number, toLat: number, toLng: number): number {
  checkLatitude(fromLat);
  checkLongitude(fromLng);
  checkLatitude(toLat);
  checkLongitude(toLng);

  const phi1 = fromLat * DEGREES;
  const phi2 = toLat * 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, so no
  // wrapping is needed.
  const [sinHalfLat] = sinCos(((toLat - fromLat) * DEGREES) / 2);
  const [sinHalfLng] = sinCos(((toLng - fromLng) * DEGREES) / 2);
  const [, cos1] = sinCos(phi1);
  const [, cos2] = sinCos(phi2);

  let a = sinHalfLat * sinHalfLat + cos1 * cos2 * sinHalfLng * sinHalfLng;
  // Rounding can push a a hair outside [0, 1] for antipodal points.
  if (a < 0) a = 0;
  if (a > 1) a = 1;
  // atan2 rather than asin(sqrt(a)): stays accurate for antipodal points too.
  const c = 2 * atan2Positive(Math.sqrt(a), Math.sqrt(1 - a));
  return roundTo(EARTH_RADIUS_METRES * c, 1000);
}