"""Lambert's formula for long lines (surface distance).

https://en.wikipedia.org/wiki/Geographical_distance#Lambert's_formula_for_long_lines

Lambert uses a reduced latitude in a first-order reduction to get far greater accuracy
than haversine.
"""

from __future__ import annotations

import math
from typing import TYPE_CHECKING

from gri_utils import constants, distance

if TYPE_CHECKING:
    from collections.abc import Sequence

    import numpy as np


def surface_dist_lambert_m(
    lla1: np.ndarray | Sequence,
    lla2: np.ndarray | Sequence,
) -> float:
    """Lambert's distance formula.

    Args:
        lla1(np.ndarray|Sequence): Array with Lat(Deg), Lon(Deg) as first two elements
        lla2(np.ndarray|Sequence): Array with Lat(Deg), Lon(Deg) as first two elements

    Returns:
        float: meters
    """
    lat1_rad = math.radians(lla1[0])
    lat2_rad = math.radians(lla2[0])
    b1 = math.atan((1 - constants.FLATTENING) * math.tan(lat1_rad))
    b2 = math.atan((1 - constants.FLATTENING) * math.tan(lat2_rad))
    p = (b1 + b2) / 2
    q = (b2 - b1) / 2
    sig = distance.central_angle(lla1, lla2)
    if sig == 0:
        return 0
    x = (
        (sig - math.sin(sig))
        * math.sin(p) ** 2
        * math.cos(q) ** 2
        / math.cos(sig / 2) ** 2
    )
    y = (
        (sig + math.sin(sig))
        * math.cos(p) ** 2
        * math.sin(q) ** 2
        / math.sin(sig / 2) ** 2
    )
    return float(constants.ER_M * (sig - constants.FLATTENING / 2 * (x + y)))
