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

# Panou, Korakitits 2019

def p_q(ell: ellipsoide.EllipsoidTriaxial, x, y, z):
    H = x ** 2 + y ** 2 / (1 - ell.ee ** 2) ** 2 + z ** 2 / (1 - ell.ex ** 2) ** 2

    n = np.array([x / np.sqrt(H), y / ((1 - ell.ee ** 2) * np.sqrt(H)), z / ((1 - ell.ex ** 2) * np.sqrt(H))])

    beta, lamb, u = ell.cart2ell(x, y, z)
    carts = ell.ell2cart(beta, lamb, u)
    B = ell.Ex ** 2 * np.cos(beta) ** 2 + ell.Ee ** 2 * np.sin(beta) ** 2
    L = ell.Ex ** 2 - ell.Ee ** 2 * np.cos(lamb) ** 2

    c1 = x ** 2 + y ** 2 + z ** 2 - (ell.ax ** 2 + ell.ay ** 2 + ell.b ** 2)
    c0 = (ell.ax ** 2 * ell.ay ** 2 + ell.ax ** 2 * ell.b ** 2 + ell.ay ** 2 * ell.b ** 2 -
          (ell.ay ** 2 + ell.b ** 2) * x ** 2 - (ell.ax ** 2 + ell.b ** 2) * y ** 2 - (
                      ell.ax ** 2 + ell.ay ** 2) * z ** 2)
    t2 = (-c1 + np.sqrt(c1**2 - 4*c0)) / 2
    t1 = c0 / t2
    t2e = ell.ax ** 2 * np.sin(lamb) ** 2 + ell.ay ** 2 * np.cos(lamb) ** 2
    t1e = ell.ay ** 2 * np.sin(beta) ** 2 + ell.b ** 2 * np.cos(beta) ** 2

    F = ell.Ey ** 2 * np.cos(beta) ** 2 + ell.Ee ** 2 * np.sin(lamb) ** 2
    p1 = -np.sqrt(L / (F * t2)) * ell.ax / ell.Ex * np.sqrt(B) * np.sin(lamb)
    p2 = np.sqrt(L / (F * t2)) * ell.ay * np.cos(beta) * np.cos(lamb)
    p3 = 1 / np.sqrt(F * t2) * (ell.b * ell.Ee ** 2) / (2 * ell.Ex) * np.sin(beta) * np.sin(2 * lamb)
    # p1 = -np.sign(y) * np.sqrt(L / (F * t2)) * ell.ax / (ell.Ex * ell.Ee) * np.sqrt(B) * np.sqrt(t2 - ell.ay ** 2)
    # p2 = np.sign(x) * np.sqrt(L / (F * t2)) * ell.ay / (ell.Ey * ell.Ee) * np.sqrt((ell.ay ** 2 - t1) * (ell.ax ** 2 - t2))
    # p3 = np.sign(x) * np.sign(y) * np.sign(z) * 1 / np.sqrt(F * t2) * ell.b / (ell.Ex * ell.Ey) * np.sqrt(
    #     (t1 - ell.b ** 2) * (t2 - ell.ay ** 2) * (ell.ax ** 2 - t2))
    p = np.array([p1, p2, p3])
    q = np.cross(n, p)

    return {"H": H, "n": n, "beta": beta, "lamb": lamb, "u": u, "B": B, "L": L, "c1": c1, "c0": c0, "t1": t1, "t2": t2,
            "F": F, "p": p, "q": q}
def gha1_num(ell: ellipsoide.EllipsoidTriaxial, x, y, z, alpha0, s, num):
    values = p_q(ell, 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 = p_q(ell, 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, x, y, z, alpha0, s, maxM):
    """
    Panou, Korakitits 2020, 5ff.
    :param ell:
    :param x:
    :param y:
    :param z:
    :param alpha0:
    :param s:
    :param maxM:
    :return:
    """
    Hsp = []
    # H Ableitungen (7)
    hsq = []
    # h Ableitungen (7)
    hHst = []
    # h/H Ableitungen (6)
    xsm = [x]
    ysm = [y]
    zsm = [z]
    # erste Ableitungen (7-8)
    # xm, ym, zm Ableitungen (6)
    am = []
    bm = []
    cm = []
    # am, bm, cm (6)
    x_s = 0
    for a in am:
        x_s = x_s * s + a
    y_s = 0
    for b in bm:
        y_s = y_s * s + b
    z_s = 0
    for c in cm:
        z_s = z_s * s + c
    pass


if __name__ == "__main__":
    ell = ellipsoide.EllipsoidTriaxial.init_name("Eitschberger1978")
    ellbi = ellipsoide.EllipsoidTriaxial.init_name("Bessel-biaxial")
    re = ellipsoide.EllipsoidBiaxial.init_name("Bessel")
    x0 = 5672455.1954766
    y0 = 2698193.7242382686
    z0 = 1103177.6450055107
    alpha0 = wu.gms2rad([20, 0, 0])
    s = 10000
    num = 10000
    # werteTri = gha1_num(ellbi, x0, y0, z0, alpha0, s, num)
    # print(aus.xyz(werteTri[-1][1], werteTri[-1][3], werteTri[-1][5], 8))
    # checkLiouville(ell, werteTri)
    # werteBi = ghark.gha1(re, x0, y0, z0, alpha0, s, num)
    # print(aus.xyz(werteBi[0], werteBi[1], werteBi[2], 8))

    gha1_ana(ell, x0, y0, z0, alpha0, s, 7)