import numpy as np
import winkelumrechnungen as wu
import ausgaben as aus
import jacobian_Ligas
from GHA_triaxial.panou import p_q


class EllipsoidBiaxial:
    def __init__(self, a: float, b: float):
        self.a = a
        self.b = b
        self.c = a ** 2 / b
        self.e = np.sqrt(a ** 2 - b ** 2) / a
        self.e_ = np.sqrt(a ** 2 - b ** 2) / b

    @classmethod
    def init_name(cls, name: str):
        if name == "Bessel":
            a = 6377397.15508
            b = 6356078.96290
            return cls(a, b)
        elif name == "Hayford":
            a = 6378388
            f = 1/297
            b = a - a * f
            return cls(a, b)
        elif name == "Krassowski":
            a = 6378245
            f = 298.3
            b = a - a * f
            return cls(a, b)
        elif name == "WGS84":
            a = 6378137
            f = 298.257223563
            b = a - a * f
            return cls(a, b)

    @classmethod
    def init_af(cls, a: float, f: float):
        b = a - a * f
        return cls(a, b)

    V = lambda self, phi: np.sqrt(1 + self.e_ ** 2 * np.cos(phi) ** 2)
    M = lambda self, phi: self.c / self.V(phi) ** 3
    N = lambda self, phi: self.c / self.V(phi)

    beta2psi = lambda self, beta: np.arctan(self.a / self.b * np.tan(beta))
    beta2phi = lambda self, beta: np.arctan(self.a ** 2 / self.b ** 2 * np.tan(beta))

    psi2beta = lambda self, psi: np.arctan(self.b / self.a * np.tan(psi))
    psi2phi = lambda self, psi: np.arctan(self.a / self.b * np.tan(psi))

    phi2beta = lambda self, phi: np.arctan(self.b ** 2 / self.a ** 2 * np.tan(phi))
    phi2psi = lambda self, phi: np.arctan(self.b / self.a * np.tan(phi))

    phi2p = lambda self, phi: self.N(phi) * np.cos(phi)

    def cart2ell(self, Eh, Ephi, x, y, z):
        p = np.sqrt(x**2+y**2)
        # print(f"p = {round(p, 5)} m")

        lamb = np.arctan(y/x)

        phi_null = np.arctan(z/p*(1-self.e**2)**-1)

        hi = [0]
        phii = [phi_null]

        i = 0

        while True:
            N = self.a*(1-self.e**2*np.sin(phii[i])**2)**(-1/2)
            h = p/np.cos(phii[i])-N
            phi = np.arctan(z/p*(1-(self.e**2*N)/(N+h))**(-1))
            hi.append(h)
            phii.append(phi)
            dh = abs(hi[i]-h)
            dphi = abs(phii[i]-phi)
            i = i+1
            if dh < Eh:
                if dphi < Ephi:
                    break
        for i in range(len(phii)):
            # print(f"P3[{i}]: {aus.gms('phi', phii[i], 5)}\th = {round(hi[i], 5)} m")
            pass
        return phi, lamb, h

    def ell2cart(self, phi, lamb, h):
        W = np.sqrt(1 - self.e**2 * np.sin(phi)**2)
        N = self.a / W
        x = (N+h) * np.cos(phi) * np.cos(lamb)
        y = (N+h) * np.cos(phi) * np.sin(lamb)
        z = (N * (1-self.e**2) + h) * np.sin(lamb)
        return x, y, z

