# Blatt 3, Aufgabe 2
# Loese mit Newton x1^3-x2=61 & x2^3-x1=23
# LGS sollen dabei mit Jacobi geloest werden

import numpy as np
def f(x):
    x1 = x[0]
    x2 = x[1]
    a = x1**3-x2-61
    b = x2**3-x1-23
    return np.array([a,b])

def df(x):
    x1 = x[0]
    x2 = x[1]
    a = 3*x1**2
    b = 3*x2**2
    m = np.array([[a, -1], [-1, b]])
    return m

def jacobi (A,b,x = None,max_iter=100):
    n = len(A)
    if x is None:
        x = np.zeros(n)
    D = np.diag(np.diag(A))
    for i in range(n):
        if D[i,i] == 0:
            print("D kann nicht invertiert werden -> anderes Verfahren gewaehlt")
            return np.linalg.solve(A,b)
    invD = np.diag(1/np.diag(D))
    R = A-D
    b1=invD@b
    invDR=invD@R
    for i in range(max_iter):
        x = b1-invDR@x
        if np.linalg.norm(A@x-b)<1e-14:
            print(i+1, "Jacobi-Schritte")
            break
    return x

def diagonalDominance(A):
    n = len(A)
    for i in range(n):
        s = 0
        for j in range(n):
            if i != j:
                s = s + abs(A[i,j])
        if abs(A[i,i]) <= s:
            return False
    return True

    
n=20
startwerte = [np.array([1,1]),np.array([3,3]),np.array([-10,4]),np.array([40,20]),np.array([0,0]),np.array([300,294])]
z = startwerte.copy()

for j in range(len(z)):
    print("++++++++++++++++++++++++++++++++++++++++++++")
    print("Startwert ",startwerte[j])
    fehler = np.linalg.norm(z[j]-[4,3], ord=2)
    for i in range(n):
        if diagonalDominance(df(z[j])) == False:
            print("Jacobi konvergiert unter Umständen nicht -> andres Verfahren gewaehlt")
            fehler_alt = fehler
            z[j] = z[j] - np.linalg.solve(df(z[j]),f(z[j]))
            fehler = np.linalg.norm(z[j]-[4,3], ord=2)
            if fehler_alt != 0:
                quotient = fehler/fehler_alt**2
        else:
            fehler_alt = fehler
            z[j] = z[j] - jacobi((df(z[j])),f(z[j]))
            fehler = np.linalg.norm(z[j]-[4,3], ord=2)
            if fehler_alt != 0:
                quotient = fehler/fehler_alt**2
        print("Iteration ",i+1,"(x,y)=", z[j], "Fehler: ", fehler,"Fehler-Quotient: ", quotient)
        if np.linalg.norm(f(z[j]))<1e-14:
            break