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 gha1(ell: ellipsoide.EllipsoidTriaxial, x, y, z, alpha0, s, num):
    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)
    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

    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)
    p = np.array([p1, p2, p3])
    q = np.cross(n, p)

    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


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(ellbi, x0, y0, z0, alpha0, s, num)
    print(aus.xyz(werteTri[-1][1], werteTri[-1][3], werteTri[-1][5], 8))
    werteBi = ghark.gha1(re, x0, y0, z0, alpha0, s, num)
    print(aus.xyz(werteBi[0], werteBi[1], werteBi[2], 8))