from Stochastisches_Modell import StochastischesModell
from Netzqualität_Genauigkeit import Genauigkeitsmaße
from Datumsfestlegung import Datumsfestlegung
import sympy as sp
import numpy as np
import Export


def ausgleichung_global(A, dl, Q_ext):
    A=np.asarray(A, float)
    dl = np.asarray(dl, float).reshape(-1, 1)
    Q_ext = np.asarray(Q_ext, float)

    # 1) Gewichtsmatrix P
    P = StochastischesModell.berechne_P(Q_ext)

    # 2) Normalgleichungsmatrix N und Absolutgliedvektor n
    N = A.T @ P @ A
    n = A.T @ P @ dl

    # 3) Zuschlagsvektor dx
    dx = np.linalg.solve(N, n)

    # 4) Residuenvektor v
    v = A @ dx - dl

    # 5) Kofaktormatrix der Unbekannten Q_xx
    Q_xx = StochastischesModell.berechne_Q_xx(N)

    # 6) Kofaktormatrix der Beobachtungen Q_ll_dach
    Q_ll_dach = StochastischesModell.berechne_Q_ll_dach(A, Q_xx)

    # 7) Kofaktormatrix der Verbesserungen Q_vv
    Q_vv = StochastischesModell.berechne_Qvv(Q_ext, Q_ll_dach)

    # 8) Ausgabe
    dict_ausgleichung = {
        "dx": dx,
        "v": v,
        "P": P,
        "N": N,
        "Q_xx": Q_xx,
        "Q_ll_dach": Q_ll_dach,
        "Q_vv": Q_vv,
        "Q_ext": Q_ext,
    }
    return dict_ausgleichung, dx


def ausgleichung_lokal(A, dl, Q_ll):
    A = np.asarray(A, dtype=float)
    dl = np.asarray(dl, dtype=float).reshape(-1, 1)
    Q_ll = np.asarray(Q_ll, dtype=float)

    # 1) Gewichtsmatrix
    P = np.linalg.inv(Q_ll)

    # 2) Normalgleichungen
    N = A.T @ P @ A
    n = A.T @ P @ dl

    # 3) Datumsfestlegung
    G = Datumsfestlegung.raenderungsmatrix_G(x0_sp, liste_punktnummern, mit_massstab=mit_massstab)
    aktive = Datumsfestlegung.datumskomponenten(auswahl, liste_punktnummern)
    E = Datumsfestlegung.auswahlmatrix_E(u=A.shape[1], aktive_unbekannte_indices=aktive)
    Gi_sp = E * G
    Gi = np.asarray(Gi_sp, dtype=float)

    # 3) Zuschlagsvektor dx
    dx = np.linalg.solve(N, n)

    # 5) Residuen
    v = dl - A @ dx

    # 5) Kofaktormatrix der Unbekannten Q_xx
    Q_xx = StochastischesModell.berechne_Q_xx(N)

    # 6) Kofaktormatrix der Beobachtungen Q_ll_dach
    Q_ll_dach = StochastischesModell.berechne_Q_ll_dach(A, Q_xx)

    # 7) Kofaktormatrix der Verbesserungen Q_vv
    Q_vv = StochastischesModell.berechne_Qvv(Q_ll, Q_ll_dach)

    # 8) Ausgabe
    dict_ausgleichung = {
        "dx": dx,
        "v": v,
        "P": P,
        "N": N,
        "Q_xx": Q_xx,
        "Q_ll_dach": Q_ll_dach,
        "Q_vv": Q_vv,
        "Q_ll": Q_ll,
    }
    return dict_ausgleichung, dx