class EllipsoidTriaxial:
    def __init__(self, ax: float, ay: float, b: float):
        self.ax = ax
        self.ay = ay
        self.b = b
        self.ex = np.sqrt((self.ax**2 - self.b**2) / self.ax**2)
        self.ey = np.sqrt((self.ay**2 - self.b**2) / self.ay**2)
        self.ee = np.sqrt((self.ax**2 - self.ay**2) / self.ax**2)
        self.ex_ = np.sqrt((self.ax**2 - self.b**2) / self.b**2)
        self.ey_ = np.sqrt((self.ay**2 - self.b**2) / self.b**2)
        self.ee_ = np.sqrt((self.ax**2 - self.ay**2) / self.ay**2)
        self.Ex = np.sqrt(self.ax**2 - self.b**2)
        self.Ey = np.sqrt(self.ay**2 - self.b**2)
        self.Ee = np.sqrt(self.ax**2 - self.ay**2)

    @classmethod
    def init_name(cls, name: str):
        if name == "BursaFialova1993":
            ax = 6378171.36
            ay = 6378101.61
            b = 6356751.84
            return cls(ax, ay, b)
        elif name == "BursaSima1980":
            ax = 6378172
            ay = 6378102.7
            b = 6356752.6
            return cls(ax, ay, b)
        elif name == "Eitschberger1978":
            ax = 6378173.435
            ay = 6378103.9
            b = 6356754.4
            return cls(ax, ay, b)
        elif name == "Bursa1972":
            ax = 6378173
            ay = 6378104
            b = 6356754
            return cls(ax, ay, b)
        elif name == "Bursa1970":
            ax = 6378173
            ay = 6378105
            b = 6356754
            return cls(ax, ay, b)
        elif name == "Bessel-biaxial":
            ax = 6377397.15509
            ay = 6377397.15508
            b = 6356078.96290
            return cls(ax, ay, b)

    def ell2cart(self, beta, lamb, u):
        """
        Panou 2014 12ff.
        :param beta: ellipsoidische Breite
        :param lamb: ellipsoidische Länge
        :param u: Höhe
        :return: kartesische Koordinaten
        """
        s1 = u**2 - self.b**2
        s2 = -self.ay**2 * np.sin(beta)**2 - self.b**2 * np.cos(beta)**2
        s3 = -self.ax**2 * np.sin(lamb)**2 - self.ay**2 * np.cos(lamb)**2
        # print(s1, s2, s3)
        xe = np.sqrt(((self.ax**2+s1) * (self.ax**2+s2) * (self.ax**2+s3)) /
                     ((self.ax**2-self.ay**2) * (self.ax**2-self.b**2)))
        ye = np.sqrt(((self.ay**2+s1) * (self.ay**2+s2) * (self.ay**2+s3)) /
                     ((self.ay**2-self.ax**2) * (self.ay**2-self.b**2)))
        ze = np.sqrt(((self.b**2+s1) * (self.b**2+s2) * (self.b**2+s3)) /
                     ((self.b**2-self.ax**2) * (self.b**2-self.ax**2)))

        x = np.sqrt(u**2 + self.Ex**2) * np.sqrt(np.cos(beta)**2 + self.Ee**2/self.Ex**2 * np.sin(beta)**2) * np.cos(lamb)
        y = np.sqrt(u**2 + self.Ey**2) * np.cos(beta) * np.sin(lamb)
        z = u * np.sin(beta) * np.sqrt(1 - self.Ee**2/self.Ex**2 * np.cos(lamb)**2)

        return x, y, z

    def ell2cart_P(self, lamb, beta):

        """
        Panou 2013, 241

        :param lamb: ellipsoidische Länge
        :param beta: reduzierte ellipsoidische Breite
        :return: kartesische Koordinaten x y z
        """

        x = self.ax * np.sqrt(np.cos(beta) ** 2 + self.Ee ** 2 / self.Ex ** 2 * np.sin(beta) ** 2) * np.cos(lamb)
        y = self.ay * np.cos(beta) * np.sin(lamb)
        z = self.b * np.sin(beta) * np.sqrt(1 - self.Ee ** 2 / self.Ex ** 2 * np.cos(lamb) ** 2)

        return x, y, z

    def cart2ell(self, x, y, z):
        """
        Panou 2014 15ff.
        :param x:
        :param y:
        :param z:
        :return:
        """
        c2 = self.ax**2 + self.ay**2 + self.b**2 - x**2 - y**2 - z**2
        c1 = (self.ax**2 * self.ay**2 + self.ax**2 * self.b**2 + self.ay**2 * self.b**2 -
              (self.ay**2+self.b**2) * x**2 - (self.ax**2 + self.b**2) * y**2 - (self.ax**2 + self.ay**2) * z**2)
        c0 = (self.ax**2 * self.ay**2 * self.b**2 - self.ay**2 * self.b**2 * x**2 -
              self.ax**2 * self.b**2 * y**2 - self.ax**2 * self.ay**2 * z**2)

        p = (c2**2 - 3*c1) / 9
        q = (9*c1*c2 - 27*c0 - 2*c2**3) / 54
        omega = np.arccos(q / np.sqrt(p**3))

        s1 = 2 * np.sqrt(p) * np.cos(omega/3) - c2/3
        s2 = 2 * np.sqrt(p) * np.cos(omega/3 - 2*np.pi/3) - c2/3
        s3 = 2 * np.sqrt(p) * np.cos(omega/3 - 4*np.pi/3) - c2/3
        # print(s1, s2, s3)

        beta = np.arctan(np.sqrt((-self.b**2 - s2) / (self.ay**2 + s2)))
        lamb = np.arctan(np.sqrt((-self.ay**2 - s3) / (self.ax**2 + s3)))
        u = np.sqrt(self.b**2 + s1)

        return beta, lamb, u

    def cart2geod(self, mode: str, xG, yG, zG, maxIter=30, maxLoa=0.005):
        """
        Ligas 2012
        :param mode:
        :param xG:
        :param yG:
        :param zG:
        :param maxIter:
        :param maxLoa:
        :return:
        """
        rG = np.sqrt(xG**2 + yG**2 + zG**2)
        pE = np.array([self.ax * xG / rG, self.ax * yG / rG, self.ax * zG / rG], dtype=np.float64)

        E = 1 / self.ax**2
        F = 1 / self.ay**2
        G = 1 / self.b**2

        i = 0
        loa = np.inf

        while i < maxIter and loa > maxLoa:
            if mode == "ligas1":
                invJ, fxE = jacobian_Ligas.case1(E, F, G, np.array([xG, yG, zG]), pE)
            elif mode == "ligas2":
                invJ, fxE = jacobian_Ligas.case2(E, F, G, np.array([xG, yG, zG]), pE)
            elif mode == "ligas3":
                invJ, fxE = jacobian_Ligas.case3(E, F, G, np.array([xG, yG, zG]), pE)
            pEi = pE.reshape(-1, 1) - invJ @ fxE.reshape(-1, 1)
            pEi = pEi.reshape(1, -1).flatten()
            loa = np.sqrt((pEi[0]-pE[0])**2 + (pEi[1]-pE[1])**2 + (pEi[2]-pE[2])**2)
            pE = pEi
            i += 1

        phi = np.arctan((1-self.ee**2) / (1-self.ex**2) * pE[2] / np.sqrt((1-self.ee**2)**2 * pE[0]**2 + pE[1]**2))
        lamb = np.arctan(1/(1-self.ee**2) * pE[1]/pE[0])
        h = np.sign(zG-pE[2]) * np.sign(pE[2]) * np.sqrt((pE[0]-xG)**2 + (pE[1]-yG)**2 + (pE[2]-zG)**2)

        return phi, lamb, h

    def geod2cart(self, phi, lamb, h):
        """
        Ligas 2012, 250
        :param phi:
        :param lamb:
        :param h:
        :return:
        """
        v = self.ax / np.sqrt(1 - self.ex**2*np.sin(phi)**2-self.ee**2*np.cos(phi)**2*np.sin(lamb)**2)
        xG = (v + h) * np.cos(phi) * np.cos(lamb)
        yG = (v * (1-self.ee**2) + h) * np.cos(phi) * np.sin(lamb)
        zG = (v * (1-self.ex**2) + h) * np.sin(phi)
        return xG, yG, zG

    def para2cart(self, u, v):
        """
        Panou, Korakitits 2020, 4
        :param u:
        :param v:
        :return:
        """
        x = self.ax * np.cos(u) * np.cos(v)
        y = self.ay * np.cos(u) * np.cos(v)
        z = self.b * np.sin(u)

    def cart2para(self, x, y, z):
        """
        Panou, Korakitits 2020, 4
        :param x:
        :param y:
        :param z:
        :return:
        """
        u_check1 = z*np.sqrt(1 - self.ee**2)
        u_check2 = np.sqrt(x**2 * (1-self.ee**2) + y**2) * np.sqrt(1-self.ex**2)
        if u_check1 <= u_check2:
            u = np.arctan(u_check1 / u_check2)
        else:
            u = np.pi/2 * np.arctan(u_check2 / u_check1)

        v_check1 = y
        v_check2 = x*np.sqrt(1-self.ee**2)
        if v_check1 <= v_check2:
            v = 2 * np.arctan(v_check1 / (v_check2 + np.sqrt(x**2*(1-self.ee**2)+y**2)))
        else:
            v = np.pi/2 - 2 * np.arctan(v_check2 / (v_check1 + np.sqrt(x**2*(1-self.ee**2)+y**2)))
        return u, v

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

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

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

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

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

        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 louvilleConstant(ell, x, y, z):
    values = p_q(ell, x, y, z)
    pass



