from Datenbank import *
import sympy as sp
import csv


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

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

        liste_beobachtungen_rohdaten = []
        liste_punktnummern =[]

        liste_beobachtungen_jacobian = []
        liste_orientierungsunbekannte = []

        liste_beobachtungsgleichungen_jacobian = []
        liste_beobachtungsgleichungen_abgeleitet = []

        liste_beobachtungen_abgeleitet = []
        liste_zeilenbeschriftungen_jacobian = []

        liste_zeilenbeschriftungen_abgeleitet = []

        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}")
            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 = dict_punkt_symbole[standpunkt]
            X_zp, Y_zp, Z_zp = dict_punkt_symbole[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"SD {beobachtungsgruppeID} {standpunkt}-{zielpunkt}")

            if beobachtungsart == "tachymeter_richtung":
                #for beobachtungenID, beobachtungsgruppeID, standpunkt, zielpunkt in liste_id_standpunkt_zielpunkt:
                B_sp, L_sp = sp.symbols(f"B{standpunkt} L{standpunkt}")
                alpha = sp.symbols(f"alpha{standpunkt}_{zielpunkt}")
                zw = sp.symbols(f"zw{standpunkt}_{zielpunkt}")
                s = sp.symbols(f"s{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"R {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

        # --- Export der A_jacobian-Matrix in eine CSV-Datei ---
        dateiname_export = "Jacobi_Matrix.csv"

        with open(dateiname_export, "w", newline="", encoding="utf-8") as csvfile:
            writer = csv.writer(csvfile, delimiter=";")

            # Kopfzeile: leere Ecke + Namen der Unbekannten
            kopfzeile = ["Beobachtung"]
            for unbekannte in liste_unbekannte:
                kopfzeile.append(str(unbekannte))
            writer.writerow(kopfzeile)

            # Zeilen: Standpunkt-Zielpunkt + Jacobimatrix-Zeile
            for zeilenbeschriftung, zeile in zip(liste_zeilenbeschriftungen_gesamt, A_gesamt.tolist()):
                zeile_als_text = [zeilenbeschriftung] + [str(eintrag) for eintrag in zeile]
                writer.writerow(zeile_als_text)


        return A_gesamt