from typing import Any
import numpy as np

import Datenbank

class Berechnungen:
    """Geodätische Hilfsberechnungen auf einem Rotationsellipsoid.

    Die Klasse stellt Methoden zur Verfügung für:

    - erste numerische Exzentrizität e² und zweite numerische Exzentrizität e'²,
    - Umrechnung ECEF (X, Y, Z) → geodätische Breite B, geodätische Länge L und ellipsoidische Höhe H,
    - lokale ENU-Komponenten (E, N, U),
    - Horizontalstrecke, Zenitwinkel, Azimut und Richtung,
    - Berechnung von Azimut, Richtung und Zenitwinkel aus Tachymeterbeobachtungen.

    :ivar a_wert: Große Halbachse a in Meter.
    :vartype a_wert: float
    :ivar b_wert: Kleine Halbachse b in Meter.
    :vartype b_wert: float
    :ivar e_quadrat_wert: Quadrat der ersten numerischen Exzentrizität e² (einheitenlos).
    :vartype e_quadrat_wert: float
    :ivar e_strich_quadrat_wert: Quadrat der zweiten numerischen Exzentrizität e'² (einheitenlos).
    :vartype e_strich_quadrat_wert: float
    """

    def __init__(self, a: float, b: float) -> None:
        """Initialisiert die Ellipsoidparameter.

        Berechnet die erste und zweite numerische Exzentrizität.

        :param a: Große Halbachse a des Rotationsellipsoids in Meter.
        :type a: float
        :param b: Kleine Halbachse b des Rotationsellipsoids in Meter.
        :type b: float
        """
        self.a_wert = a
        self.b_wert = b
        self.e_quadrat_wert = self.e_quadrat()
        self.e_strich_quadrat_wert = self.e_strich_quadrat()

    def e_quadrat(self) -> float:
        """Berechnet das Quadrat der ersten numerischen Exzentrizität e².

        Es gilt: e² = (a² − b²) / a², wobei a die große und b die kleine Halbachse ist.

        :return: Quadrat der ersten numerischen Exzentrizität e² (einheitenlos).
        :rtype: float
        """
        return (self.a_wert**2 - self.b_wert**2) / self.a_wert **2

    def e_strich_quadrat(self) -> float:
        """Berechnet das Quadrat der zweiten numerischen Exzentrizität e'².

        Es gilt: e'² = (a² − b²) / b², wobei a die große und b die kleine Halbachse ist

        :return: Quadrat der zweiten numerischen Exzentrizität e'² (einheitenlos).
        :rtype: float
        """
        return (self.a_wert**2 - self.b_wert**2) / self.b_wert **2

    def P(self, x: float, y: float) -> float:
        """Berechnet den Hilfswert P aus geozentrischen Koordinaten.

        Es gilt: P = sqrt(x² + y²).

        :param x: Geozentrische kartesische X-Koordinate (ECEF) in Meter.
        :type x: float
        :param y: Geozentrische kartesische Y-Koordinate (ECEF) in Meter.
        :type y: float
        :return: Hilfswert P in Meter.
        :rtype: float
        """
        return np.sqrt(x**2 + y**2)

    def hilfswinkel(self, x: float, y: float, z: float) -> float:
        """Berechnet den Hilfswinkel für die Bestimmung der geodätischen Breite.

        Es gilt: hw = atan2(z * a, P(x, y) * b).

        :param x: Geozentrische kartesische X-Koordinate (ECEF) in Meter.
        :type x: float
        :param y: Geozentrische kartesische Y-Koordinate (ECEF) in Meter.
        :type y: float
        :param z: Geozentrische kartesische Z-Koordinate (ECEF) in Meter.
        :type z: float
        :return: Hilfswinkel in Radiant.
        :rtype: float
        """
        hw = np.atan2(z * self.a_wert, self.P(x, y) * self.b_wert)
        return hw

    def B(self, x: float, y: float, z: float) -> float:
        """Berechnet die geodätische Breite B aus geozentrischen Koordinaten.

        Verwendet den Hilfswinkel, e² und e'² zur Berechnung.

        :param x: Geozentrische kartesische X-Koordinate (ECEF) in Meter.
        :type x: float
        :param y: Geozentrische kartesische Y-Koordinate (ECEF) in Meter.
        :type y: float
        :param z: Geozentrische kartesische Z-Koordinate (ECEF) in Meter.
        :type z: float
        :return: Geodätische Breite B in Radiant.
        :rtype: float
        """
        hilfswinkel = self.hilfswinkel(x, y, z)
        B = np.atan2((z + self.e_strich_quadrat_wert * self.b_wert * np.sin(hilfswinkel) ** 3), (self.P(x, y) - self.e_quadrat_wert * self.a_wert * np.cos(hilfswinkel) ** 3))
        return B

    def L(self, x: float, y: float) -> float:
        """Berechnet die geodätische Länge L aus geozentrischen Koordinaten.

        Es gilt: L = atan2(y, x).

        :param x: Geozentrische kartesische X-Koordinate (ECEF) in Meter.
        :type x: float
        :param y: Geozentrische kartesische Y-Koordinate (ECEF) in Meter.
        :type y: float
        :return: Geodätische Länge L in Radiant.
        :rtype: float
        """
        return np.atan2(y, x)

    def H(self, x: float, y: float, z: float) -> float:
        """Berechnet die ellipsoidische Höhe H aus geozentrisch kartesischen Koordinaten.

        Die ellipsoidische Höhe wird mithilfe der geodätischen Breite B und der Ellipsoidparameter berechnet.

        :param x: Geozentrische kartesische X-Koordinate (ECEF) in Meter.
        :type x: float
        :param y: Geozentrische kartesische Y-Koordinate (ECEF) in Meter.
        :type y: float
        :param z: Geozentrische kartesische Z-Koordinate (ECEF) in Meter.
        :type z: float
        :return: Ellipsoidische Höhe H in Meter.
        :rtype: float
        """
        B = self.B(x, y, z)
        H = (self.P(x, y) / np.cos(B)) - self.a_wert / (np.sqrt(1 - self.e_quadrat_wert * np.sin(B) ** 2))
        return H

    def E(self, L: float, dX: float, dY: float) -> float:
        """Berechnet die Ostkomponente E im lokalen ENU-System.

        :param L: Geodätische Länge in Radiant.
        :type L: float
        :param dX: Differenz dX = X2 − X1 in Meter.
        :type dX: float
        :param dY: Differenz dY = Y2 − Y1 in Meter.
        :type dY: float
        :return: Ostkomponente E in Meter.
        :rtype: float
        """
        E = -np.sin(L) * dX + np.cos(L) * dY
        return E

    def N(self, B: float, L: float, dX: float, dY: float, dZ: float) -> float:
        """Berechnet die Nordkomponente N im lokalen ENU-System.

        :param B: Geodätische Breite in Radiant.
        :type B: float
        :param L: Geodätische Länge in Radiant.
        :type L: float
        :param dX: Differenz dX = X2 − X1 in Meter.
        :type dX: float
        :param dY: Differenz dY = Y2 − Y1 in Meter.
        :type dY: float
        :param dZ: Differenz dZ = Z2 − Z1 in Meter.
        :type dZ: float
        :return: Nordkomponente N in Meter.
        :rtype: float
        """
        N = -np.sin(B) * np.cos(L) * dX - np.sin(B) * np.sin(L) * dY + np.cos(B) * dZ
        return N

    def U(self, B: float, L: float, dX: float, dY: float, dZ: float) -> float:
        """Berechnet die Up-Komponente U im lokalen ENU-System.

        :param B: Geodätischee Breite in Radiant.
        :type B: float
        :param L: Geodätische Länge in Radiant.
        :type L: float
        :param dX: Differenz dX = X2 − X1 in Meter.
        :type dX: float
        :param dY: Differenz dY = Y2 − Y1 in Meter.
        :type dY: float
        :param dZ: Differenz dZ = Z2 − Z1 in Meter.
        :type dZ: float
        :return: Up-Komponente U in Meter.
        :rtype: float
        """
        U = np.cos(B) * np.cos(L) * dX + np.cos(B) * np.sin(L) *dY + np.sin(B) * dZ
        return U

    def horizontalstrecke_ENU(self, E: float, N: float) -> float:
        """Berechnet die Horizontalstrecke aus den ENU-Komponenten E und N.

        Es gilt: s = sqrt(E² + N²).

        :param E: Ostkomponente E in Meter.
        :type E: float
        :param N: Nordkomponente N in Meter.
        :type N: float
        :return: Horizontalstrecke in Meter.
        :rtype: float
        """
        s = np.sqrt(E**2 + N**2)
        return s

    def Zenitwinkel(self, horizontalstrecke_ENU: float, U: float) -> float:
        """Berechnet den Zenitwinkel aus Horizontalstrecke und Up-Komponente.

        Es gilt: zw = atan2(horizontalstrecke_ENU, U).

        :param horizontalstrecke_ENU: Horizontalstrecke in Meter.
        :type horizontalstrecke_ENU: float
        :param U: Up-Komponente U in Meter.
        :type U: float
        :return: Zenitwinkel in Radiant.
        :rtype: float
        """
        zw = np.atan2(horizontalstrecke_ENU, U)
        return zw

    def Azimut(self, E: float, N: float) -> float:
        """Berechnet den Azimut aus den ENU-Komponenten E und N.

        Der Azimut wird auf [0, 2π) normiert.

        :param E: Ostkomponente E in Meter.
        :type E: float
        :param N: Nordkomponente N in Meter.
        :type N: float
        :return: Azimut in Radiant im Bereich [0, 2π).
        :rtype: float
        """
        Azimut = np.atan2(E, N)
        if Azimut < 0:
            Azimut += 2 * np.pi
        if Azimut > 2 * np.pi:
            Azimut -= 2 * np.pi
        return Azimut

    def Richtung(self, Azimut: float, Orientierung: float) -> float:
        """Berechnet die Richtung aus Azimut und Orientierung.

        Die Richtung wird auf [0, 2π) normiert.

        :param Azimut: Azimut in Radiant.
        :type Azimut: float
        :param Orientierung: Orientierung in Radiant.
        :type Orientierung: float
        :return: Richtung in Radiant im Bereich [0, 2π).
        :rtype: float
        """
        Richtung = Azimut - Orientierung
        if Richtung < 0:
            Richtung += 2 * np.pi
        if Richtung > 2 * np.pi:
            Richtung -= 2 * np.pi
        return Richtung

    def geodätische_breite_laenge(self, dict_koordinaten: dict) -> dict:
        """Berechnet geodätische Breite und Länge für alle Punkte eines Koordinatendictionaries.

        Die Einträge des Dictionaries werden in einer Schleife erweitert zu [Koordinatenmatrix, B, L], wobei Koordinatenmatrix = (X, Y, Z) ist.

        :param dict_koordinaten: Dictionary mit geozentrischen Koordinaten je Punkt.
        :type dict_koordinaten: dict
        :return: Das übergebene Dictionary mit erweiterten Einträgen.
        :rtype: dict
        """
        for punktnummer, matrix in dict_koordinaten.items():

            dict_koordinaten[punktnummer] = [matrix, self.B(matrix[0], matrix[1], matrix[2]), self.L(matrix[0], matrix[1])]
        return dict_koordinaten

    def berechnung_richtung_azimut_zenitwinkel(self, pfad_datenbank: str, dict_koordinaten: dict) -> tuple[list[Any], dict[Any, Any]]:
        """Berechnet Azimut, Richtung und Zenitwinkel aus Tachymeterbeobachtungen.

        Die Tachymeterbeobachtungen werden aus der Datenbank gelesen. Für jede Beobachtung
        (Standpunkt → Zielpunkt) werden aus den Koordinatendifferenzen die lokalen ENU-Komponenten
        am Standpunkt berechnet und daraus Azimut, Richtung und Zenitwinkel berechnet.

        Die Orientierung wird pro Beobachtungsgruppe durch den ersten Azimut der Gruppe gesetzt.

        :param pfad_datenbank: Pfad zur Datenbank.
        :type pfad_datenbank: str
        :param dict_koordinaten: Dictionary mit geozentrisch kartesischen Koordinaten je Punkt (ECEF).
        :type dict_koordinaten: dict
        :return: Tupel aus Ergebnisliste mit (beobachtungsgruppeID, standpunkt, zielpunkt, Azimut, richtung, Zenitwinkel, schraegstrecke, orientierung) und Dictionary der Orientierungen je Beobachtungsgruppe. Winkel in Radiant, Strecken in Meter.
        :rtype: tuple[list[Any], dict[Any, Any]]
        """
        dict_koordinaten_erweitert = {}
        dict_orientierungen = {}
        liste_azimut_richtungen = []
        db_zugriff = Datenbank.Datenbankzugriff(pfad_datenbank)
        liste_beobachtungen_tachymeter = db_zugriff.get_beobachtungen_from_beobachtungenid()

        for punktnummer, koordinaten in dict_koordinaten.items():
            X = float(koordinaten[0])
            Y = float(koordinaten[1])
            Z = float(koordinaten[2])

            P = self.P(X, Y)
            hilfwinkel = self.hilfswinkel(X, Y, Z)
            B = self.B(X, Y, Z)
            L = self.L(X, Y)
            H = self.H(X, Y, Z)

            dict_koordinaten_erweitert[punktnummer] = koordinaten, P, hilfwinkel, B, L, H

        beobachtsgruppeID_vorher = None
        for beobachtung_tachymeter in liste_beobachtungen_tachymeter:
            standpunkt = beobachtung_tachymeter[0]
            zielpunkt = beobachtung_tachymeter[1]

            X1 = dict_koordinaten[beobachtung_tachymeter[0]][0]
            X2 = dict_koordinaten[beobachtung_tachymeter[1]][0]
            Y1 = dict_koordinaten[beobachtung_tachymeter[0]][1]
            Y2 = dict_koordinaten[beobachtung_tachymeter[1]][1]
            Z1 = dict_koordinaten[beobachtung_tachymeter[0]][2]
            Z2 = dict_koordinaten[beobachtung_tachymeter[1]][2]

            dX = float(X2 - X1)
            dY = float(Y2 - Y1)
            dZ = float(Z2 - Z1)

            schraegstrecke = np.sqrt(dX ** 2 + dY ** 2 + dZ ** 2)

            B = float(dict_koordinaten_erweitert[beobachtung_tachymeter[0]][3])
            L = float(dict_koordinaten_erweitert[beobachtung_tachymeter[0]][4])

            E = self.E(L, dX, dY)
            N = self.N(B, L, dX, dY, dZ)
            U = self.U(B, L, dX, dY, dZ)

            horizontalstrecke_ENU = self.horizontalstrecke_ENU(E, N)
            Azimut = float(self.Azimut(E, N))
            Zenitwinkel = float(self.Zenitwinkel(horizontalstrecke_ENU, U))

            beobachtsgruppeID_aktuell = beobachtung_tachymeter[3]
            if beobachtsgruppeID_aktuell == beobachtsgruppeID_vorher:
                richtung = float(self.Richtung(Azimut, orientierung))
                liste_azimut_richtungen.append((beobachtsgruppeID_aktuell, standpunkt, zielpunkt, Azimut, richtung, Zenitwinkel, schraegstrecke, orientierung))

            else:
                orientierung = Azimut
                dict_orientierungen[beobachtsgruppeID_aktuell] = orientierung

                richtung = float(self.Richtung(Azimut, orientierung))
                liste_azimut_richtungen.append((beobachtsgruppeID_aktuell, standpunkt, zielpunkt, Azimut, richtung, Zenitwinkel, schraegstrecke, orientierung))

            beobachtsgruppeID_vorher = beobachtsgruppeID_aktuell
        return liste_azimut_richtungen, dict_orientierungen

    def berechne_zenitwinkel_distanz_bodenbezogen(self, zenitwinkel_messung: float, schraegdistanz_messung: float,
                                                  instrumentenhoehe: float, prismenhoehe: float):
        """Berechnet bodenbezogene Schrägdistanz und bodenbezogenen Zenitwinkel.

        Aus gemessener Schrägdistanz und gemessenem Zenitwinkel werden die Horizontalstrecke, der bodenbezogene Höhenunterschied sowie die bodenbezogenen Größen abgeleitet.

        :param zenitwinkel_messung: Gemessener Zenitwinkel in Radiant.
        :type zenitwinkel_messung: float
        :param schraegdistanz_messung: Gemessene Schrägdistanz in Meter.
        :type schraegdistanz_messung: float
        :param instrumentenhoehe: Instrumentenhöhe in Meter.
        :type instrumentenhoehe: float
        :param prismenhoehe: Prismen-/Zielhöhe in Meter.
        :type prismenhoehe: float
        :return: Bodenbezogene Schrägdistanz in Meter und bodenbezogener Zenitwinkel in Radiant.
        :rtype: tuple[float, float]
        """
        HD = np.sin(np.pi - zenitwinkel_messung) * schraegdistanz_messung
        delta_h_ihzh = schraegdistanz_messung * np.cos(zenitwinkel_messung)
        delta_h_boden = delta_h_ihzh + instrumentenhoehe - prismenhoehe
        schraegdistanz_boden = np.sqrt(HD ** 2 + delta_h_boden ** 2)
        zw_boden = np.atan2(HD, delta_h_boden)
        return schraegdistanz_boden, zw_boden


