import math
from math import comb
from typing import Tuple

import numpy as np
from numpy import sin, cos, arctan2
from numpy._typing import NDArray
import winkelumrechnungen as wu

from ellipsoide import EllipsoidTriaxial
from GHA_triaxial.utils import pq_para


def gha1_ana_step(ell: EllipsoidTriaxial, point: NDArray, alpha0: float, s: float, maxM: int) -> Tuple[NDArray, float]:
    """
    Panou, Korakitits 2020, 5ff.
    :param ell: Ellipsoid
    :param point: Punkt in kartesischen Koordinaten
    :param alpha0: Azimut im Startpunkt
    :param s: Strecke
    :param maxM: maximale Ordnung
    :return: Zwischenpunkt, Azimut im Zwischenpunkt
    """
    x, y, z = point

    # S. 6
    x_m = [x]
    y_m = [y]
    z_m = [z]

    p, q = pq_para(ell, point)

    # 48-50
    x_m.append(p[0] * sin(alpha0) + q[0] * cos(alpha0))
    y_m.append(p[1] * sin(alpha0) + q[1] * cos(alpha0))
    z_m.append(p[2] * sin(alpha0) + q[2] * cos(alpha0))

    # 34
    H_ = lambda p: np.sum([comb(p, p - i) * (x_m[p - i] * x_m[i] +
                                             1 / (1 - ell.ee ** 2) ** 2 * y_m[p - i] * y_m[i] +
                                             1 / (1 - ell.ex ** 2) ** 2 * z_m[p - i] * z_m[i])
                           for i in range(0, p + 1)])

    # 35
    h_ = lambda q: np.sum([comb(q, q-j) * (x_m[q-j+1] * x_m[j+1] +
                                           1 / (1 - ell.ee ** 2) * y_m[q-j+1] * y_m[j+1] +
                                           1 / (1 - ell.ex ** 2) * z_m[q-j+1] * z_m[j+1])
                           for j in range(0, q+1)])

    # 31
    hH_ = lambda t: 1/H_(0) * (h_(t) - np.sum([comb(t, l-1) * H_(t+1-l) * hH_t[l-1] for l in range(1, t+1)]))

    # 28-30
    x_ = lambda m: - np.sum([comb(m-2, k) * hH_t[m-2-k] * x_m[k] for k in range(0, m-2+1)])
    y_ = lambda m: -1 / (1-ell.ee**2) * np.sum([comb(m-2, k) * hH_t[m-2-k] * y_m[k] for k in range(0, m-2+1)])
    z_ = lambda m: -1 / (1-ell.ex**2) * np.sum([comb(m-2, k) * hH_t[m-2-k] * z_m[k] for k in range(0, m-2+1)])

    hH_t = []
    a_m = []
    b_m = []
    c_m = []
    for m in range(0, maxM+1):
        if m >= 2:
            hH_t.append(hH_(m-2))
            x_m.append(x_(m))
            y_m.append(y_(m))
            z_m.append(z_(m))
        fact_m = math.factorial(m)

        # 22-24
        a_m.append(x_m[m] / fact_m)
        b_m.append(y_m[m] / fact_m)
        c_m.append(z_m[m] / fact_m)

    # 19-21
    x_s = 0
    for a in reversed(a_m):
        x_s = x_s * s + a
    y_s = 0
    for b in reversed(b_m):
        y_s = y_s * s + b
    z_s = 0
    for c in reversed(c_m):
        z_s = z_s * s + c

    p1 = np.array([x_s, y_s, z_s])
    p_s, q_s = pq_para(ell, p1)

    # 57-59
    dx_s = 0
    for i, a in reversed(list(enumerate(a_m[1:], start=1))):
        dx_s = dx_s * s + i * a

    dy_s = 0
    for i, b in reversed(list(enumerate(b_m[1:], start=1))):
        dy_s = dy_s * s + i * b

    dz_s = 0
    for i, c in reversed(list(enumerate(c_m[1:], start=1))):
        dz_s = dz_s * s + i * c

    # 52-53
    sigma = np.array([dx_s, dy_s, dz_s])
    P = float(p_s @ sigma)
    Q = float(q_s @ sigma)

    # 51
    alpha1 = arctan2(P, Q)

    if alpha1 < 0:
        alpha1 += 2 * np.pi

    return p1, alpha1


def gha1_ana(ell: EllipsoidTriaxial, point: NDArray, alpha0: float, s: float, maxM: int, maxPartCircum: int = 4) -> Tuple[NDArray, float]:
    """
    :param ell: Ellipsoid
    :param point: Punkt in kartesischen Koordinaten
    :param alpha0: Azimut im Startpunkt
    :param s: Strecke
    :param maxM: maximale Ordnung
    :param maxPartCircum: maximale Aufteilung (1/x halber Ellipsoidumfang)
    :return: Zielpunkt, Azimut im Zielpunkt
    """
    if s > np.pi / maxPartCircum * ell.ax:
        s /= 2
        point_step, alpha_step = gha1_ana(ell, point, alpha0, s, maxM, maxPartCircum)
        point_end, alpha_end = gha1_ana(ell, point_step, alpha_step, s, maxM, maxPartCircum)
    else:
        point_end, alpha_end = gha1_ana_step(ell, point, alpha0, s, maxM)

    _, _, h = ell.cart2geod(point_end, "ligas3")
    if h > 1e-5:
        raise Exception("Analytische Methode ist explodiert, Punkt liegt nicht mehr auf dem Ellipsoid")

    return point_end, alpha_end


if __name__ == "__main__":
    ell = EllipsoidTriaxial.init_name("BursaSima1980round")
    p0 = ell.ell2cart(wu.deg2rad(10), wu.deg2rad(20))
    p1, alpha1 = gha1_ana(ell, p0, wu.deg2rad(36), 200000, 70)
    print(p1, wu.rad2gms(alpha1))