from numpy import sin, cos, pi, sqrt, tan, arcsin, arccos, arctan
import ausgaben as aus
from ellipsoide import EllipsoidBiaxial
from typing import Tuple


def gha1(re: EllipsoidBiaxial, phi0: float, lamb0: float, alpha0: float, s: float, eps: float = 1e-12) -> Tuple[float, float, float]:
    """
    Berechnung der 1. Geodätische Hauptaufgabe nach Gauß´schen Mittelbreitenformeln
    :param re: Klasse Ellipsoid
    :param phi0: Breite Punkt 0
    :param lamb0: Länge Punkt 0
    :param alpha0: Azimut der geodätischen Linie in Punkt 1
    :param s: Strecke zu Punkt 1
    :param eps: Abbruchkriterium für Winkelgrößen
    :return: Breite, Länge, Azimut von Punkt 21
    """
    t = lambda phi: tan(phi)
    eta = lambda phi: sqrt(re.e_ ** 2 * cos(phi) ** 2)

    F1 = lambda alpha, phi, s: 1 + (2 + 3 * t(phi) ** 2 + 2 * eta(phi) ** 2) / (24 * re.N(phi) ** 2) * sin(alpha) ** 2 * s ** 2 \
                               + (t(phi)**2 - 1) * eta(phi) ** 2 / (8 * re.N(phi) ** 2) * cos(alpha) ** 2 * s ** 2

    F2 = lambda alpha, phi, s: 1 + t(phi) ** 2 / (24 * re.N(phi) ** 2) * sin(alpha) ** 2 * s ** 2 \
                               - (1 + eta(phi)**2 - 9 * eta(phi)**2 * t(phi)**2) / (24 * re.N(phi) ** 2) * cos(alpha) ** 2 * s ** 2

    F3 = lambda alpha, phi, s: 1 + (1 + eta(phi) ** 2) / (12 * re.N(phi) ** 2) * sin(alpha) ** 2 * s ** 2 \
                               + (3 + 8 * eta(phi)**2) / (24 * re.N(phi) ** 2) * cos(alpha) ** 2 * s ** 2

    phi1_i = lambda alpha, phi: phi0 + cos(alpha) / re.M(phi) * s * F1(alpha, phi, s)
    lamb1_i = lambda alpha, phi: lamb0 + sin(alpha) / (re.N(phi) * cos(phi)) * s * F2(alpha, phi, s)
    alpha1_i = lambda alpha, phi: alpha0 + sin(alpha) * tan(phi) / re.N(phi) * s * F3(alpha, phi, s)

    phi_m_i = lambda phi1: (phi0 + phi1) / 2
    alpha_m_i = lambda alpha1: (alpha0 + alpha1) / 2

    phi_m = []
    alpha_m = []
    phi1 = []
    lamb1 = []
    alpha1 = []

    # 1. Näherung für P1
    phi1.append(phi0 + cos(alpha0) / re.M(phi0) * s)
    lamb1.append(lamb0 + sin(alpha0) / (re.N(phi0) * cos(phi0)) * s)
    alpha1.append(alpha0 + sin(alpha0) * tan(phi0) / re.N(phi0) * s)

    while True:
        # Berechnug P_m durch Mittelbildung
        phi_m.append(phi_m_i(phi1[-1]))
        alpha_m.append(alpha_m_i(alpha1[-1]))
        # Berechnung P1
        phi1.append(phi1_i(alpha_m[-1], phi_m[-1]))
        lamb1.append(lamb1_i(alpha_m[-1], phi_m[-1]))
        alpha1.append(alpha1_i(alpha_m[-1], phi_m[-1]))

        # Abbruchkriterium
        if abs(phi1[-2] - phi1[-1]) < eps and \
           abs(lamb1[-2] - lamb1[-1]) < eps and \
           abs(alpha1[-2] - alpha1[-1]) < eps:
            break
    return phi1[-1], lamb1[-1], alpha1[-1]


def gha2(re: EllipsoidBiaxial, phi0: float, lamb0: float, phi1: float, lamb1: float):
    """
    Berechnung der 2. Geodätische Hauptaufgabe nach Gauß´schen Mittelbreitenformeln
    :param re: Klasse Ellipsoid
    :param phi0: Breite Punkt 1
    :param lamb0: Länge Punkt 1
    :param phi1: Breite Punkt 2
    :param lamb1: Länge Punkt 2
    :return: Länge der geodätischen Linie, Azimut von P1 nach P2, Azimut von P2 nach P1
    """

    t = lambda phi: tan(phi)
    eta = lambda phi: sqrt(re.e_ ** 2 * cos(phi) ** 2)

    phi_0 = (phi0 + phi1) / 2
    d_phi = phi1 - phi0
    d_lambda = lamb1 - lamb0

    f_A = lambda phi: (2 + 3*t(phi)**2 + 2*eta(phi)**2) / 24
    f_B = lambda phi: ((t(phi)**2 - 1) * eta(phi)**2) / 8
    # f_C = lambda phi: (t(phi)**2) / 24
    f_D = lambda phi: (1 + eta(phi)**2 - 9 * eta(phi)**2 * t(phi)**2) / 24

    F1 = lambda phi: d_phi * re.M(phi) * (1 - f_A(phi) * d_lambda ** 2 * cos(phi) ** 2 -
                                          f_B(phi) * d_phi ** 2 / re.V(phi) ** 4)
    F2 = lambda phi: d_lambda * re.N(phi) * cos(phi) * (1 - 1 / 24 * d_lambda ** 2 * sin(phi) ** 2 +
                                                           f_D(phi) * d_phi ** 2 / re.V(phi) ** 4)

    s = sqrt(F1(phi_0) ** 2 + F2(phi_0) ** 2)
    A_0 = arctan(F2(phi_0) / F1(phi_0))
    d_A = d_lambda * sin(phi_0) * (1 + (1 + eta(phi_0) ** 2) / 12 * s ** 2 * sin(A_0) ** 2 / re.N(phi_0) ** 2 +
                                      (3 + 8 * eta(phi_0) ** 2) / 24 * s ** 2 * cos(A_0) ** 2 / re.N(phi_0) ** 2)

    alpha0 = A_0 - d_A / 2
    alpha1 = A_0 + d_A / 2

    return alpha0, alpha1, s