# Kleinste-Quadrate Loesung
# Aufgabe 6.4

import numpy as np

def mysign(x): #Konvention sign(0)=1
    y=np.sign(x)
    if abs(x)<1e-15:
        y=1
    return y

def qr(A):
    n_row, n_col = A.shape
    P = np.eye(n_row,n_row)
    for k in range(n_col):
        T1= A[k:,k]
        alpha = -mysign(T1[0])*np.linalg.norm(T1)
        v=np.array(T1.copy())
        v[0] = v[0]-alpha
        c = 2/np.linalg.norm(v)**2
        P[k:,:] = P[k:,:]-np.outer(c*v,v@P[k:,:])
        A[k:,k:] = A[k:,k:]-np.outer(c*v,v@A[k:,k:])
    return P.transpose(), A

# Loesung durch Rueckwaertseinsetzen
def loese_R_system(R,b):
    n=R.shape[0]
    for j in range(n):
        S=0
        for k in range(n-j,n):
            S+=R[n-j-1,k]*b[k]
        b[n-j-1]=1/R[n-j-1,n-j-1] * (b[n-j-1] - S)
    return b


A = 1.0 * np.array( [ [4,2,1],[9,3,1],[16,4,1],[25,5,1] ] )
b=np.array([90,75,65,40])

Q,R = qr(A)
btilde =Q.transpose()@b
z = loese_R_system(R[0:3,0:3],btilde[0:3])
print(z)