import math
import numpy as np
from numpy.linalg import solve, norm, inv, qr

def is_upper_triangular(RQ, tolerance):
    for i in range(1, len(RQ)):
        for j in range(i):
            if abs(RQ[i][j]) > tolerance:
                return False
            
    return True

def factorize(A, tolerance):
    RQ = A

    for n in range(101):
        if is_upper_triangular(RQ, tolerance):
            break
            
        Q,R = np.linalg.qr(RQ)
        RQ = R@Q
                
    return n, RQ

def find_eigenvalues(A, tolerance=0.0001):
    size = len(A)
    eigenvalues = np.zeros(size)
    n, RQ = factorize(A, tolerance)
    
    for i in range(size):
        eigenvalues[i] = RQ[i][i]
                       
    return n, RQ, eigenvalues

def find_eigenvector(A, eigenvalue, x_index, normalize, use_qr):
    size = len(A)
    T = A - eigenvalue*np.identity(size)
    b = np.zeros(size+1)
    
    if math.isnan(x_index) or (x_index < 1):
        x_index = 1
        b[0] = 1
    elif x_index > size:
        x_index = size
        b[size] = 1
    else:
        b[x_index-1] = 1
        
    one_row = np.zeros(size)
    one_row[x_index-1] = 1
    
    A = np.append([one_row], T, axis=0)
    
    if use_qr:
        Q, R = qr(A)
        xhat = inv(R)@Q.T@b
    else:
        xhat = inv(A.T@A)@A.T@b  
    
    if normalize:
        return xhat/norm(xhat)
    else:
        return xhat

def find_eigenvectors(A, lambdas, x_index=math.nan, normalize=True, use_qr=False):
    size = len(lambdas)
    V = np.array([]).reshape(-1, size)
    
    for i in range(size):
        eigenvector = find_eigenvector(A, lambdas[i], x_index, normalize, use_qr)
        V = np.append(V, [eigenvector], axis=0)
    
    return V.T