rcnn/web/src/maplibre/geoUtils.ts

import * as THREE from 'three'

// WGS-84 ellipsoid constants
const A  = 6378137.0          // semi-major axis (m)
const E2 = 0.00669437999014   // first eccentricity squared

/**
 * Convert geographic coordinates to ECEF (Earth-Centred Earth-Fixed) metres.
 */
export function lngLatAltToECEF(lon: number, lat: number, alt: number): THREE.Vector3 {
  const lonR = lon * Math.PI / 180
  const latR = lat * Math.PI / 180
  const sinLat = Math.sin(latR)
  const cosLat = Math.cos(latR)
  const N = A / Math.sqrt(1 - E2 * sinLat * sinLat)
  return new THREE.Vector3(
    (N + alt) * cosLat * Math.cos(lonR),
    (N + alt) * cosLat * Math.sin(lonR),
    (N * (1 - E2) + alt) * sinLat,
  )
}

/**
 * Convert ECEF metres back to [longitude, latitude, altitude].
 * Uses Bowring's iterative method (5 iterations → sub-mm accuracy).
 */
export function ecefToLngLatAlt(v: THREE.Vector3): [number, number, number] {
  const p = Math.sqrt(v.x * v.x + v.y * v.y)
  const lon = Math.atan2(v.y, v.x) * 180 / Math.PI
  let lat = Math.atan2(v.z, p * (1 - E2))
  for (let i = 0; i < 5; i++) {
    const sinLat = Math.sin(lat)
    const N = A / Math.sqrt(1 - E2 * sinLat * sinLat)
    lat = Math.atan2(v.z + E2 * N * sinLat, p)
  }
  const sinLat = Math.sin(lat)
  const N = A / Math.sqrt(1 - E2 * sinLat * sinLat)
  const alt = p / Math.cos(lat) - N
  return [lon, lat * 180 / Math.PI, alt]
}

/**
 * Convert a point from ECEF to ENU coordinates relative to an origin.
 * ENU: X=East, Y=North, Z=Up
 */
export function ecefToEnu(
  point: THREE.Vector3,
  origin: [number, number, number],
): THREE.Vector3 {
  const [oLon, oLat, oAlt] = origin
  const org = lngLatAltToECEF(oLon, oLat, oAlt)
  const diff = point.clone().sub(org)

  const oLonR = oLon * Math.PI / 180
  const oLatR = oLat * Math.PI / 180
  // ENU basis vectors in ECEF
  const east  = new THREE.Vector3(-Math.sin(oLonR), Math.cos(oLonR), 0)
  const north = new THREE.Vector3(
    -Math.sin(oLatR) * Math.cos(oLonR),
    -Math.sin(oLatR) * Math.sin(oLonR),
     Math.cos(oLatR),
  )
  const up = new THREE.Vector3(
    Math.cos(oLatR) * Math.cos(oLonR),
    Math.cos(oLatR) * Math.sin(oLonR),
    Math.sin(oLatR),
  )

  return new THREE.Vector3(diff.dot(east), diff.dot(north), diff.dot(up))
}

/**
 * Build a Three.js Matrix4 that positions and orients a local scene
 * at the given geographic coordinate (ENU → ECEF transform + heading rotation).
 *
 * @param lon       Longitude in degrees
 * @param lat       Latitude in degrees
 * @param alt       Altitude in metres above WGS-84 ellipsoid
 * @param headingDeg  Clockwise heading in degrees (0 = North)
 */
export function buildSplatWorldMatrix(
  lon: number,
  lat: number,
  alt: number,
  headingDeg: number,
): THREE.Matrix4 {
  const lonR = lon * Math.PI / 180
  const latR = lat * Math.PI / 180

  // ENU basis vectors in ECEF
  const east  = new THREE.Vector3(-Math.sin(lonR), Math.cos(lonR), 0)
  const north = new THREE.Vector3(
    -Math.sin(latR) * Math.cos(lonR),
    -Math.sin(latR) * Math.sin(lonR),
     Math.cos(latR),
  )
  const up = new THREE.Vector3(
    Math.cos(latR) * Math.cos(lonR),
    Math.cos(latR) * Math.sin(lonR),
    Math.sin(latR),
  )

  // Apply heading rotation (clockwise from North = negative rotation around Up/Z-ENU)
  const hRad = -headingDeg * Math.PI / 180
  const cosH = Math.cos(hRad)
  const sinH = Math.sin(hRad)

  // Rotated ENU: X' = cosH*East + sinH*North, Y' = -sinH*East + cosH*North, Z' = Up
  const xAxis = east.clone().multiplyScalar(cosH).addScaledVector(north, sinH)
  const yAxis = east.clone().multiplyScalar(-sinH).addScaledVector(north, cosH)
  const zAxis = up.clone()

  const pos = lngLatAltToECEF(lon, lat, alt)

  // Column-major Matrix4 (Three.js): each column is [x, y, z, 0], last col is translation
  return new THREE.Matrix4().set(
    xAxis.x, yAxis.x, zAxis.x, pos.x,
    xAxis.y, yAxis.y, zAxis.y, pos.y,
    xAxis.z, yAxis.z, zAxis.z, pos.z,
    0,       0,       0,       1,
  )
}

/**
 * Estimate MapLibre camera altitude above terrain in metres.
 * Falls back to zoom-based approximation when transform internals are unavailable.
 */
export function getMapCameraAltitude(map: { getZoom(): number; getCenter(): { lat: number }; transform?: unknown }): number {
  const t = (map as { transform?: { cameraToCenterDistance?: number; pixelsPerMeter?: number; altitude?: number } }).transform
  if (t) {
    if (typeof t.altitude === 'number') return t.altitude
    if (t.cameraToCenterDistance && t.pixelsPerMeter) {
      return t.cameraToCenterDistance / t.pixelsPerMeter
    }
  }
  // Fallback: approximate from zoom (rough but covers our visibility thresholds)
  const zoom = map.getZoom()
  const lat  = map.getCenter().lat
  return (40075016.7 * Math.cos(lat * Math.PI / 180)) / Math.pow(2, zoom + 8) * 600
}

/**
 * Remove MapLibre layers and sources without throwing if the map was already
 * destroyed (map.remove() sets this.style to undefined internally).
 */
export function safeRemoveLayers(
  map: {
    getLayer:    (id: string) => unknown
    removeLayer: (id: string) => void
    getSource:   (id: string) => unknown
    removeSource:(id: string) => void
  },
  layerIds:  string[],
  sourceIds: string[],
) {
  try {
    for (const id of layerIds)  { if (map.getLayer(id))  map.removeLayer(id) }
    for (const id of sourceIds) { if (map.getSource(id)) map.removeSource(id) }
  } catch {
    // Map was already destroyed before this cleanup ran — safe to ignore.
  }
}