import numpy as np
from numpy import sin, cos, arctan, arctan2, sqrt, pi, arccos
import winkelumrechnungen as wu
import jacobian_Ligas
import matplotlib.pyplot as plt
from typing import Tuple
from numpy.typing import NDArray
import math


class EllipsoidBiaxial:
    def __init__(self, a: float, b: float):
        self.a = a
        self.b = b
        self.c = a ** 2 / b
        self.e = sqrt(a ** 2 - b ** 2) / a
        self.e_ = 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: sqrt(1 + self.e_ ** 2 * 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.arctan2(self.a * np.sin(beta), self.b * np.cos(beta))
    beta2phi = lambda self, beta: np.arctan2(self.a ** 2 * np.sin(beta), self.b ** 2 * np.cos(beta))

    psi2beta = lambda self, psi: np.arctan2(self.b * np.sin(psi), self.a * np.cos(psi))
    psi2phi = lambda self, psi: np.arctan2(self.a * np.sin(psi), self.b * np.cos(psi))

    phi2beta = lambda self, phi: np.arctan2(self.b**2 * np.sin(phi), self.a**2 * np.cos(phi))
    phi2psi = lambda self, phi: np.arctan2(self.b * np.sin(phi), self.a * np.cos(phi))

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

    def bi_cart2ell(self, point: NDArray, Eh: float = 0.001, Ephi: float = wu.gms2rad([0, 0, 0.001])) -> Tuple[float, float, float]:
        """
        Umrechnung von kartesischen in ellipsoidische Koordinaten auf einem Rotationsellipsoid
        # TODO: Quelle
        :param point: Punkt in kartesischen Koordinaten
        :param Eh: Grenzwert für die Höhe
        :param Ephi: Grenzwert für die Breite
        :return: ellipsoidische Breite, Länge, geodätische Höhe
        """
        x, y, z = point

        lamb = arctan2(y, x)

        p = sqrt(x**2+y**2)

        phi_null = arctan2(z, p*(1 - self.e**2))

        hi = [0]
        phii = [phi_null]

        i = 0

        while True:
            N = self.a / sqrt(1 - self.e**2 * sin(phii[i])**2)
            h = p / cos(phii[i]) - N
            phi = arctan2(z,  p * (1-(self.e**2*N) / (N+h)))
            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
        return phi, lamb, h

    def bi_ell2cart(self, phi: float, lamb: float, h: float) -> NDArray:
        """
        Umrechnung von ellipsoidischen in kartesische Koordinaten auf einem Rotationsellipsoid
        # TODO: Quelle
        :param phi: ellipsoidische Breite
        :param lamb: ellipsoidische Länge
        :param h: geodätische Höhe
        :return: Punkt in kartesischen Koordinaten
        """
        W = sqrt(1 - self.e**2 * sin(phi)**2)
        N = self.a / W
        x = (N+h) * cos(phi) * cos(lamb)
        y = (N+h) * cos(phi) * sin(lamb)
        z = (N * (1-self.e**2) + h) * sin(phi)
        return np.array([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 = sqrt((self.ax**2 - self.b**2) / self.ax**2)
        self.ey = sqrt((self.ay**2 - self.b**2) / self.ay**2)
        self.ee = sqrt((self.ax**2 - self.ay**2) / self.ax**2)
        self.ex_ = sqrt((self.ax**2 - self.b**2) / self.b**2)
        self.ey_ = sqrt((self.ay**2 - self.b**2) / self.b**2)
        self.ee_ = sqrt((self.ax**2 - self.ay**2) / self.ay**2)
        self.Ex = sqrt(self.ax**2 - self.b**2)
        self.Ey = sqrt(self.ay**2 - self.b**2)
        self.Ee = sqrt(self.ax**2 - self.ay**2)

    @classmethod
    def init_name(cls, name: str):
        """
        Mögliche Ellipsoide: BursaFialova1993, BursaSima1980, BursaSima1980round, Eitschberger1978, Bursa1972,
        Bursa1970, BesselBiaxial, Fiction, KarneyTest2024
        Panou et al (2020)
        :param name: Name des dreiachsigen Ellipsoids
        """
        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 == "BursaSima1980round":
            # Panou 2013
            ax = 6378172
            ay = 6378103
            b = 6356753
            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 == "BesselBiaxial":
            ax = 6377397.15509
            ay = 6377397.15508
            b = 6356078.96290
            return cls(ax, ay, b)
        elif name == "Fiction":
            ax = 6000000
            ay = 4000000
            b = 2000000
            return cls(ax, ay, b)
        elif name == "KarneyTest2024":
            ax = sqrt(2)
            ay = 1
            b = 1 / sqrt(2)
            return cls(ax, ay, b)

    def func_H(self, point: NDArray) -> float:
        """
        Berechnung H
        Panou, Korakitis 2019 [43]
        :param point: Punkt
        :return: H
        """
        x, y, z = point
        return x ** 2 + y ** 2 / (1 - self.ee ** 2) ** 2 + z ** 2 / (1 - self.ex ** 2) ** 2

    def func_n(self, point: NDArray, H: float = None) -> NDArray:
        """
        Berechnung normalen Vektor
        Panou, Korakitis 2019 [9-12]
        :param point: Punkt
        :param H:
        :return:
        """
        if H is None:
            H = self.func_H(point)
        sqrtH = sqrt(H)
        x, y, z = point
        return np.array([x / sqrtH,
                         y / ((1 - self.ee ** 2) * sqrtH),
                         z / ((1 - self.ex ** 2) * sqrtH)])

    def func_t12(self, point: NDArray) -> Tuple[float, float]:
        """
        Berechnung Wurzeln
        Panou, Korakitis 2019 [9-12]
        :param point: Punkt
        :return: Wurzeln t1, t2
        """
        x, y, z = point
        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)
        if c1 ** 2 - 4 * c0 < 0:
            t2 = np.nan
        else:
            t2 = (-c1 + sqrt(c1 ** 2 - 4 * c0)) / 2
        if t2 == 0:
            t2 = 1e-18
        t1 = c0 / t2
        return t1, t2

    def ellu2cart(self, beta: float, lamb: float, u: float) -> NDArray:
        """
        Panou 2014 12ff.
        :param beta: ellipsoidische Breite
        :param lamb: ellipsoidische Länge
        :param u: radiale Koordinate entlang der kleinen Halbachse
        :return: Punkt in kartesischen Koordinaten
        """
        x = sqrt(u**2 + self.Ex**2) * sqrt(cos(beta)**2 + self.Ee**2/self.Ex**2 * sin(beta)**2) * cos(lamb)
        y = sqrt(u**2 + self.Ey**2) * cos(beta) * sin(lamb)
        z = u * sin(beta) * sqrt(1 - self.Ee**2/self.Ex**2 * cos(lamb)**2)

        return np.array([x, y, z])

    def cart2ellu(self, point: NDArray) -> Tuple[float, float, float]:
        """
        Panou 2014 15ff.
        :param point: Punkt in kartesischen Koordinaten
        :return: ellipsoidische Breite, ellipsoidische Länge, radiale Koordinate entlang der kleinen Halbachse
        """
        x, y, z = point
        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 = arccos(q / sqrt(p**3))

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

        beta = arctan(sqrt((-self.b**2 - s2) / (self.ay**2 + s2)))
        if abs((-self.ay**2 - s3) / (self.ax**2 + s3)) > 1e-7:
            lamb = arctan(sqrt((-self.ay**2 - s3) / (self.ax**2 + s3)))
        else:
            lamb = 0
        u = sqrt(self.b**2 + s1)

        return beta, lamb, u

    def ell2cart(self, beta: float | NDArray, lamb: float | NDArray) -> NDArray:
        """
        Panou, Korakitis 2019 2
        :param beta: ellipsoidische Breite
        :param lamb: ellipsoidische Länge
        :return: Punkt in kartesischen Koordinaten
        """
        beta = np.asarray(beta, dtype=float)
        lamb = np.asarray(lamb, dtype=float)

        beta, lamb = np.broadcast_arrays(beta, lamb)

        B = self.Ex ** 2 * cos(beta) ** 2 + self.Ee ** 2 * sin(beta) ** 2
        L = self.Ex ** 2 - self.Ee ** 2 * cos(lamb) ** 2

        x = self.ax / self.Ex * sqrt(B) * cos(lamb)
        y = self.ay * cos(beta) * sin(lamb)
        z = self.b / self.Ex * sin(beta) * sqrt(L)

        xyz = np.stack((x, y, z), axis=-1)

        # Pole
        mask_south = beta == -pi / 2
        mask_north = beta == pi / 2
        xyz[mask_south] = np.array([0, 0, -self.b])
        xyz[mask_north] = np.array([0, 0, self.b])

        # Äquator
        mask_eq = beta == 0
        xyz[mask_eq & (lamb == -pi / 2)] = np.array([0, -self.ay, 0])
        xyz[mask_eq & (lamb == pi / 2)] = np.array([0, self.ay, 0])
        xyz[mask_eq & (lamb == 0)] = np.array([self.ax, 0, 0])
        xyz[mask_eq & (lamb == pi)] = np.array([-self.ax, 0, 0])

        return xyz

    def ell2cart_bektas(self, beta: float | NDArray, omega: float | NDArray) -> NDArray:
        """
        Bektas 2015
        :param beta: ellipsoidische Breite
        :param omega: ellipsoidische Länge
        :return: Punkt in kartesischen Koordinaten
        """
        x = self.ax * cos(omega) * sqrt((self.ax**2 - self.ay**2 * sin(beta)**2 - self.b**2 * cos(beta)**2) / (self.ax**2 - self.b**2))
        y = self.ay * cos(beta) * sin(omega)
        z = self.b * sin(beta) * sqrt((self.ax**2 * sin(omega)**2 + self.ay**2 * cos(omega)**2 - self.b**2) / (self.ax**2 - self.b**2))

        return np.array([x, y, z])

    def ell2cart_karney(self, beta: float | NDArray, lamb: float | NDArray) -> NDArray:
        """
        Karney 2025 Geographic Lib
        :param beta: ellipsoidische Breite
        :param lamb: ellipsoidische Länge
        :return: Punkt in kartesischen Koordinaten
        """
        k = sqrt(self.ay**2 - self.b**2) / sqrt(self.ax**2 - self.b**2)
        k_ = sqrt(self.ax**2 - self.ay**2) / sqrt(self.ax**2 - self.b**2)
        X = self.ax * cos(lamb) * sqrt(k**2*cos(beta)**2+k_**2)
        Y = self.ay * cos(beta) * sin(lamb)
        Z = self.b * sin(beta) * sqrt(k**2 + k_**2 * sin(lamb)**2)
        return np.array([X, Y, Z])

    def cart2ell_yFake(self, point: NDArray, delta_y: float = 1e-4) -> Tuple[float, float]:
        """
        Bei Fehlschlagen von cart2ell
        :param point: Punkt in kartesischen Koordinaten
        :param delta_y: Startwert für Suche nach kleinstmöglichem delta_y
        :return: ellipsoidische Breite und Länge
        """
        x, y, z = point
        best_delta = np.inf
        while True:
            try:
                y1 = y - delta_y
                beta1, lamb1 = self.cart2ell(np.array([x, y1, z]), noFake=True)
                point1 = self.ell2cart(beta1, lamb1)

                y2 = y + delta_y
                beta2, lamb2 = self.cart2ell(np.array([x, y2, z]), noFake=True)
                point2 = self.ell2cart(beta2, lamb2)

                pointM = (point1 + point2) / 2

                actual_delta = np.linalg.norm(point - pointM)
            except:
                actual_delta = np.inf

            if actual_delta < best_delta:
                best_delta = actual_delta
                delta_y /= 10
            else:
                delta_y *= 10

                y1 = y - delta_y
                beta1, lamb1 = self.cart2ell(np.array([x, y1, z]), noFake=True)

                return beta1, lamb1

    def cart2ell(self, point: NDArray, eps: float = 1e-12, maxI: int = 100, noFake: bool = False) -> Tuple[float, float]:
        """
        Panou, Korakitis 2019 3f. (num)
        :param point:  Punkt in kartesischen Koordinaten
        :param eps: zu erreichende Genauigkeit
        :param maxI: maximale Anzahl Iterationen
        :param noFake: y numerisch anpassen?
        :return: ellipsoidische Breite und Länge
        """
        x, y, z = point
        beta, lamb = self.cart2ell_panou(point)
        delta_ell = np.array([np.inf, np.inf]).T
        tiny = 1e-30

        try:
            i = 0
            while np.linalg.norm(delta_ell) > eps and i < maxI:
                x0, y0, z0 = self.ell2cart(beta, lamb)
                delta_l = np.array([x-x0, y-y0, z-z0]).T

                B = max(self.Ex ** 2 * cos(beta) ** 2 + self.Ee ** 2 * sin(beta) ** 2, tiny)
                L = max(self.Ex ** 2 - self.Ee ** 2 * cos(lamb) ** 2, tiny)

                J = np.array([[(-self.ax * self.Ey ** 2) / (2 * self.Ex) * sin(2 * beta) / sqrt(B) * cos(lamb),
                               -self.ax / self.Ex * sqrt(B) * sin(lamb)],
                              [-self.ay * sin(beta) * sin(lamb),
                               self.ay * cos(beta) * cos(lamb)],
                              [self.b / self.Ex * cos(beta) * sqrt(L),
                               (self.b * self.Ee ** 2) / (2 * self.Ex) * sin(beta) * sin(2 * lamb) / sqrt(L)]])

                N = J.T @ J
                det = N[0, 0] * N[1, 1] - N[0, 1] * N[1, 0]
                N_inv = 1 / det * np.array([[N[1, 1], -N[0, 1]], [-N[1, 0], N[0, 0]]])
                delta_ell = N_inv @ J.T @ delta_l
                # delta_ell, *_ = np.linalg.lstsq(J, delta_l, rcond=None)

                beta += delta_ell[0]
                lamb += delta_ell[1]
                i += 1

            if i == maxI:
                raise Exception("Umrechnung cart2ell: nicht konvergiert")

            point_n = self.ell2cart(beta, lamb)
            delta_r = np.linalg.norm(point - point_n, axis=-1)
            if delta_r > 1e-6:
                raise Exception("Umrechnung cart2ell: Punktdifferenz")

            return beta, lamb

        except Exception as e:
            # Wenn die Berechnung fehlschlägt auf Grund von sehr kleinem y, solange anpassen, bis Umrechnung ohne Fehler
            delta_y = 10 ** math.floor(math.log10(abs(self.ay/1000)))
            if abs(y) < delta_y and not noFake:
                return self.cart2ell_yFake(point, delta_y)
            else:
                raise e

    def cart2ell_panou(self, point: NDArray) -> Tuple[float, float]:
        """
        Panou, Korakitis 2019 2f. (analytisch -> Näherung)
        :param point: Punkt in kartesischen Koordinaten
        :return: ellipsoidische Breite und Länge
        """
        x, y, z = point

        eps = 1e-9

        if abs(x) < eps and abs(y) < eps:  # Punkt in der z-Achse
            beta = pi / 2 if z > 0 else -pi / 2
            lamb = 0.0
            return beta, lamb

        elif abs(x) < eps and abs(z) < eps:  # Punkt in der y-Achse
            beta = 0.0
            lamb = pi / 2 if y > 0 else -pi / 2
            return beta, lamb

        elif abs(y) < eps and abs(z) < eps:  # Punkt in der x-Achse
            beta = 0.0
            lamb = 0.0 if x > 0 else pi
            return beta, lamb

        # ---- Allgemeiner Fall -----

        t1, t2 = self.func_t12(point)

        num_beta = max(t1 - self.b ** 2, 0)
        den_beta = max(self.ay ** 2 - t1, 1e-30)
        num_lamb = max(t2 - self.ay ** 2, 0)
        den_lamb = max(self.ax ** 2 - t2, 1e-30)

        beta = arctan(sqrt(num_beta / den_beta))
        lamb = arctan(sqrt(num_lamb / den_lamb))

        if z < 0:
            beta = -beta

        if y < 0:
            lamb = -lamb

        if x < 0:
            lamb = pi - lamb

        if abs(x) < eps:
            lamb = -pi/2 if y < 0 else pi/2

        elif abs(y) < eps:
            lamb = 0 if x > 0 else pi

        elif abs(z) < eps:
            beta = 0

        return beta, lamb

    def cart2ell_bektas(self, point: NDArray, eps: float = 1e-12, maxI: int = 100) -> Tuple[float, float]:
        """
        Bektas 2015
        :param point:  Punkt in kartesischen Koordinaten
        :param eps: zu erreichende Genauigkeit
        :param maxI: maximale Anzahl Iterationen
        :return: ellipsoidische Breite und Länge
        """
        x, y, z = point
        phi, lamb = self.cart2para(point)
        p = sqrt((self.ax**2 - self.ay**2) / (self.ax**2 - self.b**2))
        d_phi = np.inf
        d_lamb = np.inf

        i = 0
        while d_phi > eps and d_lamb > eps and i < maxI:
            lamb_new = arctan2(self.ax * y * sqrt((p**2-1) * sin(phi)**2 + 1), self.ay * x * cos(phi))
            phi_new = arctan2(self.ay * z * sin(lamb), self.b * y * sqrt(1 - p**2 * cos(lamb)**2))
            d_phi = abs(phi_new - phi)
            phi = phi_new
            d_lamb = abs(lamb_new - lamb)
            lamb = lamb_new
            i += 1

        if i == maxI:
            raise Exception("Umrechnung cart2ell: nicht konvergiert")

        return phi, lamb

    def geod2cart(self, phi: float | NDArray, lamb: float | NDArray, h: float) -> NDArray:
        """
        Ligas 2012, 250
        :param phi: geodätische Breite
        :param lamb: geodätische Länge
        :param h: geodätische Höhe
        :return: kartesische Koordinaten
        """
        v = self.ax / sqrt(1 - self.ex**2*sin(phi)**2-self.ee**2*cos(phi)**2*sin(lamb)**2)
        xG = (v + h) * cos(phi) * cos(lamb)
        yG = (v * (1-self.ee**2) + h) * cos(phi) * sin(lamb)
        zG = (v * (1-self.ex**2) + h) * sin(phi)
        return np.array([xG, yG, zG])

    def cart2geod(self, point: NDArray, mode: str = "ligas3", maxIter: int = 30, maxLoa: float = 0.005) -> Tuple[float, float, float]:
        """
        Ligas 2012
        :param mode: ligas1, ligas2, oder ligas3
        :param point: Punkt in kartesischen Koordinaten
        :param maxIter: maximale Anzahl Iterationen
        :param maxLoa: Level of Accuracy, das erreicht werden soll
        :return: geodätische Breite, Länge, Höhe
        """
        xG, yG, zG = point

        eps = 1e-9

        if abs(xG) < eps and abs(yG) < eps:  # Punkt in der z-Achse
            phi = pi / 2 if zG > 0 else -pi / 2
            lamb = 0.0
            h = abs(zG) - self.b
            return phi, lamb, h

        elif abs(xG) < eps and abs(zG) < eps:  # Punkt in der y-Achse
            phi = 0.0
            lamb = pi / 2 if yG > 0 else -pi / 2
            h = abs(yG) - self.ay
            return phi, lamb, h

        elif abs(yG) < eps and abs(zG) < eps:  # Punkt in der x-Achse
            phi = 0.0
            lamb = 0.0 if xG > 0 else pi
            h = abs(xG) - self.ax
            return phi, lamb, h

        rG = 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 = sqrt((pEi[0]-pE[0])**2 + (pEi[1]-pE[1])**2 + (pEi[2]-pE[2])**2)
            pE = pEi
            i += 1

        if i == maxIter and loa > maxLoa:
            act_mode = int(mode[-1])
            new_mode = 3 if act_mode == 1 else act_mode - 1
            phi, lamb, h = self.cart2geod(point, f"ligas{new_mode}", maxIter, maxLoa)

        else:
            phi = arctan((1-self.ee**2) / (1-self.ex**2) * pE[2] / sqrt((1-self.ee**2)**2 * pE[0]**2 + pE[1]**2))
            lamb = arctan(1/(1-self.ee**2) * pE[1]/pE[0])
            h = np.sign(zG - pE[2]) * np.sign(pE[2]) * sqrt((pE[0] - xG) ** 2 + (pE[1] - yG) ** 2 + (pE[2] - zG) ** 2)
            if h < -self.ax:
                act_mode = int(mode[-1])
                new_mode = 3 if act_mode == 1 else act_mode - 1
                phi, lamb, h = self.cart2geod(point, f"ligas{new_mode}", maxIter, maxLoa)
            else:
                if xG < 0 and yG < 0:
                    lamb = -pi + lamb

                elif xG < 0:
                    lamb = pi + lamb

                if abs(zG) < eps:
                    phi = 0

        return phi, lamb, h

    def para2cart(self, u: float | NDArray, v: float | NDArray) -> NDArray:
        """
        Panou, Korakitits 2020, 4
        :param u: parametrische Breite
        :param v: parametrische Länge
        :return: Punkt in kartesischen Koordinaten
        """
        x = self.ax * cos(u) * cos(v)
        y = self.ay * cos(u) * sin(v)
        z = self.b * sin(u)
        z = np.broadcast_to(z, np.shape(x))
        return np.array([x, y, z])

    def cart2para(self, point: NDArray) -> Tuple[float, float]:
        """
        Panou, Korakitits 2020, 4
        :param point: Punkt in kartesischen Koordinaten
        :return: parametrische Breite, Länge
        """
        x, y, z = point

        u_check1 = z*sqrt(1 - self.ee**2)
        u_check2 = sqrt(x**2 * (1-self.ee**2) + y**2) * sqrt(1-self.ex**2)
        if u_check1 <= u_check2:
            u = arctan2(u_check1, u_check2)
        else:
            u = pi/2 - arctan2(u_check2, u_check1)

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

        return u, v

    def ell2para(self, beta: float, lamb: float) -> Tuple[float, float]:
        """
        Umrechung von ellipsoidischen in parametrische Koordinaten (über kartesische Koordinaten)
        :param beta: ellipsoidische Breite
        :param lamb: ellipsoidische Länge
        :return: parametrische Breite, Länge
        """
        cart = self.ell2cart(beta, lamb)
        return self.cart2para(cart)

    def para2ell(self, u: float, v: float) -> Tuple[float, float]:
        """
        Umrechung von parametrischen in ellipsoidische Koordinaten (über kartesische Koordinaten)
        :param u: parametrische Breite
        :param v: parametrische Länge
        :return: ellipsoidische Breite, Länge
        """
        cart = self.para2cart(u, v)
        return self.cart2ell(cart)

    def para2geod(self, u: float, v: float, mode: str = "ligas3", maxIter: int = 30, maxLoa: float = 0.005) -> Tuple[float, float, float]:
        """
        Umrechung von parametrischen in geodätische Koordinaten (über kartesische Koordinaten)
        :param u: parametrische Breite
        :param v: parametrische Länge
        :param mode: ligas1, ligas2, oder ligas3
        :param maxIter: maximale Anzahl Iterationen
        :param maxLoa: Level of Accuracy, das erreicht werden soll
        :return: geodätische Breite, Länge, Höhe
        """
        cart = self.para2cart(u, v)
        return self.cart2geod(cart, mode, maxIter, maxLoa)

    def geod2para(self, phi: float, lamb: float, h: float) -> Tuple[float, float]:
        """
        Umrechung von geodätischen in parametrische Koordinaten (über kartesische Koordinaten)
        :param phi: geodätische Breite
        :param lamb: geodätische Länge
        :param h: geodätische Höhe
        :return: parametrische Breite, Länge
        """
        cart = self.geod2cart(phi, lamb, h)
        return self.cart2para(cart)

    def ell2geod(self, beta: float, lamb: float, mode: str = "ligas3", maxIter: int = 30, maxLoa: float = 0.005) -> Tuple[float, float, float]:
        """
        Umrechung von ellipsoidischen in geodätische Koordinaten (über kartesische Koordinaten)
        :param beta: ellipsoidische Breite
        :param lamb: ellipsoidische Länge
        :param mode: ligas1, ligas2, oder ligas3
        :param maxIter: maximale Anzahl Iterationen
        :param maxLoa: Level of Accuracy, das erreicht werden soll
        :return: geodätische Breite, Länge, Höhe
        """
        cart = self.ell2cart(beta, lamb)
        return self.cart2geod(cart, mode, maxIter, maxLoa)

    def geod2ell(self, phi: float, lamb: float, h: float) -> Tuple[float, float]:
        """
        Umrechung von geodätischen in ellipsoidische Koordinaten (über kartesische Koordinaten)
        :param phi: geodätische Breite
        :param lamb: geodätische Länge
        :param h: geodätische Höhe
        :return: ellipsoidische Breite, Länge
        """
        cart = self.geod2cart(phi, lamb, h)
        return self.cart2ell(cart)

    def point_on(self, point: NDArray) -> bool:
        """
        Test, ob ein Punkt auf dem Ellipsoid liegt
        :param point: kartesische 3D-Koordinaten
        :return: Punkt auf dem Ellispoid?
        """
        value = point[0]**2/self.ax**2 + point[1]**2/self.ay**2 + point[2]**2/self.b**2
        if abs(1-value) < 1e-6:
            return True
        else:
            return False

    def point_onto_ellipsoid(self, point: NDArray) -> NDArray:
        """
        Berechnung des Lotpunktes entlang der Normalkrümmung auf einem Ellipsoiden
        :param point: Punkt in kartesischen Koordinaten, der gelotet werden soll
        :return: Lotpunkt in kartesischen Koordinaten
        """
        phi, lamb, h = self.cart2geod(point, "ligas3")
        p = self. geod2cart(phi, lamb, 0)
        return p

    def cart_ellh(self, point: NDArray, h: float) -> NDArray:
        """
        Punkt auf Ellipsoid hoch loten
        :param point: Punkt auf dem Ellipsoid
        :param h: Höhe über dem Ellipsoid
        :return: hochgeloteter Punkt
        """
        phi, lamb, _ = self.cart2geod(point, "ligas3")
        pointH = self. geod2cart(phi, lamb, h)
        return pointH


if __name__ == "__main__":
    ell = EllipsoidTriaxial.init_name("BursaSima1980")
    diff_list = []
    diffs_para = []
    diffs_ell = []
    diffs_geod = []
    points = []
    for v_deg in range(-180, 181, 5):
        for u_deg in range(-90, 91, 5):
            v = wu.deg2rad(v_deg)
            u = wu.deg2rad(u_deg)
            point = ell.para2cart(u, v)
            points.append(point)

            elli = ell.cart2ell(point)
            cart_elli = ell.ell2cart(elli[0], elli[1])
            diff_ell = np.linalg.norm(point - cart_elli, axis=-1)

            para = ell.cart2para(point)
            cart_para = ell.para2cart(para[0], para[1])
            diff_para = np.linalg.norm(point - cart_para, axis=-1)

            geod = ell.cart2geod(point, "ligas3")
            cart_geod = ell.geod2cart(geod[0], geod[1], geod[2])
            diff_geod3 = np.linalg.norm(point - cart_geod, axis=-1)

            diff_list.append([v_deg, u_deg, diff_ell, diff_para, diff_geod3])
            diffs_ell.append([diff_ell])
            diffs_para.append([diff_para])
            diffs_geod.append([diff_geod3])

    diff_list = np.array(diff_list)
    diffs_ell = np.array(diffs_ell)
    diffs_para = np.array(diffs_para)
    diffs_geod = np.array(diffs_geod)

    pass

    points = np.array(points)
    fig = plt.figure()
    ax = fig.add_subplot(projection='3d')

    sc = ax.scatter(
        points[:, 0],
        points[:, 1],
        points[:, 2],
        c=diffs_ell,  # Farbcode = diff
        cmap='viridis',  # Colormap
        s=10 + 20 * diffs_ell,  # optional: Größe abhängig vom diff
        alpha=0.8
    )

    # Farbskala
    cbar = plt.colorbar(sc)
    cbar.set_label("diff")

    ax.set_xlabel("X")
    ax.set_ylabel("Y")
    ax.set_zlabel("Z")

    plt.show()