import numpy as np

### Testdaten ##################################################################

B = np.array([[ 2.,   4., -9. ],
              [-2., -10.,  3. ],
	      [ 1.,  -1.,  6. ]])

A2 = np.array([ [ 2., 1., 1., 0. ], [ 4., 3., 3., 1. ], [ 8., 7., 9., 5. ], [ 6., 7., 9., 8. ]])
b2 = np.array([ 13., 32., 76., 71. ])

### Funktionen von frueheren Uebungsblaettern ##################################

def backwardSubstitution(R, y):
    n = len(R)
    x = np.zeros(n)
    x[n-1] = y[n-1]/R[n-1][n-1]
    for i in range(n-2, -1, -1):   # x_i = (y_i - sum_{j=i+1}^n r_{i,j} x_j) / r_{i,i}
        x[i] = y[i]
        for j in range(i+1,n):
            x[i] = x[i] - R[i][j]*x[j]
        x[i] = x[i] / R[i][i]
    return x

### Aufgabe 3 ##################################################################

def gramSchmidt(A):
    m, n = A.shape			# Matrixinitialisierungen mit den
    q = np.zeros((m,n))			# korrekten Groessen fuer eine
    r = np.zeros((n,n))			# reduzierte QR-Zerlegung
    a0 = A[:,0]				# erste Spalte von A
    r[0][0] = np.linalg.norm(a0)	# r_1,1 := ||a^(1)||
    q[:,0] = a0 / r[0][0]		# q^(1) := 1/r_1,1 * a^(1)
    for j in range(1,m):
        aj = A[:,j]
        qj = aj.copy()			# q^(j) := a^(j)
        for i in range(j):
            qi = q[:,i]
            r[i][j] = np.dot(qi, aj)	# r_i,j := <q^(i),a^(j)>
            qj = qj - r[i][j] * qi	# q^(j) := q^(j) - r_i,j*q^(i)
        r[j][j] = np.linalg.norm(qj)	# r_j,j := ||q^(j)||
        qj = qj / r[j][j]		# q^(j) := 1/r_j,j * q^(j)
        q[:,j] = qj
    return q, r


def solveLESwithQR(A, b):
    Q, R = gramSchmidt(A)		# A = Q R
    c = np.dot(np.transpose(Q), b)	# c = Q^T b
    x = backwardSubstitution(R, c)	# loese Rx = Q^T b
    return x

# Test QR-Zerlegung
Q, R = gramSchmidt(B)
print(Q)
print(R)
print(np.dot(Q,R))

# Test LES Solver
x = solveLESwithQR(A2, b2)
print(x)