import numpy as np
from numpy import sqrt, arctan2, sin, cos, arcsin, arccos
from numpy.typing import NDArray
from typing import Tuple
import winkelumrechnungen as wu


def cart2sph(point: NDArray) -> Tuple[float, float, float]:
    x, y, z = point
    r = sqrt(x**2 + y**2 + z**2)
    phi = arctan2(z, sqrt(x**2 + y**2))
    lamb = arctan2(y, x)

    return r, phi, lamb


def sph2cart(r: float, phi: float, lamb: float) -> NDArray:
    x = r * cos(phi) * cos(lamb)
    y = r * cos(phi) * sin(lamb)
    z = r * sin(phi)

    return np.array([x, y, z])


def gha1(R, phi0, lamb0, s, alpha0):
    s_ = s / R

    lamb1 = lamb0 + arctan2(sin(s_) * sin(alpha0),
                            cos(phi0) * cos(s_) - sin(phi0) * sin(s_) * cos(alpha0))

    phi1 = arcsin(sin(phi0) * cos(s_) + cos(phi0) * sin(s_) * cos(alpha0))

    return phi1, lamb1


def gha2(R, phi0, lamb0, phi1, lamb1):
    s_ = arccos(sin(phi0) * sin(phi1) + cos(phi0) * cos(phi1) * cos(lamb1 - lamb0))
    s = R * s_

    alpha0 = arctan2(cos(phi1) * sin(lamb1 - lamb0),
                     cos(phi0) * sin(phi1) - sin(phi0) * cos(phi1) * cos(lamb1 - lamb0))

    alpha1 = arctan2(-cos(phi0) * sin(lamb1 - lamb0),
                     cos(phi1) * sin(phi0) - sin(phi1) * cos(phi0) * cos(lamb1 - lamb0))
    if alpha1 < 0:
        alpha1 += 2 * np.pi

    return alpha0, alpha1, s


if __name__ == "__main__":
    R = 6378815.904                                             # Bern
    phi0 = wu.deg2rad(10)
    lamb0 = wu.deg2rad(40)
    alpha0 = wu.deg2rad(100)
    s = 10000
    phi1, lamb1 = gha1(R, phi0, lamb0, s, alpha0)
    alpha0_g, alpha1, s_g = gha2(R, phi0, lamb0, phi1, lamb1)
    phi1 = wu.rad2deg(phi1)
    lamb1 = wu.rad2deg(lamb1)
    alpha0_g = wu.rad2deg(alpha0_g)
    alpha1 = wu.rad2deg(alpha1)

    pass