import runge_kutta as rk
from numpy import sin, cos, tan
import winkelumrechnungen as wu
from ellipsoide import EllipsoidBiaxial
import numpy as np

def gha1(re: EllipsoidBiaxial, x0, y0, z0, A0, s, num):
    phi0, lamb0, h0 = re.cart2ell(0.001, wu.gms2rad([0, 0, 0.001]), x0, y0, z0)

    def buildODE():
        def ODE(s, v):
            phi, lam, A = v
            dphi = cos(A) * re.V(phi) ** 3 / re.c
            dlam = sin(A) * re.V(phi) / (cos(phi) * re.c)
            dA = tan(phi) * sin(A) * re.V(phi) / re.c
            return np.array([dphi, dlam, dA])
        return ODE

    _, funktionswerte = rk.rk4(buildODE(), 0, np.array([phi0, lamb0, A0]), s, num)
    coords = re.ell2cart(funktionswerte[-1][0], funktionswerte[-1][1], h0)
    return coords