/*  Midterm Part 2 Extra Credit

    Let A be a nxn invertible matrix.  Consider the algorithm

        Repeatedly factorize A = QR and replace A with RQ

    described on page 122 of Numerical Algorithms by Justin Solomon.
    Write a program that implements this algorithm using one of the
    QR factorization routines written in class.  Note that since A is
    square, then R and Q are also square.  Under certain conditions the
    diagonal elements of R will converge to the eigenvalues of A up to
    a possible sign difference as the program runs.  Starting with the
    matrix contained in the file extra.c print the diagonal elements of
    R at each iteration to show whether and how they converge.
*/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <strings.h>
#include "matrixlib.h"

double dotprod(int n,double x[n],double y[n]){
    double s=0;
    for(int i=0;i<n;i++){
        s+=x[i]*y[i];
    }
    return s;
}

void hheliminate(int m,int n,double w[m],
    double A[m][n]){
    for(int j=0;j<n;j++){
        double at[m];
        for(int i=0;i<m;i++) at[i]=A[i][j];
        double wdpat=dotprod(m,w,at);
        for(int i=0;i<m;i++) at[i]-=2*w[i]*wdpat;
        for(int i=0;i<m;i++) A[i][j]=at[i];
    }
}
void householder(int m,int n,double A[m][n],
    double R[m][n], double Q[m][n]){
    double Hs[n][m];
    memcpy(R,A,sizeof(double)*m*n);
    for(int j=0;j<n;j++){
        double vtilde[m];
        for(int i=0;i<m;i++){
            if(i<j) vtilde[i]=0;
            else vtilde[i]=R[i][j];
        }
        double c=vecnorm2(m,vtilde);
        if(vtilde[j]<0) vtilde[j]-=c;
        else vtilde[j]+=c;
        double vnorm=vecnorm2(m,vtilde);
        for(int i=0;i<m;i++){
            vtilde[i]/=vnorm;
        }
        hheliminate(m,n,vtilde,R);
 //       printf("R=\n"); matprint(m,n,R);
        memcpy(Hs[j],vtilde,sizeof(double)*m);
    }
    bzero(Q,sizeof(double)*m*n);
    for(int j=0;j<n;j++) Q[j][j]=1;
    for(int j=n-1;j>=0;j--){
        hheliminate(m,n,Hs[j],Q);
    }
    for(int i=0;i<m;i++){
        for(int j=0;j<n;j++){
            if(i>j) R[i][j]=0;
        }
    }
}

void diagprint(int n,double A[n][n]){
	for(int i=0;i<n;i++){
		printf("%g ",A[i][i]);
	}
	printf("\n");
}

#define N 5
#include "extra.c"
double R[N][N],Q[N][N];

int main(){
    printf("Midterm Part 2 Extra Credit.\n");
    printf("N=%d\n",N);
	printf("A=\n"); matprint(N,N,A);
	printf("\n%-12s Diagonal elements of R\n","Iteration");
	for(int p=0;;p++){
		householder(N,N,A,R,Q);
		printf("%-12d ",p);
		diagprint(N,R);
		if(p>=50) break;
		bzero(A,sizeof(double)*N*N);
		for(int i=0;i<N;i++){
			for(int j=0;j<N;j++){
				for(int k=0;k<N;k++){
					A[i][j]+=R[i][k]*Q[k][j];
				}
			}
		}
	}
    return 0;
}