class ENU:
    """Transformationen zwischen XYZ und lokalem ENU-System.

    Die Klasse stellt Methoden zur Verfügung für:

    - Bestimmung eines ENU-Referenzpunkts über den Schwerpunkt gegebener XYZ-Koordinaten,
    - Aufbau der lokalen Rotationsmatrix R0 (XYZ nach ENU) aus geodätischer Breite B und Länge L,
    - Aufbau einer Rotationsmatrix R_ENU für einen gesamten Unbekanntenvektor,
    - Transformation der Kovarianz-Matrix Qxx in das ENU-System,
    - Transformation von Punktkoordinaten (XYZ) in lokale ENU-Koordinaten relativ zum Schwerpunkt.
    """

    @staticmethod
    def berechne_schwerpunkt_fuer_enu(berechnungen, dict_xyz):
        """
        Berechnet die ENU-Referenz (B0, L0) aus dem Schwerpunkt gegebener XYZ-Koordinaten.

        :param berechnungen: Hilfsobjekt mit Methoden zur Berechnung von Breite B und Länge L.
        :type berechnungen: Any
        :param dict_xyz: Dictionary der Koordinaten.
        :type dict_xyz: dict
        :return: Tuple aus geodätischer Breite B0 und Länge L0 des Schwerpunktes.
        :rtype: tuple[float, float]
        """
        XYZ = np.array(list(dict_xyz.values()), dtype=float)
        X0, Y0, Z0 = XYZ.mean(axis=0)
        B0 = float(berechnungen.B(X0, Y0, Z0))
        L0 = float(berechnungen.L(X0, Y0))
        return B0, L0


    @staticmethod
    def berechne_R0_ENU(berechnungen, B, L):
        """
        Erzeugt die 3×3-Rotationsmatrix R0 für die Transformation von XYZ nach ENU.

        :param berechnungen: Hilfsobjekt mit Methoden zur Berechnung der ENU-Komponenten.
        :type berechnungen: Any
        :param B: Geodätische Breite des ENU-Referenzpunkts.
        :type B: float
        :param L: Geodätische Länge des ENU-Referenzpunkts.
        :type L: float
        :return: Rotationsmatrix R0 (3×3) für XYZ nach ENU.
        :rtype: numpy.ndarray
        """
        # East
        r11 = berechnungen.E(L, 1, 0)
        r12 = berechnungen.E(L, 0, 1)
        r13 = berechnungen.E(L, 0, 0)

        # North
        r21 = berechnungen.N(B, L, 1, 0, 0)
        r22 = berechnungen.N(B, L, 0, 1, 0)
        r23 = berechnungen.N(B, L, 0, 0, 1)

        # Up
        r31 = berechnungen.U(B, L, 1, 0, 0)
        r32 = berechnungen.U(B, L, 0, 1, 0)
        r33 = berechnungen.U(B, L, 0, 0, 1)

        R0 = np.array([
            [r11, r12, r13],
            [r21, r22, r23],
            [r31, r32, r33]
        ], dtype=float)

        return R0


    @staticmethod
    def berechne_R_ENU(unbekannten_liste, R0):
        """
        Erstellt eine Rotationsmatrix R für die Umrechnung eines Unbekanntenvektors ins ENU-System.

        Für jede Punkt-ID, die über Symbolnamen (z.B. X1, Y1, Z1) in unbekannten_liste erkannt wird,
        wird die 3×3-Rotation R0 in die passende Position der Gesamtmatrix eingesetzt.

        :param unbekannten_liste: Liste der Unbekannten
        :type unbekannten_liste: list
        :param R0: Rotationsmatrix (3×3) für XYZ nach ENU.
        :type R0: numpy.ndarray
        :return: Rotationsmarix R (n×n) für den gesamten Unbekanntenvektor.
        :rtype: numpy.ndarray
        """

        names = [str(s) for s in unbekannten_liste]
        n = len(names)
        R = np.eye(n, dtype=float)

        punkt_ids = [nm[1:] for nm in names if nm and nm[0].upper() == "X"]

        for pid in punkt_ids:
            try:
                ix = next(i for i, nm in enumerate(names) if nm.upper() == f"X{pid}".upper())
                iy = next(i for i, nm in enumerate(names) if nm.upper() == f"Y{pid}".upper())
                iz = next(i for i, nm in enumerate(names) if nm.upper() == f"Z{pid}".upper())
            except StopIteration:
                continue

            I = [ix, iy, iz]
            R[np.ix_(I, I)] = R0

        return R


    @staticmethod
    def transform_Qxx_zu_QxxENU(Qxx, unbekannten_liste, berechnungen, dict_xyz):
        """
        Transformiert die Kofaktor-Matrix Qxx in das ENU-System.

        Es wird zunächst eine ENU-Referenz über den Schwerpunkt der Koordinaten bestimmt.
        Anschließend wird R0 (XYZ nach ENU) und die Matrix R_ENU aufgebaut. Die Transformation
        erfolgt über:

        Qxx_ENU = R_ENU · Qxx · R_ENUᵀ

        :param Qxx: Kofaktor-Matrix der Unbekannten im XYZ-System.
        :type Qxx: numpy.ndarray
        :param unbekannten_liste: Liste der Unbekannten.
        :type unbekannten_liste: list
        :param berechnungen: Hilfsobjekt für Breite/Länge und ENU-Komponenten.
        :type berechnungen: Any
        :param dict_xyz: Dictionary der Koordinaten.
        :type dict_xyz: dict
        :return: Qxx_ENU, (B0, L0), R0 mit Schwerpunkt-Referenz und lokaler Rotationsmatrix.
        :rtype: tuple[numpy.ndarray, tuple[float, float], numpy.ndarray]
        """

        B0, L0 = ENU.berechne_schwerpunkt_fuer_enu(berechnungen, dict_xyz)
        R0 = ENU.berechne_R0_ENU(berechnungen, B0, L0)
        R_ENU = ENU.berechne_R_ENU(unbekannten_liste, R0)

        Qenu = R_ENU @ Qxx @ R_ENU.T
        Qenu = 0.5 * (Qenu + Qenu.T)

        return Qenu, (B0, L0), R0


    @staticmethod
    def transform_Koord_zu_KoordENU(dict_xyz, R0):
        """
        Transformiert Punktkoordinaten (XYZ) in ENU-Koordinaten relativ zum Schwerpunkt.

        :param dict_xyz: Dictionary der Koordinaten.
        :type dict_xyz: dict
        :param R0: Rotationsmatrix (3×3) für XYZ nach ENU.
        :type R0: numpy.ndarray
        :return: Dictionary der ENU-Koordinaten.
        :rtype: dict
        """

        XYZ = np.asarray(list(dict_xyz.values()), dtype=float).reshape(-1, 3)
        XYZ0 = XYZ.mean(axis=0).reshape(3, )

        Koord_ENU = {}
        for pid, xyz in dict_xyz.items():
            xyz = np.asarray(xyz, dtype=float).reshape(3, )
            enu = (R0 @ (xyz - XYZ0)).reshape(3, )
            Koord_ENU[str(pid)] = (float(enu[0]), float(enu[1]), float(enu[2]))
        return Koord_ENU