from numpy import *
%matplotlib inline
import matplotlib.pyplot as plt
import pylab as pl

def backsubs(R,b):
    """Solution d'un système linéaire - méthode substitution inverse
    
    Usage:
    x = backsubs(R,b)
    
    Arguments:
    R: matrice carrée mxm triangulaire supérieure inversible
    b: vecteur mx1
    
    Retour:
    x: solution de Rx = b
    """

    m, n = R.shape
    x = zeros((m,1))
    
    for j in range(m-1,-1,-1):
        xr = dot(R[j,(j+1):m],x[(j+1):m])
        x[j] = (b[j]-xr)/R[j,j]

    return x

def clgs(A):
    """factorisation QR Gram-Schmidt classique - instable
    
    Usage:
    Q, R = clgs(A)
    
    Argument:
    A: matrice mxn avec m>=n
    
    Retour:
    Q: matrice Q de la factorisation QR
    R: matrice R de la factorisation QR
    
    Note:
    Cette factorisation n'est pas stable numériquement
    Tire de: LN Trefethen & D Bau III, Numerical Linear Algebra, 1997 SIAM Philadelphia 
    """
    
    m,n = A.shape
    v = zeros((m,n))
    R = zeros((n,n))
    Q = zeros((m,n))

    for j in range(0,n):
        v[:,j] = A[:,j]
        for i in range(0,j):
            R[i,j] = dot( (Q[:,i].conjugate()).T, A[:,j] )
            v[:,j] = v[:,j] - R[i,j]*Q[:,i]
            
        R[j,j] = linalg.norm(v[:,j],2)
        Q[:,j] = v[:,j]/R[j,j]

    return (Q,R)

def mgs(A):
    """factorisation QR Gram-Schmidt modifiée - stable
    
    Usage:
    Q, R = mgs(A)
    
    Argument:
    A: matrice mxn avec m>=n
    
    Retour:
    Q: matrice Q de la factorisation QR
    R: matrice R de la factorisation QR
    
    Note:
    Cette factorisation est stable numériquement
    Tire de: LN Trefethen & D Bau III, Numerical Linear Algebra, 1997 SIAM Philadelphia 
    """
    
    m,n = A.shape
    v = zeros((m,n))
    R = zeros((n,n))
    Q = zeros((m,n))

    for i in range(0,n):
        v[:,i] = A[:,i]

    for i in range(0,n):
        R[i,i] = linalg.norm(v[:,i],2)
        Q[:,i] = v[:,i]/R[i,i]
        for j in range(i,n):
            R[i,j] = dot( (Q[:,i].conjugate()).T, v[:,j])
            v[:,j] = v[:,j] - R[i,j]*Q[:,i]

    return (Q,R)

def  householder(A): # computes a QR factorization upper triangular matrix R 
    """matrices W, R pour la factorisation QR'
    
    Usage: 
    W, R = householder(A)

    Argument:
    A: matrice mxn avec m>=n
    
    Retour:
    W: matrice de vecteurs de reflexions de householder
    R: matrice R de la factorisation QR

    Tire de: LN Trefethen & D Bau III, Numerical Linear Algebra, 1997 SIAM Philadelphia 
    """
    m,n = A.shape
    ident = eye(m)
    W = zeros((m,n))
    R = copy(A)

    for k in range(0,n):
        e1=ident[k:m,k]
        x = R[k:m,k]
        W[k:m,k]= ( int(x[0]>=0) - int(x[0]<0) )*linalg.norm(x,2)*e1+x
        W[k:m,k]=W[k:m,k]/linalg.norm(W[k:m,k],2)
        R[k:m][:,k:n]=R[k:m][:,k:n]-dot(outer(2*W[k:m,k],W[k:m,k]),R[k:m][:,k:n])

    return W, R


def formQ(W): 
    """Matrice Q de la factorisation QR
    
    Usage: 
    Q = formQ(W)
    
    Argument:
    W: matrice mxn de reflexions obtenue de la décomposition Householder
    
    Retour:
    Q: Matrice Q de la factorisation QR
    
    Note:
    La matrice W se calcule avec W, R = householder(A)
    """
    
    m,n = W.shape
    Q=zeros((m,m))

    # compute the product QI = Q, column by column
    for i in range(0,m):
        Q[i,i]=1
        for k in range(n-1,-1,-1): 
            Q[k:m,i]=Q[k:m,i]-2*dot(outer(W[k:m,k],W[k:m,k]),Q[k:m,i])

    return Q

def Qadjb(W,b): 
    """Produit matrice-vecteur Q adjoint fois b
    
    Usage:
    qb = Qadjb(W,b)
    
    Arguments:
    W: matrice mxn de reflexions obtenue de la décomposition Householder
    b: vecteur mx1
    
    Retour:
    Produit Q adjoint par b, où Q est la matrice de la factorisation QR
    
    Note:
    La matrice W se calcule avec W, R = householder(A)
    Tire de: LN Trefethen & D Bau III, Numerical Linear Algebra, 1997 SIAM Philadelphia 
    """

    m,n = W.shape
    qb = copy(b)

    for k in range(0,n):
        qb[k:m]=qb[k:m]-2*dot(outer(W[k:m,k],W[k:m,k]),qb[k:m])

    return qb

A = random.randn(5,5)
print A
b = random.randn(5,1)
print b
B = random.randn(5,3)
print B

W, R = householder(A)
qb = Qadjb(W,b)
x = backsubs(R,qb)
print dot(A,x)
print b

W, R = householder(B)
Q = formQ(W)
print B
print dot(Q,R)

N = 500
sigma2 = 0.1
a = -0.2
b = 0.5
epsilon = sigma2*random.randn(N)
x = random.randn(N)
y = a*x + b + epsilon

pl.plot(x,y,'b.');
pl.xlabel('x');
pl.ylabel('y');

# construction de la matrice A pour la régression linéaire
A = ones((N,2))
A[:,0] = x

# étape 1.
W, R = householder(A)
R_hat = R[0:2][:,0:2]

# étape 2.
qy = Qadjb(W,y)
qy_hat = qy[0:2]

# étape 3.
c = backsubs(R_hat, qy_hat)

print c
print a, b

pl.plot(x,y,'b.');
xx = linspace(min(x),max(x),50)
pl.plot(xx,c[0]*xx + c[1],'k')
pl.xlabel('x');
pl.ylabel('y');