import numpy as np


def cart2sph(x, y, z):
    r = np.sqrt(x**2 + y**2 + z**2)
    phi = np.atan2(z, np.sqrt(x**2 + y**2))
    lamb = np.atan2(y, x)

    return r, np.rad2deg(phi), np.rad2deg(lamb)


def sph2cart(r, phi, lamb):
    phi_rad = np.deg2rad(phi)
    lamb_rad = np.deg2rad(lamb)

    x = r * np.cos(phi_rad) * np.cos(lamb_rad)
    y = r * np.cos(phi_rad) * np.sin(lamb_rad)
    z = r * np.sin(phi_rad)

    return x, y, z


def kugel_erste_gha(R, phi_1, lamb_1, s_d, a_12):

    s = s_d / R
    lamb_1_rad = np.deg2rad(lamb_1)
    phi_1_rad = np.deg2rad(phi_1)
    a_12_rad = np.deg2rad(a_12)

    sin_satz = np.sin(s) * np.sin(a_12_rad)
    sin_kos = np.cos(phi_1_rad) * np.cos(s) - np.sin(phi_1_rad) * np.sin(s) * np.cos(a_12_rad)
    delta_lam = np.atan2(sin_satz, sin_kos)
    lamb_2_rad = lamb_1_rad + delta_lam

    phi_2_rad = np.asin(np.sin(phi_1_rad) * np.cos(s) + np.cos(phi_1_rad) * np.sin(s) * np.cos(a_12_rad))

    sin_satz_2 = -np.cos(phi_1_rad) * np.sin(a_12_rad)
    sin_kos_2 = np.sin(s) * np.sin(phi_1_rad) - np.cos(s) * np.cos(phi_1_rad) * np.cos(a_12_rad)
    a_21_rad = np.atan2(sin_satz_2, sin_kos_2)
    if a_21_rad < 0:
        a_21_rad += 2 * np.pi

    return np.rad2deg(phi_2_rad), np.rad2deg(lamb_2_rad), np.rad2deg(a_21_rad)


def kugel_zweite_gha(R, phi_1, lamb_1, phi_2, lamb_2):
    phi_1_rad = np.deg2rad(phi_1)
    lamb_1_rad = np.deg2rad(lamb_1)
    phi_2_rad = np.deg2rad(phi_2)
    lamb_2_rad = np.deg2rad(lamb_2)

    sin_satz_1 = np.cos(phi_2_rad) * np.sin(lamb_2_rad - lamb_1_rad)
    sin_kos_1 = np.cos(phi_1_rad) * np.sin(phi_2_rad) - np.sin(phi_1_rad) * np.cos(phi_2_rad) * np.cos(lamb_2_rad - lamb_1_rad)
    a_12_rad = np.atan2(sin_satz_1, sin_kos_1)

    kos = np.sin(phi_1_rad) * np.sin(phi_2_rad) + np.cos(phi_1_rad) * np.cos(phi_2_rad) * np.cos(lamb_2_rad - lamb_1_rad)
    s = np.acos(kos)
    s_d = R * s

    sin_satz_2 = -np.cos(phi_1_rad) * np.sin(lamb_2_rad - lamb_1_rad)
    sin_kos_2 = np.cos(phi_2_rad) * np.sin(phi_1_rad) - np.sin(phi_2_rad) * np.cos(phi_1_rad) * np.cos(lamb_2_rad - lamb_1_rad)
    a_21_rad = np.atan2(sin_satz_2, sin_kos_2)
    if a_21_rad < 0:
        a_21_rad += 2 * np.pi

    return s_d, np.rad2deg(a_12_rad), np.rad2deg(a_21_rad)


if __name__ == "__main__":
    R = 6378815.904                                             # Bern
    phi_1 = 10
    lamb_1 = 40
    a_12 = 100
    s = 10000
    phi_2, lamb_2, _ = kugel_erste_gha(R, phi_1, lamb_1, s, a_12)

    print(kugel_erste_gha(R, phi_1, lamb_1, s, a_12))
    print(kugel_zweite_gha(R, phi_1, lamb_1, phi_2, lamb_2))