import numpy as np
import ellipsoide
import Numerische_Integration.num_int_runge_kutta as rk
import winkelumrechnungen as wu
import ausgaben as aus
import GHA.rk as ghark
from scipy.special import factorial as fact
from math import comb

# Panou, Korakitits 2019


def gha1_num(ell: ellipsoide.EllipsoidTriaxial, point, alpha0, s, num):
    phi, lamb, h = ell.cart2geod("ligas3", point)
    x, y, z = ell.geod2cart(phi, lamb, 0)
    values = ell.p_q(x, y, z)
    H = values["H"]
    p = values["p"]
    q = values["q"]

    dxds0 = p[0] * np.sin(alpha0) + q[0] * np.cos(alpha0)
    dyds0 = p[1] * np.sin(alpha0) + q[1] * np.cos(alpha0)
    dzds0 = p[2] * np.sin(alpha0) + q[2] * np.cos(alpha0)

    h = lambda dxds, dyds, dzds: dxds**2 + 1/(1-ell.ee**2)*dyds**2 + 1/(1-ell.ex**2)*dzds**2

    f1 = lambda s, x, dxds, y, dyds, z, dzds: dxds
    f2 = lambda s, x, dxds, y, dyds, z, dzds: -h(dxds, dyds, dzds) / H * x
    f3 = lambda s, x, dxds, y, dyds, z, dzds: dyds
    f4 = lambda s, x, dxds, y, dyds, z, dzds: -h(dxds, dyds, dzds) / H * y/(1-ell.ee**2)
    f5 = lambda s, x, dxds, y, dyds, z, dzds: dzds
    f6 = lambda s, x, dxds, y, dyds, z, dzds: -h(dxds, dyds, dzds) / H * z/(1-ell.ex**2)

    funktionswerte = rk.verfahren([f1, f2, f3, f4, f5, f6], [0, x, dxds0, y, dyds0, z, dzds0], s, num)
    return funktionswerte

def checkLiouville(ell: ellipsoide.EllipsoidTriaxial, points):
    constantValues = []
    for point in points:
        x = point[1]
        dxds = point[2]
        y = point[3]
        dyds = point[4]
        z = point[5]
        dzds = point[6]

        values = ell.p_q(x, y, z)
        p = values["p"]
        q = values["q"]
        t1 = values["t1"]
        t2 = values["t2"]

        P = p[0]*dxds + p[1]*dyds + p[2]*dzds
        Q = q[0]*dxds + q[1]*dyds + q[2]*dzds
        alpha = np.arctan(P/Q)

        c = ell.ay**2 - (t1 * np.sin(alpha)**2 + t2 * np.cos(alpha)**2)
        constantValues.append(c)
    pass


def gha1_ana(ell: ellipsoide.EllipsoidTriaxial, point, alpha0, s, maxM):
    """
    Panou, Korakitits 2020, 5ff.
    :param ell:
    :param x:
    :param y:
    :param z:
    :param alpha0:
    :param s:
    :param maxM:
    :return:
    """
    x, y, z = point
    x_m = [x]
    y_m = [y]
    z_m = [z]

    # erste Ableitungen (7-8)
    sqrtH = np.sqrt(ell.p_q(x, y, z)["H"])
    n = np.array([x / sqrtH,
                  y / ((1-ell.ee**2) * sqrtH),
                  z / ((1-ell.ex**2) * sqrtH)])
    u, v = ell.cart2para(np.array([x, y, z]))
    G = np.sqrt(1 - ell.ex**2 * np.cos(u)**2 - ell.ee**2 * np.sin(u)**2 * np.sin(v)**2)
    q = np.array([-1/G * np.sin(u) * np.cos(v),
                  -1/G * np.sqrt(1-ell.ee**2) * np.sin(u) * np.sin(v),
                  1/G * np.sqrt(1-ell.ex**2) * np.cos(u)])
    p = np.array([q[1]*n[2] - q[2]*n[1],
                  q[2]*n[0] - q[0]*n[2],
                  q[0]*n[1] - q[1]*n[0]])
    x_m.append(p[0] * np.sin(alpha0) + q[0] * np.cos(alpha0))
    y_m.append(p[1] * np.sin(alpha0) + q[1] * np.cos(alpha0))
    z_m.append(p[2] * np.sin(alpha0) + q[2] * np.cos(alpha0))

    # H Ableitungen (7)
    H_ = lambda p: np.sum([comb(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)])

    # h Ableitungen (7)
    h_ = lambda q: np.sum([comb(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)])

    # h/H Ableitungen (6)
    hH_ = lambda t: 1/H_(0) * (h_(t) - fact(t) *
                               np.sum([H_(t+1-l) / (fact(t+1-l) * fact(l-1)) * hH_t[l-1] for l in range(1, t+1)]))

    # xm, ym, zm Ableitungen (6)
    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))
        a_m.append(x_m[m] / fact(m))
        b_m.append(y_m[m] / fact(m))
        c_m.append(z_m[m] / fact(m))

    # am, bm, cm (6)
    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

    return x_s, y_s, z_s
    pass


if __name__ == "__main__":
    # ell = ellipsoide.EllipsoidTriaxial.init_name("Eitschberger1978")
    ell = ellipsoide.EllipsoidTriaxial.init_name("BursaSima1980")
    ellbi = ellipsoide.EllipsoidTriaxial.init_name("Bessel-biaxial")
    re = ellipsoide.EllipsoidBiaxial.init_name("Bessel")

    # Panou 2013, 7, Table 1, beta0=60°
    beta1 = wu.deg2rad(60)
    lamb1 = wu.deg2rad(0)
    beta2 = wu.deg2rad(60)
    lamb2 = wu.deg2rad(175)
    P1 = ell.ell2cart2(wu.deg2rad(60), wu.deg2rad(0))
    P2 = ell.ell2cart2(wu.deg2rad(60), wu.deg2rad(175))
    para1 = ell.cart2para(P1)
    para2 = ell.cart2para(P2)
    cart1 = ell.para2cart(para1[0], para1[1])
    cart2 = ell.para2cart(para2[0], para2[1])
    ell11 = ell.cart2ell2(P1)
    ell21 = ell.cart2ell2(P2)
    ell1 = ell.cart2ell2(cart1)
    ell2 = ell.cart2ell2(cart2)

    c = 0.06207487624
    alpha0 = wu.gms2rad([2, 52, 26.2393])
    alpha1 = wu.gms2rad([177, 4, 13.6373])
    s = 6705715.1610
    pass

    P2_num = gha1_num(ell, P1, alpha0, s, 1000)
    P2_ana = gha1_ana(ell, P1, alpha0, s, 70)
    pass