from Datenbank import *
import sympy as sp
from Export import Export
from Berechnungen import Berechnungen


class FunktionalesModell:
    def __init__(self, pfad_datenbank, a, b):
        self.pfad_datenbank = pfad_datenbank
        self.berechnungen = Berechnungen(a, b)
        self.substitutionen_dict = self.dict_substitutionen_uebergeordnetes_system()
        self.dict_punkt_symbole = {}

    def jacobi_matrix_symbolisch(self):
        #liste_beobachtungsarten = ["tachymeter_distanz", "tachymeter_richtung", "tachymeter_zenitwinkel"]
        liste_beobachtungsarten = ["tachymeter_distanz"]
        db_zugriff = Datenbankzugriff(self.pfad_datenbank)

        liste_beobachtungen_rohdaten = []
        liste_punktnummern =[]

        liste_orientierungsunbekannte = []

        for beobachtungsart in liste_beobachtungsarten:
            liste_id_standpunkt_zielpunkt = db_zugriff.get_beobachtungen_id_beobachtungsgruppe_standpunkt_zielpunkt(beobachtungsart)

            for beobachtungenID, beobachtungsgruppeID, standpunkt, zielpunkt in liste_id_standpunkt_zielpunkt:
                liste_beobachtungen_rohdaten.append(
                    (beobachtungsart, beobachtungenID, beobachtungsgruppeID, standpunkt, zielpunkt)
                )

                if standpunkt not in liste_punktnummern:
                    liste_punktnummern.append(standpunkt)
                if zielpunkt not in liste_punktnummern:
                    liste_punktnummern.append(zielpunkt)

                if beobachtungsart == "tachymeter_richtung":
                    if beobachtungsgruppeID not in liste_orientierungsunbekannte:
                        liste_orientierungsunbekannte.append(beobachtungsgruppeID)
        if liste_beobachtungen_rohdaten == []:
            return None

        #dict_punkt_symbole = {}
        liste_unbekannte = []

        for punkt in liste_punktnummern:
            X, Y, Z = sp.symbols(f"X{punkt} Y{punkt} Z{punkt}")
            self.dict_punkt_symbole[punkt] = (X, Y, Z)
            liste_unbekannte.append(X)
            liste_unbekannte.append(Y)
            liste_unbekannte.append(Z)

        dict_orientierung_symbole = {}
        for orientierungsunbekannte in liste_orientierungsunbekannte:
            O = sp.symbols(f"O{orientierungsunbekannte}")
            dict_orientierung_symbole[orientierungsunbekannte] = O
            liste_unbekannte.append(O)

        liste_beobachtungsgleichungen_distanz =[]
        liste_zeilenbeschriftungen_distanz = []

        liste_A_richtung_zeilen = []
        liste_zeilenbeschriftungen_richtung = []

        for beobachtungsart, beobachtungenID, beobachtungsgruppeID, standpunkt, zielpunkt in liste_beobachtungen_rohdaten:
            X_sp, Y_sp, Z_sp = self.dict_punkt_symbole[standpunkt]
            X_zp, Y_zp, Z_zp = self.dict_punkt_symbole[zielpunkt]
            B_sp, L_sp = sp.symbols(f"B{standpunkt} L{standpunkt}")
            alpha = sp.symbols(f"{beobachtungenID}_R_{beobachtungsgruppeID}_{standpunkt}_{zielpunkt}")
            zw = sp.symbols(f"{beobachtungenID}_ZW_{beobachtungsgruppeID}_{standpunkt}_{zielpunkt}")
            s = sp.symbols(f"{beobachtungenID}_SD_{beobachtungsgruppeID}_{standpunkt}_{zielpunkt}")

            if beobachtungsart == "tachymeter_distanz":
                beobachtungsgleichung = sp.sqrt(
                    (X_zp - X_sp) ** 2
                    + (Y_zp - Y_sp) ** 2
                    + (Z_zp - Z_sp) ** 2
                )
                liste_beobachtungsgleichungen_distanz.append(beobachtungsgleichung)
                liste_zeilenbeschriftungen_distanz.append(f"{beobachtungenID}_SD_{beobachtungsgruppeID}_{standpunkt}_{zielpunkt}")

            if beobachtungsart == "tachymeter_richtung":
                #for beobachtungenID, beobachtungsgruppeID, standpunkt, zielpunkt in liste_id_standpunkt_zielpunkt:
                d_r_dX_zp = ((sp.sin(B_sp)*sp.cos(L_sp)*sp.sin(alpha) - sp.sin(L_sp)*sp.cos(alpha)) / (s * sp.sin(zw)))
                d_r_dX_sp = - d_r_dX_zp
                d_r_dY_zp = ((sp.sin(B_sp)*sp.sin(L_sp)*sp.sin(alpha) + sp.cos(L_sp)*sp.cos(alpha)) / (s * sp.sin(zw)))
                d_r_dY_sp = - d_r_dY_zp
                d_r_dZ_zp = ((-sp.cos(B_sp) * sp.sin(alpha) / (s * sp.sin(zw))))
                d_r_dZ_sp = - d_r_dZ_zp
                d_r_dO_sp = -1

                zeile_A_Matrix = []
                for punkt in liste_punktnummern:
                    if punkt == standpunkt:
                        zeile_A_Matrix.extend([d_r_dX_sp, d_r_dY_sp, d_r_dZ_sp])
                    elif punkt == zielpunkt:
                        zeile_A_Matrix.extend([d_r_dX_zp, d_r_dY_zp, d_r_dZ_zp])
                    else:
                        zeile_A_Matrix.extend([0, 0, 0])

                for orientierung in liste_orientierungsunbekannte:
                    if orientierung == beobachtungsgruppeID:
                        zeile_A_Matrix.append(d_r_dO_sp)
                    else:
                        zeile_A_Matrix.append(0)

                liste_A_richtung_zeilen.append(zeile_A_Matrix)
                liste_zeilenbeschriftungen_richtung.append(
                f"{beobachtungenID}_R_{beobachtungsgruppeID}_{standpunkt}_{zielpunkt}"
            )

            if beobachtungsart == "tachymeter_zenitwinkel":
                d_r_dX_zp = ((X_zp - X_sp) * sp.cos(zw) - s * sp.cos(B_sp) * sp.cos(L_sp)) / (s ** 2 * sp.sin(zw))
                d_r_dX_sp = - d_r_dX_zp
                d_r_dY_zp = ((Y_zp - Y_sp) * sp.cos(zw) - s * sp.cos(B_sp) * sp.sin(L_sp)) / (s ** 2 * sp.sin(zw))
                d_r_dY_sp = - d_r_dY_zp
                d_r_dZ_zp = ((Z_zp - Z_sp) * sp.cos(zw) - s * sp.sin(B_sp)) / (s ** 2 * sp.sin(zw))
                d_r_dZ_sp = - d_r_dZ_zp

                zeile_A_Matrix = []
                for punkt in liste_punktnummern:
                    if punkt == standpunkt:
                        zeile_A_Matrix.extend([d_r_dX_sp, d_r_dY_sp, d_r_dZ_sp])
                    elif punkt == zielpunkt:
                        zeile_A_Matrix.extend([d_r_dX_zp, d_r_dY_zp, d_r_dZ_zp])
                    else:
                        zeile_A_Matrix.extend([0, 0, 0])

                for orientierung in liste_orientierungsunbekannte:
                    zeile_A_Matrix.append(0)

                liste_A_richtung_zeilen.append(zeile_A_Matrix)
                liste_zeilenbeschriftungen_richtung.append(
                    f"{beobachtungenID}_ZW_{beobachtungsgruppeID}_{standpunkt}_{zielpunkt}"
                )

        if liste_beobachtungsgleichungen_distanz:
            f_matrix_dist = sp.Matrix(liste_beobachtungsgleichungen_distanz)
            unbekanntenvektor = sp.Matrix(liste_unbekannte)
            A_dist = f_matrix_dist.jacobian(unbekanntenvektor)
        else:
            A_dist = None

        if liste_A_richtung_zeilen:
            A_richtung = sp.Matrix(liste_A_richtung_zeilen)
        else:
            A_richtung = None

        if A_dist is not None and A_richtung is not None:
            A_gesamt = A_dist.col_join(A_richtung)
            liste_zeilenbeschriftungen_gesamt = (
                    liste_zeilenbeschriftungen_distanz + liste_zeilenbeschriftungen_richtung
            )
        elif A_dist is not None:
            A_gesamt = A_dist
            liste_zeilenbeschriftungen_gesamt = liste_zeilenbeschriftungen_distanz
        elif A_richtung is not None:
            A_gesamt = A_richtung
            liste_zeilenbeschriftungen_gesamt = liste_zeilenbeschriftungen_richtung
        else:
            return None

        self.liste_unbekanntenvektor_symbolisch = liste_unbekannte

        Export.matrix_to_csv(r"Zwischenergebnisse\Jacobi_Matrix_Symbolisch.csv", liste_unbekannte,
                             liste_zeilenbeschriftungen_gesamt, A_gesamt, "Beobachtung")
        return A_gesamt, liste_unbekannte, liste_zeilenbeschriftungen_gesamt

    def jacobi_matrix_zahlen_iteration_0(self, A_symbolisch, koordinatenart, liste_unbekannte = None, liste_zeilenbeschriftungen_gesamt = None):

        if koordinatenart == "naeherung_us":
            A_numerisch = A_symbolisch.xreplace(self.substitutionen_dict)

            Export.matrix_to_csv(r"Zwischenergebnisse\Jacobi_Matrix_Numerisch_Iteration0.csv", liste_unbekannte,
                                 liste_zeilenbeschriftungen_gesamt, A_numerisch, "Beobachtung")

            return A_numerisch
        else:
            print("Koordinaten noch nicht implementiert!")

    def beobachtungsvektor_numerisch(self, liste_beobachtungsvektor_symbolisch):
        liste_beobachtungsvektor_numerisch = []
        for beobachtung_symbolisch in liste_beobachtungsvektor_symbolisch:
            liste_beobachtungsvektor_numerisch.append(self.substitutionen_dict[sp.Symbol(beobachtung_symbolisch)])

        beobachtungsvektor_numerisch = sp.Matrix(liste_beobachtungsvektor_numerisch)
        Export.matrix_to_csv(r"Zwischenergebnisse\Beobachtungsvektor_Numerisch.csv", [""], liste_beobachtungsvektor_symbolisch, beobachtungsvektor_numerisch, "Beobachtungsvektor")
        return beobachtungsvektor_numerisch

    def beobachtungsvektor_naeherung_symbolisch(self, liste_beobachtungsvektor_symbolisch):
        liste_beobachtungsgleichungen = []
        self.dict_punkt_symbole = {}
        liste_punktnummern = []

        for beobachtung_symbolisch in liste_beobachtungsvektor_symbolisch:
            aufgeteilt = beobachtung_symbolisch.split("_")
            standpunkt = str(aufgeteilt[3])
            zielpunkt = str(aufgeteilt[4])

            if standpunkt not in liste_punktnummern:
                liste_punktnummern.append(standpunkt)
            if zielpunkt not in liste_punktnummern:
                liste_punktnummern.append(zielpunkt)

        for punkt in liste_punktnummern:
            X, Y, Z = sp.symbols(f"X{punkt} Y{punkt} Z{punkt}")
            self.dict_punkt_symbole[str(punkt)] = (X, Y, Z)

        for beobachtung_symbolisch in liste_beobachtungsvektor_symbolisch:
            aufgeteilt = beobachtung_symbolisch.split("_")
            #beobachtungen_ID = aufgeteilt[0]
            beobachtungsart = aufgeteilt[1]  # "SD", "R", "ZW"
            #beobachtungsgruppeID = aufgeteilt[2]
            standpunkt = str(aufgeteilt[3])
            zielpunkt = str(aufgeteilt[4])

            if beobachtungsart == "SD":
                X_sp, Y_sp, Z_sp = self.dict_punkt_symbole[standpunkt]
                X_zp, Y_zp, Z_zp = self.dict_punkt_symbole[zielpunkt]

                beobachtungsgleichung = sp.sqrt(
                    (X_zp - X_sp) ** 2
                    + (Y_zp - Y_sp) ** 2
                    + (Z_zp - Z_sp) ** 2
                )
                liste_beobachtungsgleichungen.append(beobachtungsgleichung)

        beobachtungsvektor_naeherung_symbolisch = sp.Matrix(liste_beobachtungsgleichungen)
        Export.matrix_to_csv(r"Zwischenergebnisse\Beobachtungsvektor_Näherung_Symbolisch.csv", [""],
                             liste_beobachtungsvektor_symbolisch, beobachtungsvektor_naeherung_symbolisch, "Beobachtungsvektor")

        return beobachtungsvektor_naeherung_symbolisch

    def beobachtungsvektor_naeherung_numerisch_iteration0(self, liste_beobachtungsvektor_symbolisch, beobachtungsvektor_naeherung_symbolisch):
        beobachtungsvektor_naeherung_numerisch_iteration0 = beobachtungsvektor_naeherung_symbolisch.xreplace(self.substitutionen_dict)
        Export.matrix_to_csv(r"Zwischenergebnisse\Beobachtungsvektor_Näherung_Numerisch_Iteration0.csv", [""],
                             liste_beobachtungsvektor_symbolisch, beobachtungsvektor_naeherung_numerisch_iteration0,
                             "Beobachtungsvektor")

        return beobachtungsvektor_naeherung_numerisch_iteration0

    def unbekanntenvektor_symbolisch(self, liste_unbekannte):
        unbekanntenvektor_symbolisch = sp.Matrix(liste_unbekannte)
        Export.matrix_to_csv(r"Zwischenergebnisse\Unbekanntenvektor_Symbolisch.csv", [""], liste_unbekannte, unbekanntenvektor_symbolisch,
                             "Unbekanntenvektor")
        return(unbekanntenvektor_symbolisch)

    def unbekanntenvektor_numerisch(self, liste_unbekanntenvektor_symbolisch, unbekanntenvektor_symbolisch, dX_Vektor = None, unbekanntenvektor_neumerisch_vorherige_Iteration = None):
        if not hasattr(self, "liste_unbekanntenvektor_symbolisch"):
            self.liste_unbekanntenvektor_symbolisch = liste_unbekanntenvektor_symbolisch

        if dX_Vektor is None and unbekanntenvektor_neumerisch_vorherige_Iteration is None:
            unbekanntenvektor_numerisch = unbekanntenvektor_symbolisch.xreplace(self.substitutionen_dict)
        else:
            unbekanntenvektor_numerisch = unbekanntenvektor_neumerisch_vorherige_Iteration + dX_Vektor

        self.substitutionen_dict = self.dict_substitutionen_uebergeordnetes_system(unbekanntenvektor_numerisch)

        Export.matrix_to_csv(r"Zwischenergebnisse\Unbekanntenvektor_Numerisch_Iteration0.csv", [""],
                             liste_unbekanntenvektor_symbolisch, unbekanntenvektor_numerisch,
                             "Unbekanntenvektor")
        return unbekanntenvektor_numerisch

    def unbekanntenvektor_numerisch_to_dict_unbekanntenvektor(self, liste_unbekanntenvektor_symbolisch, unbekanntenvektor_numerisch):
        dict_unbekanntenvektor_numerisch = {}
        index = 0

        for symbol in liste_unbekanntenvektor_symbolisch:
            name = str(symbol)
            if not name.startswith("X"):
                continue

            punktnummer = str(name[1:])

            dict_unbekanntenvektor_numerisch[punktnummer] = sp.Matrix([
                unbekanntenvektor_numerisch[index],
                unbekanntenvektor_numerisch[index + 1],
                unbekanntenvektor_numerisch[index + 2]
            ])
            index += 3
        return dict_unbekanntenvektor_numerisch



    def berechnung_dl(self, beobachtungsvektor_numerisch, beobachtungsvektor_naeherung_numerisch):
        dl = beobachtungsvektor_numerisch - beobachtungsvektor_naeherung_numerisch
        return dl

    def dict_substitutionen_uebergeordnetes_system(self, unbekanntenvektor_aus_iteration = None):
        db_zugriff = Datenbankzugriff(self.pfad_datenbank)
        if unbekanntenvektor_aus_iteration is None:
            dict_koordinaten = db_zugriff.get_koordinaten("naeherung_us")
        else:
            dict_koordinaten = self.unbekanntenvektor_numerisch_to_dict_unbekanntenvektor(
                self.liste_unbekanntenvektor_symbolisch,
                unbekanntenvektor_aus_iteration
            )

        dict_koordinaten_B_L = self.berechnungen.geometrische_breite_laenge(dict_koordinaten)
        liste_beobachtungen = db_zugriff.get_beobachtungen_from_beobachtungenid()
        substitutionen = {}

        for punktnummer, vektor in dict_koordinaten_B_L.items():
            X_sym, Y_sym, Z_sym, B_sym, L_Sym = sp.symbols(
                f"X{punktnummer} Y{punktnummer} Z{punktnummer} B{punktnummer} L{punktnummer}")

            substitutionen[X_sym] = vektor[0][0]
            substitutionen[Y_sym] = vektor[0][1]
            substitutionen[Z_sym] = vektor[0][2]
            substitutionen[B_sym] = vektor[1]
            substitutionen[L_Sym] = vektor[2]

        for standpunkt, zielpunkt, beobachtungenID, beobachtungsgruppeID, tachymeter_richtung, tachymeter_zenitwinkel, tachymeter_distanz in liste_beobachtungen:
            alpha = sp.symbols(f"{beobachtungenID}_R_{beobachtungsgruppeID}_{standpunkt}_{zielpunkt}")
            zw = sp.symbols(f"{beobachtungenID}_ZW_{beobachtungsgruppeID}_{standpunkt}_{zielpunkt}")
            s = sp.symbols(f"{beobachtungenID}_SD_{beobachtungsgruppeID}_{standpunkt}_{zielpunkt}")

            substitutionen[alpha] = tachymeter_richtung
            substitutionen[zw] = tachymeter_zenitwinkel
            substitutionen[s] = tachymeter_distanz

        return substitutionen