# -*- coding: utf-8 -*-
"""
Created on Sat Nov  7 13:26:09 2020

Höhere Mathematik 1, Serie 8, Gerüst für Aufgabe 2

Description: calculates the QR factorization of A so that A = QR
Input Parameters: A: array, n*n matrix
Output Parameters: Q : n*n orthogonal matrix
                   R : n*n upper right triangular matrix            
Remarks: none
Example: A = np.array([[1,2,-1],[4,-2,6],[3,1,0]]) 
        [Q,R]=Serie8_Aufg2(A)

@author: knaa
"""
import numpy as np

def Serie8_Aufg2(A):
    unknown = 0

    
    A = np.copy(A)                       #necessary to prevent changes in the original matrix A_in
    A = A.astype('float64')              #change to float
    
    n = np.shape(A)[0]
    
    if n != np.shape(A)[1]:
        raise Exception('Matrix is not square') 
    
    Q = np.eye(n)
    R = A

    for j in np.arange(0,n-1):
        # erzeuge Nullen in R in der j-ten Spalte unterhalb der Diagonalen:
        #a = (Q @ A)[j:,j:][:,0]
        a = np.copy((Q @ A)[j:,j:][:,0]).reshape(n-j,1)
        e = np.eye(n-j)[:,0].reshape(n-j,1)
        length_a = np.linalg.norm(a)
        if a[0] >= 0: sig = 1
        else: sig = -1
        v = a + sig * length_a * e          # vj := aj + sign(a1j) · |aj| · ej
        u = 1 / np.linalg.norm(v) * v       # uj := 1/|vj|*vj
        ut = u.T
        ua = u @ ut
        ub = 2 * ua
        x = np.eye(n)
        H = np.eye(n-j) - (2 * (u @ u.T))   # Hj := In − 2u1u1T bestimme die (n − j + 1) × (n − j + 1) Householder-Matrix Hj
        Qj = np.eye(n)
        Qj[j:,j:] = H                       # erweitere Hi durch einen Ii−1 Block links oben zur n × n Matrix Qi
        R = Qj @ R                          # R := Qj · R
        Q = Q @ Qj.T                        # Q := Q · QjT
        
    return(Q,R)


if __name__ == '__main__':
    # Beispiel
    # A = np.array([[1, 2, -1], [4, -2, 6], [3, 1, 0]])
    # b = np.array([
    #     [9],
    #     [-4],
    #     [9]
    # ])

    # Example from Task 1
    A = np.array([
        [1, -2, 3],
        [-5, 4, 1],
        [2, -1, 3]
    ])
    b = np.array([
        [1],
        [9],
        [5]
    ])

    [Q,R]=Serie8_Aufg2(A)

    n = len(b) - 1
    QTb = Q.T @ b
    result = [0 for i in range(n+1)]
    row = n
    while row >= 0:
        value = QTb[row][0]
        column = n
        while column > row:
            value -= R[row][column] * result[column]
            column -= 1
        value = value / R[row,row]
        result[row] = value
        row -= 1

    print("\nQ:\n", Q)
    print("\nR:\n", R)
    print("\Result:\n", result)