"""Vicenty's formula for Earth surface distance.

https://en.wikipedia.org/wiki/Vincenty's_formulae

An interative and more computationally complex distance calculation for the WGS-84
ellipsoid. It can calculate the surface distance as well as the forward and reverse
azimuth angles across between two points.

It is considered to be highly accurate and is often the distance metric to use as a
benchmark to compare other distance metrics.
"""

from __future__ import annotations

import math
from typing import TYPE_CHECKING

from gri_utils import constants

if TYPE_CHECKING:
    from collections.abc import Sequence

    import numpy as np
    from numpy.typing import NDArray


def surface_dist_vincenty(
    lla1: NDArray[np.floating] | Sequence,
    lla2: NDArray[np.floating] | Sequence,
    *,
    converge_m: float = 0.1,
) -> float:
    """Vicenty's formula for distance.

    Convergence by default is set to 0.1m, meaning the last step in delta lambda was
    less than that. This is a bit of an approximation arrived at by this formula:

    0.1m / 111137m/deg * pi/180 = 1.57043e-8 radians

    This is considered highly accurate for the WGS-84 ellipsoid and converges fairly
    rapidly to .1m (3 iterations on average).

    Args:
        lla1(NDArray[np.floating]|Sequence): Array with Lat(Deg), Lon(Deg) as first two
            elements
        lla2(NDArray[np.floating]|Sequence): Array with Lat(Deg), Lon(Deg) as first two
            elements
        converge_m(float): approximate convergence, default is 0.1 (meters)

    Returns:
        float: meters
    """
    lat1_rad = math.radians(lla1[0])
    lon1_rad = math.radians(lla1[1])
    lat2_rad = math.radians(lla2[0])
    lon2_rad = math.radians(lla2[1])
    delta_lon = lon2_rad - lon1_rad
    u1 = math.atan((1 - constants.FLATTENING) * math.tan(lat1_rad))
    u2 = math.atan((1 - constants.FLATTENING) * math.tan(lat2_rad))
    sin_u1 = math.sin(u1)
    cos_u1 = math.cos(u1)
    sin_u2 = math.sin(u2)
    cos_u2 = math.cos(u2)

    converge = converge_m / math.degrees(111137)
    lam = delta_lon
    iterations = 0
    max_iter = 10
    delta = 1
    while iterations < max_iter:
        iterations += 1
        sin_lam = math.sin(lam)
        cos_lam = math.cos(lam)
        sin_sigma = math.sqrt(
            (cos_u2 * sin_lam) ** 2
            + (cos_u1 * sin_u2 - sin_u1 * cos_u2 * cos_lam) ** 2,
        )
        if sin_sigma == 0:
            return 0  # co-incident points
        cos_sigma = sin_u1 * sin_u2 + cos_u1 * cos_u2 * cos_lam
        sigma = math.atan2(sin_sigma, cos_sigma)
        sin_alpha = cos_u1 * cos_u2 * sin_lam / sin_sigma
        cos2_alpha = 1 - sin_alpha**2
        if cos2_alpha == 0:
            cos_2_sig_m = 0
        else:
            cos_2_sig_m = cos_sigma - 2 * sin_u1 * sin_u2 / cos2_alpha
        c = (
            constants.FLATTENING
            / 16
            * cos2_alpha
            * (4 + constants.FLATTENING * (4 - 3 * cos2_alpha))
        )
        lam_prev = lam
        lam = delta_lon + (1 - c) * constants.FLATTENING * sin_alpha * (
            sigma
            + c * sin_sigma * (cos_2_sig_m + c * cos_sigma * (-1 + 2 * cos_2_sig_m**2))
        )
        delta = abs(lam - lam_prev)

        if delta <= converge:
            # Converged!
            # print(f"{iterations} iterations")
            u2 = (
                cos2_alpha * (constants.ER_M**2 - constants.PR_M**2) / constants.PR_M**2
            )
            a = 1 + u2 / 16384 * (4096 + u2 * (-768 + u2 * (320 - 175 * u2)))
            b = u2 / 1024 * (256 + u2 * (-128 + u2 * (74 - 47 * u2)))
            delta_sigma = (
                b
                * sin_sigma
                * (
                    cos_2_sig_m
                    + b
                    / 4
                    * (
                        cos_sigma * (-1 + 2 * cos_2_sig_m**2)
                        - b
                        / 6
                        * cos_2_sig_m
                        * (-3 + 4 * sin_sigma**2)
                        * (-3 + 4 * cos_2_sig_m**2)
                    )
                )
            )
            return constants.PR_M * a * (sigma - delta_sigma)
            # Can also get forward and reverse azimuth here
    raise ValueError("Failed to Converge")
