from typing import Tuple

import numpy as np
from numpy import arctan2, cos, sin, sqrt
from numpy.typing import NDArray

import winkelumrechnungen as wu


class EllipsoidBiaxial:
    """
    Klasse für Rotationsellipdoide
    """
    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) -> EllipsoidBiaxial:
        """
        Erstellen eines Rotationsellipdoids nach Namen
        :param name: Name des Rotationsellipsoids
        :return: Rotationsellipsoid
        """
        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)
        else:
            raise Exception(f"EllipsoidBiaxial.init_name: Name {name} unbekannt")

    @classmethod
    def init_af(cls, a: float, f: float) -> EllipsoidBiaxial:
        """
        Erstellen eines Rotationsellipdoids aus der großen Halbachse und der Abplattung
        :param a: große Halbachse
        :param f: großen Halbachse
        :return: Rotationsellipsoid
        """
        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: arctan2(self.a * sin(beta), self.b * cos(beta))
    beta2phi = lambda self, beta: arctan2(self.a ** 2 * sin(beta), self.b ** 2 * cos(beta))

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

    phi2beta = lambda self, phi: arctan2(self.b**2 * sin(phi), self.a**2 * cos(phi))
    phi2psi = lambda self, phi: arctan2(self.b * sin(phi), self.a * 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 += 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])