import numpy as np
from numpy import sin, cos, arctan2
import ellipsoide
import runge_kutta as rk
import winkelumrechnungen as wu
import GHA_triaxial.numeric_examples_karney as ne_karney
from GHA_triaxial.gha1_ana import gha1_ana
from ellipsoide import EllipsoidTriaxial
from typing import Callable, Tuple, List
from numpy.typing import NDArray

from GHA_triaxial.utils import alpha_ell2para, pq_ell


def gha1_num(ell: EllipsoidTriaxial, point: NDArray, alpha0: float, s: float, num: int, all_points: bool = False) -> Tuple[NDArray, float] | Tuple[NDArray, float, List]:
    """
    Panou, Korakitits 2019
    :param ell: Ellipsoid
    :param point: Punkt in kartesischen Koordinaten
    :param alpha0: Azimut im Startpunkt
    :param s: Strecke
    :param num: Anzahl Zwischenpunkte
    :param all_points: Ausgabe aller Punkte?
    :return: Zielpunkt, Azimut im Zielpunkt (, alle Punkte)
    """
    phi, lam, _ = ell.cart2geod(point, "ligas3")
    p0 = ell.geod2cart(phi, lam, 0)
    x0, y0, z0 = p0

    p, q = pq_ell(ell, p0)
    dxds0 = p[0] * sin(alpha0) + q[0] * cos(alpha0)
    dyds0 = p[1] * sin(alpha0) + q[1] * cos(alpha0)
    dzds0 = p[2] * sin(alpha0) + q[2] * cos(alpha0)

    v_init = np.array([x0, dxds0, y0, dyds0, z0, dzds0])

    def buildODE(ell: EllipsoidTriaxial) -> Callable:
        """
        Aufbau des DGL-Systems
        :param ell: Ellipsoid
        :return: DGL-System
        """

        def ODE(s: float, v: NDArray) -> NDArray:
            """
            DGL-System
            :param s: unabhängige Variable
            :param v: abhängige Variablen
            :return: Ableitungen der abhängigen Variablen
            """
            x, dxds, y, dyds, z, dzds = v

            H = ell.func_H(np.array([x, y, z]))
            h = dxds ** 2 + 1 / (1 - ell.ee ** 2) * dyds ** 2 + 1 / (1 - ell.ex ** 2) * dzds ** 2

            ddx = -(h / H) * x
            ddy = -(h / H) * y / (1 - ell.ee ** 2)
            ddz = -(h / H) * z / (1 - ell.ex ** 2)

            return np.array([dxds, ddx, dyds, ddy, dzds, ddz])

        return ODE

    ode = buildODE(ell)

    _, werte = rk.rk4(ode, 0, v_init, s, num)
    x1, dx1ds, y1, dy1ds, z1, dz1ds = werte[-1]

    point1 = np.array([x1, y1, z1])

    p1, q1 = pq_ell(ell, point1)
    sigma = np.array([dx1ds, dy1ds, dz1ds])
    P = float(p1 @ sigma)
    Q = float(q1 @ sigma)

    alpha1 = arctan2(P, Q)

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

    if all_points:
        return point1, alpha1, werte
    else:
        return point1, alpha1

if __name__ == "__main__":

    # ell = ellipsoide.EllipsoidTriaxial.init_name("BursaSima1980round")
    # diffs_panou = []
    # examples_panou = ne_panou.get_random_examples(5)
    # for example in examples_panou:
    #     beta0, lamb0, beta1, lamb1, _, alpha0_ell, alpha1_ell, s = example
    #     P0 = ell.ell2cart(beta0, lamb0)
    #
    #     P1_num, alpha1_num = gha1_num(ell, P0, alpha0_ell, s, 100)
    #     beta1_num, lamb1_num = ell.cart2ell(P1_num)
    #
    #     _, _, alpha0_para = alpha_ell2para(ell, beta0, lamb0, alpha0_ell)
    #     P1_ana, alpha1_ana = gha1_ana(ell, P0, alpha0_para, s, 60)
    #     beta1_ana, lamb1_ana = ell.cart2ell(P1_ana)
    #     diffs_panou.append((abs(beta1-beta1_num), abs(lamb1-lamb1_num), abs(beta1-beta1_ana), abs(lamb1-lamb1_ana)))
    # diffs_panou = np.array(diffs_panou)
    # mask_360 = (diffs_panou > 359) & (diffs_panou < 361)
    # diffs_panou[mask_360] = np.abs(diffs_panou[mask_360] - 360)
    # print(diffs_panou)

    ell: EllipsoidTriaxial = ellipsoide.EllipsoidTriaxial.init_name("KarneyTest2024")
    diffs_karney = []
    # examples_karney = ne_karney.get_examples((30499, 30500, 40500))
    examples_karney = ne_karney.get_random_examples(20)
    for example in examples_karney:
        beta0, lamb0, alpha0_ell, beta1, lamb1, alpha1_ell, s = example
        P0 = ell.ell2cart(beta0, lamb0)

        P1_num, alpha1_num = gha1_num(ell, P0, alpha0_ell, s, 10000)
        beta1_num, lamb1_num = ell.cart2ell(P1_num)

        try:
            _, _, alpha0_para = alpha_ell2para(ell, beta0, lamb0, alpha0_ell)
            P1_ana, alpha1_ana = gha1_ana(ell, P0, alpha0_para, s, 45, maxPartCircum=32)
            beta1_ana, lamb1_ana = ell.cart2ell(P1_ana)
        except:
            beta1_ana, lamb1_ana = np.inf, np.inf

        diffs_karney.append((wu.rad2deg(abs(beta1-beta1_num)), wu.rad2deg(abs(lamb1-lamb1_num)), wu.rad2deg(abs(beta1-beta1_ana)), wu.rad2deg(abs(lamb1-lamb1_ana))))
    diffs_karney = np.array(diffs_karney)
    mask_360 = (diffs_karney > 359) & (diffs_karney < 361)
    diffs_karney[mask_360] = np.abs(diffs_karney[mask_360] - 360)
    print(diffs_karney)