if __name__ == "__main__":
    ellips = EllipsoidTriaxial.init_name("Eitschberger1978")

    # carts = ellips.ell2cart(wu.deg2rad(10), wu.deg2rad(30), 6378172)
    # ells = ellips.cart2ell(carts[0], carts[1], carts[2])
    # print(aus.gms("beta", ells[0], 3), aus.gms("lambda", ells[1], 3), "u =", ells[2])
    # ells2 = ellips.cart2ell(5712200, 2663400, 1106000)
    # carts2 = ellips.ell2cart(ells2[0], ells2[1], ells2[2])
    # print(aus.xyz(carts2[0], carts2[1], carts2[2], 10))

    # stellen = 20
    # geod1 = ellips.cart2geod("ligas1", 5712200, 2663400, 1106000)
    # print(aus.gms("phi", geod1[0], stellen), aus.gms("lambda", geod1[1], stellen), "h =", geod1[2])
    # geod2 = ellips.cart2geod("ligas2", 5712200, 2663400, 1106000)
    # print(aus.gms("phi", geod2[0], stellen), aus.gms("lambda", geod2[1], stellen), "h =", geod2[2])
    # geod3 = ellips.cart2geod("ligas3", 5712200, 2663400, 1106000)
    # print(aus.gms("phi", geod3[0], stellen), aus.gms("lambda", geod3[1], stellen), "h =", geod3[2])
    # cart1 = ellips.geod2cart(geod1[0], geod1[1], geod1[2])
    # print(aus.xyz(cart1[0], cart1[1], cart1[2], 10))
    # cart2 = ellips.geod2cart(geod2[0], geod2[1], geod2[2])
    # print(aus.xyz(cart2[0], cart2[1], cart2[2], 10))
    # cart3 = ellips.geod2cart(geod3[0], geod3[1], geod3[2])
    # print(aus.xyz(cart3[0], cart3[1], cart3[2], 10))

    # test_cart = ellips.geod2cart(0.175, 0.444, 100)
    # print(aus.xyz(test_cart[0], test_cart[1], test_cart[2], 10))

    ellips.louvilleConstant(5712200, 2663400, 1106000)
    pass