import numpy as np

def atpv_probe(A, P, v, tol=1e-7):
    """
    Führt die ATPv-Probe zur Kontrolle der Lösung des Normalgleichungssystems durch.

    Die Funktion überprüft, ob der Ausdruck Aᵀ · P · v näherungsweise Null ist.
    Die Prüfung erfolgt unter Verwendung einer vorgegebenen Toleranz.

    :param A: Jacobi-Matrix (A-Matrix).
    :type A: np.asarray
    :param P: Gewichtsmatrix der Beobachtungen.
    :type P: np.asarray
    :param v: Residuenvektor der Beobachtungen.
    :type v: np.asarray
    :param tol: Absolute Toleranz für den Vergleich mit Null.
    :type tol: float
    :return: None
    :rtype: None
    """
    A = np.asarray(A, float)
    P = np.asarray(P, float)
    v = np.asarray(v, float).reshape(-1, 1)

    ATPV = A.T @ P @ v

    if np.allclose(ATPV, 0, atol=tol):
        print("✅ ATPv-Probe erfolgreich")
    else:
        print("❌ ATPv-Probe nicht erfolgreich. Fehler bei der Lösung des Normalgleichungssystems")


def hauptprobe(A, x, l, v, tol=1e-7):
    """
    Führt die Hauptprobe zur Überprüfung der berechneten Residuen durch.

    Die Hauptprobe kontrolliert, ob die Residuen v mit der Beziehung
    v = A · x − l übereinstimmen. Stimmen der berechnete Residuenvektor
    und der über das funktionale Modell rekonstruierte Residuenvektor
    innerhalb der Toleranz überein, gilt die Ausgleichung als konsistent.

    :param A: Jacobi-Matrix (A-Matrix).
    :type A: np.asarray
    :param x: Lösungsvektor der Unbekannten.
    :type x: np.asarray
    :param l: Beobachtungsvektor.
    :type l: np.asarray
    :param v: Residuenvektor aus der Ausgleichung.
    :type v: np.asarray
    :param tol: Absolute Toleranz für den Vergleich der Residuen.
    :type tol: float
    :return: None
    :rtype: None
    """
    A = np.asarray(A, float)
    x = np.asarray(x, float).reshape(-1, 1)
    l = np.asarray(l, float).reshape(-1, 1)
    v = np.asarray(v, float).reshape(-1, 1)

    v_test = A @ x - l

    if np.allclose(v, v_test, atol=tol):
        print("✅ Hauptprobe erfolgreich")
    else:
        diff = v - v_test
        print("❌ Hauptprobe nicht erfolgreich. Abweichung zu v: ", diff)