import sympy as sp
from sympy.algebras.quaternion import Quaternion
import Datenbank
from itertools import combinations


class Transformationen:
    def __init__(self, pfad_datenbank):
        self.pfad_datenbank = pfad_datenbank

    @staticmethod
    def R_matrix_aus_euler(e1, e2, e3):
        return sp.Matrix([
            [
                sp.cos(e2) * sp.cos(e3),
                sp.cos(e1) * sp.sin(e3) + sp.sin(e1) * sp.sin(e2) * sp.cos(e3),
                sp.sin(e1) * sp.sin(e3) - sp.cos(e1) * sp.sin(e2) * sp.cos(e3)
            ],
            [
                -sp.cos(e2) * sp.sin(e3),
                sp.cos(e1) * sp.cos(e3) - sp.sin(e1) * sp.sin(e2) * sp.sin(e3),
                sp.sin(e1) * sp.cos(e3) + sp.cos(e1) * sp.sin(e2) * sp.sin(e3)
            ],
            [
                sp.sin(e2),
                -sp.sin(e1) * sp.cos(e2),
                sp.cos(e1) * sp.cos(e2)
            ]
        ])

    def Helmerttransformation_Euler_Transformationsparameter_berechne(self):
        db = Datenbank.Datenbankzugriff(self.pfad_datenbank)
        dict_ausgangssystem = db.get_koordinaten("naeherung_lh", "Dict")
        dict_zielsystem = db.get_koordinaten("naeherung_us", "Dict")

        gemeinsame_punktnummern = sorted(set(dict_ausgangssystem.keys()) & set(dict_zielsystem.keys()))
        anzahl_gemeinsame_punkte = len(gemeinsame_punktnummern)

        liste_punkte_ausgangssystem = [dict_ausgangssystem[i] for i in gemeinsame_punktnummern]
        liste_punkte_zielsystem = [dict_zielsystem[i] for i in gemeinsame_punktnummern]

        print("Anzahl gemeinsame Punkte:", anzahl_gemeinsame_punkte)

        print("\nErste Zielpunkte:")
        for pn, P in list(zip(gemeinsame_punktnummern, liste_punkte_zielsystem))[:5]:
            print(pn, [float(P[0]), float(P[1]), float(P[2])])

        print("\nErste Ausgangspunkte:")
        for pn, p in list(zip(gemeinsame_punktnummern, liste_punkte_ausgangssystem))[:5]:
            print(pn, [float(p[0]), float(p[1]), float(p[2])])

        # --- Näherungswerte (minimal erweitert) ---
        p1, p2, p3 = liste_punkte_ausgangssystem[0], liste_punkte_ausgangssystem[1], liste_punkte_ausgangssystem[2]
        P1, P2, P3 = liste_punkte_zielsystem[0], liste_punkte_zielsystem[1], liste_punkte_zielsystem[2]

        # 1) Näherungswert Maßstab: Mittelwert aus allen Punktpaaren
        ratios = []
        for i, j in combinations(range(anzahl_gemeinsame_punkte), 2):
            dp = (liste_punkte_ausgangssystem[j] - liste_punkte_ausgangssystem[i]).norm()
            dP = (liste_punkte_zielsystem[j] - liste_punkte_zielsystem[i]).norm()
            dp_f = float(dp)
            if dp_f > 0:
                ratios.append(float(dP) / dp_f)

        m0 = sum(ratios) / len(ratios)

        if ratios:
            print("min/mean/max:",
                  min(ratios),
                  sum(ratios) / len(ratios),
                  max(ratios))

        U = (P2 - P1) / (P2 - P1).norm()
        W = (U.cross(P3 - P1)) / (U.cross(P3 - P1)).norm()
        V = W.cross(U)

        u = (p2 - p1) / (p2 - p1).norm()
        w = (u.cross(p3 - p1)) / (u.cross(p3 - p1)).norm()
        v = w.cross(u)

        R0 = sp.Matrix.hstack(U, V, W) * sp.Matrix.hstack(u, v, w).T

        XS = sum(liste_punkte_zielsystem, sp.Matrix([0, 0, 0])) / anzahl_gemeinsame_punkte
        xS = sum(liste_punkte_ausgangssystem, sp.Matrix([0, 0, 0])) / anzahl_gemeinsame_punkte

        Translation0 = XS - m0 * R0 * xS

        # 2) Test auf orthonormale Drehmatrix bei 3 Nachkommastellen!
        if R0.T.applyfunc(lambda x: round(float(x), 3)) == R0.inv().applyfunc(lambda x: round(float(x), 3)) \
                and (R0.T * R0).applyfunc(lambda x: round(float(x), 3)) == sp.eye(3).applyfunc(
            lambda x: round(float(x), 3)) \
                and ((round(R0.det(), 3) == 1.000 or round(R0.det(), 3) == -1.000)):
            print("R ist Orthonormal!")
        else:
            print("R ist nicht Orthonormal!")

        # 3) Euler-Näherungswerte aus R0
        e2_0 = sp.asin(R0[2, 0])
        # Schutz gegen Division durch 0 wenn cos(e2) ~ 0:
        cos_e2_0 = sp.cos(e2_0)

        e1_0 = sp.acos(R0[2, 2] / cos_e2_0)
        e3_0 = sp.acos(R0[0, 0] / cos_e2_0)

        # --- Symbolische Unbekannte (klassische 7 Parameter) ---
        dX, dY, dZ, m, e1, e2, e3 = sp.symbols('dX dY dZ m e1 e2 e3')
        R_symbolisch = self.R_matrix_aus_euler(e1, e2, e3)

        # 4) Funktionales Modell
        f_zeilen = []
        for punkt in liste_punkte_ausgangssystem:
            punkt_vektor = sp.Matrix([punkt[0], punkt[1], punkt[2]])
            f_zeile_i = sp.Matrix([dX, dY, dZ]) + m * R_symbolisch * punkt_vektor
            f_zeilen.extend(list(f_zeile_i))

        f_matrix = sp.Matrix(f_zeilen)
        f = f_matrix

        A_ohne_zahlen = f_matrix.jacobian([dX, dY, dZ, m, e1, e2, e3])

        # Parameterschätzung
        schwellenwert = 1e-4
        anzahl_iterationen = 0
        alle_kleiner_vorherige_iteration = False

        l_vektor = sp.Matrix([koord for P in liste_punkte_zielsystem for koord in P])
        l = l_vektor

        P_mat = sp.eye(3 * anzahl_gemeinsame_punkte)
        l_berechnet_0 = None

        while True:
            if anzahl_iterationen == 0:
                zahlen_0 = {
                    dX: float(Translation0[0]),
                    dY: float(Translation0[1]),
                    dZ: float(Translation0[2]),
                    m: float(m0),
                    e1: float(e1_0),
                    e2: float(e2_0),
                    e3: float(e3_0)
                }

                x0 = sp.Matrix([zahlen_0[dX], zahlen_0[dY], zahlen_0[dZ],
                                zahlen_0[m], zahlen_0[e1], zahlen_0[e2], zahlen_0[e3]])

                l_berechnet_0 = f.subs(zahlen_0).evalf(n=30)
                dl_0 = l_vektor - l_berechnet_0

                A_0 = A_ohne_zahlen.subs(zahlen_0).evalf(n=30)
                N = A_0.T * P_mat * A_0
                n_0 = A_0.T * P_mat * dl_0
                Qxx_0 = N.inv()
                dx = Qxx_0 * n_0
                x = x0 + dx
                x = sp.N(x, 30)

                anzahl_iterationen += 1
                print(f"Iteration Nr.{anzahl_iterationen} abgeschlossen")
                print(dx.evalf(n=3))

            else:
                zahlen_i = {
                    dX: float(x[0]),
                    dY: float(x[1]),
                    dZ: float(x[2]),
                    m: float(x[3]),
                    e1: float(x[4]),
                    e2: float(x[5]),
                    e3: float(x[6])
                }

                l_berechnet_i = f.subs(zahlen_i).evalf(n=30)
                dl_i = l_vektor - l_berechnet_i

                A_i = A_ohne_zahlen.subs(zahlen_i).evalf(n=30)
                N_i = A_i.T * P_mat * A_i
                Qxx_i = N_i.inv()
                n_i = A_i.T * P_mat * dl_i

                dx = Qxx_i * n_i
                x = sp.Matrix(x + dx)

                anzahl_iterationen += 1
                print(f"Iteration Nr.{anzahl_iterationen} abgeschlossen")
                print(dx.evalf(n=3))

            alle_kleiner = True
            for i in range(dx.rows):
                wert = float(dx[i])
                if abs(wert) > schwellenwert:
                    alle_kleiner = False

            if (alle_kleiner and alle_kleiner_vorherige_iteration) or anzahl_iterationen == 100:
                break

            alle_kleiner_vorherige_iteration = alle_kleiner

        print(l.evalf(n=3))
        print(l_berechnet_0.evalf(n=3))
        print(f"x = {x.evalf(n=3)}")

        # --- Neuberechnung Zielsystem ---
        zahlen_final = {
            dX: float(x[0]),
            dY: float(x[1]),
            dZ: float(x[2]),
            m: float(x[3]),
            e1: float(x[4]),
            e2: float(x[5]),
            e3: float(x[6])
        }

        l_berechnet_final = f.subs(zahlen_final).evalf(n=30)

        liste_l_berechnet_final = []
        for i in range(anzahl_gemeinsame_punkte):
            Xi = l_berechnet_final[3 * i + 0]
            Yi = l_berechnet_final[3 * i + 1]
            Zi = l_berechnet_final[3 * i + 2]
            liste_l_berechnet_final.append(sp.Matrix([Xi, Yi, Zi]))

        print("")
        print("l_berechnet_final:")
        for punktnummer, l_fin in zip(gemeinsame_punktnummern, liste_l_berechnet_final):
            print(f"{punktnummer}: {float(l_fin[0]):.3f}, {float(l_fin[1]):.3f}, {float(l_fin[2]):.3f}")

        print("Streckendifferenzen:")
        streckendifferenzen = [
            (punkt_zielsys - l_final).norm()
            for punkt_zielsys, l_final in zip(liste_punkte_zielsystem, liste_l_berechnet_final)
        ]
        print([round(float(s), 6) for s in streckendifferenzen])

        Schwerpunkt_Zielsystem = sum(liste_punkte_zielsystem, sp.Matrix([0, 0, 0])) / anzahl_gemeinsame_punkte
        Schwerpunkt_berechnet = sum(liste_l_berechnet_final, sp.Matrix([0, 0, 0])) / anzahl_gemeinsame_punkte

        Schwerpunktsdifferenz = Schwerpunkt_Zielsystem - Schwerpunkt_berechnet

        print("\nDifferenz Schwerpunkt (Vektor):")
        print(Schwerpunktsdifferenz.evalf(3))

        print("Betrag der Schwerpunkt-Differenz:")
        print(f"{float(Schwerpunktsdifferenz.norm()):.3f}m")

        return zahlen_final

    def Helmerttransformation(self, transformationsparameter):
        db = Datenbank.Datenbankzugriff(self.pfad_datenbank)
        dict_ausgangssystem = db.get_koordinaten("naeherung_lh", "Dict")
        dict_zielsystem = db.get_koordinaten("naeherung_us", "Dict")

        dX, dY, dZ, m, e1, e2, e3 = sp.symbols('dX dY dZ m e1 e2 e3')

        unterschiedliche_punktnummern = sorted(set(dict_ausgangssystem.keys()) ^ set(dict_zielsystem.keys()))
        punktnummern_transformieren = [
            punktnummer for punktnummer in unterschiedliche_punktnummern if punktnummer in dict_ausgangssystem
        ]
        liste_punkte_ausgangssystem = [dict_ausgangssystem[punktnummer] for punktnummer in punktnummern_transformieren]

        R = self.R_matrix_aus_euler(transformationsparameter[e1], transformationsparameter[e2], transformationsparameter[e3])

        f_zeilen = []
        for punkt in liste_punkte_ausgangssystem:
            punkt_vektor = sp.Matrix([punkt[0], punkt[1], punkt[2]])
            f_zeile_i = sp.Matrix([transformationsparameter[dX], transformationsparameter[dY], transformationsparameter[dZ]]) + transformationsparameter[m] * R * punkt_vektor
            f_zeilen.extend(list(f_zeile_i))

        f_matrix = sp.Matrix(f_zeilen)
        dict_transformiert = {}
        for i, pn in enumerate(punktnummern_transformieren):
            Xi = f_matrix[3 * i + 0]
            Yi = f_matrix[3 * i + 1]
            Zi = f_matrix[3 * i + 2]

            dict_transformiert[str(pn)] = sp.Matrix([
                [float(Xi)],
                [float(Yi)],
                [float(Zi)]
            ])
        return dict_transformiert