from numpy import sin, cos, pi, sqrt, tan, arcsin, arccos, arctan
import ausgaben as aus


def gha1(re, phi_p1, lambda_p1, A_p1, s, eps):
    """
    Berechnung der 1. Geodätische Hauptaufgabe nach Gauß´schen Mittelbreitenformeln
    :param re: Klasse Ellipsoid
    :param phi_p1: Breite Punkt 1
    :param lambda_p1: Länge Punkt 1
    :param A_p1: Azimut der geodätischen Linie in Punkt 1
    :param s: Strecke zu Punkt 2
    :param eps: Abbruchkriterium für Winkelgrößen
    :return: Breite, Länge, Azimut von Punkt 2
    """

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

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

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

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

    phi_p2_i = lambda A, phi: phi_p1 + cos(A) / re.M(phi) * s * F1(A, phi, s)
    lambda_p2_i = lambda A, phi: lambda_p1 + sin(A) / (re.N(phi) * cos(phi)) * s * F2(A, phi, s)
    A_p2_i = lambda A, phi: A_p1 + sin(A) * tan(phi) / re.N(phi) * s * F3(A, phi, s)

    phi_p0_i = lambda phi2: (phi_p1 + phi2) / 2
    A_p1_i = lambda A2: (A_p1 + A2) / 2

    phi_p0 = []
    A_p0 = []
    phi_p2 = []
    lambda_p2 = []
    A_p2 = []

    # 1. Näherung für P2
    phi_p2.append(phi_p1 + cos(A_p1) / re.M(phi_p1) * s)
    lambda_p2.append(lambda_p1 + sin(A_p1) / (re.N(phi_p1) * cos(phi_p1)) * s)
    A_p2.append(A_p1 + sin(A_p1) * tan(phi_p1) / re.N(phi_p1) * s)

    while True:
        # Berechnug P0 durch Mittelbildung
        phi_p0.append(phi_p0_i(phi_p2[-1]))
        A_p0.append(A_p1_i(A_p2[-1]))
        # Berechnung P2
        phi_p2.append(phi_p2_i(A_p0[-1], phi_p0[-1]))
        lambda_p2.append(lambda_p2_i(A_p0[-1], phi_p0[-1]))
        A_p2.append(A_p2_i(A_p0[-1], phi_p0[-1]))

        # Abbruchkriterium
        if abs(phi_p2[-2] - phi_p2[-1]) < eps and \
           abs(lambda_p2[-2] - lambda_p2[-1]) < eps and \
           abs(A_p2[-2] - A_p2[-1]) < eps:
            break

    nks = 5
    for i in range(len(phi_p2)):
        print(f"P2[{i}]: {aus.gms('phi', phi_p2[i], nks)}\t{aus.gms('lambda', lambda_p2[i], nks)}\t{aus.gms('A', A_p2[i], nks)}")
        if i != len(phi_p2)-1:
            print(f"P0[{i}]: {aus.gms('phi', phi_p0[i], nks)}\t\t\t\t\t\t\t\t{aus.gms('A', A_p0[i], nks)}")
    return phi_p2, lambda_p2, A_p2


def gha2(re, phi_p1, lambda_p1, phi_p2, lambda_p2):
    """
    Berechnung der 2. Geodätische Hauptaufgabe nach Gauß´schen Mittelbreitenformeln
    :param re: Klasse Ellipsoid
    :param phi_p1: Breite Punkt 1
    :param lambda_p1: Länge Punkt 1
    :param phi_p2: Breite Punkt 2
    :param lambda_p2: 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 = (phi_p1 + phi_p2) / 2
    d_phi = phi_p2 - phi_p1
    d_lambda = lambda_p2 - lambda_p1

    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)

    A_p1 = A_0 - d_A / 2
    A_p2 = A_0 + d_A / 2
    A_p2_p1 = A_p2 + pi

    return s, A_p1, A_p2_p1