#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <sys/time.h>
#include <sys/resource.h>
#include <string.h>
#include <strings.h>
#include <cilk/cilk.h>
#include <lapacke.h>
#include "matrixlib.h"
#include "tictoc.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 gramschmidt(int m,int n,double A[m][n],
    double QT[n][m]){
    double vtilde[m];
    for(int j=0;j<n;j++){
        for(int i=0;i<m;i++){
            vtilde[i]=A[i][j];
        }
        for(int k=0;k<j;k++){
            double adotv=dotprod(m,vtilde,QT[k]);
            for(int l=0;l<m;l++){
                vtilde[l]-=adotv*QT[k][l];
            }
        }
        double vnorm=vecnorm2(m,vtilde);
        for(int i=0;i<m;i++){
            QT[j][i]=vtilde[i]/vnorm;
        }
    }
}

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;
        }
    }
}

#define N 4
#define M 6
double A[M][N],QT[N][M],R[M][N],
    QR[M][N],Q[M][N],B[N][N];
int main(){
    setrlimit(RLIMIT_STACK,
        &(const struct rlimit)
        {RLIM_INFINITY,RLIM_INFINITY});
    printf("M,N=%d,%d\n",M,N);
    for(int i=0;i<M;i++){
        for(int j=0;j<N;j++){
            A[i][j]=2.0*random()/RAND_MAX-1.0;
//            A[i][j]=1.0/(i+j+1);
        }
    }
    printf("A=\n"); matprint(M,N,A);
    householder(M,N,A,R,Q);
    printf("Q=\n"); matprint(M,N,Q);
    printf("R=\n"); matprint(N,N,R);
    bzero(QR,sizeof(double)*M*N);
    for(int i=0;i<M;i++){
        for(int j=0;j<N;j++){
            for(int k=0;k<N;k++){
                QR[i][j]+=Q[i][k]*R[k][j];    
            }
        }
    }
    printf("QR=\n"); matprint(M,N,QR);
    bzero(B,sizeof(double)*N*N);
    for(int i=0;i<N;i++){
        for(int j=0;j<N;j++){
            for(int k=0;k<M;k++){
                B[i][j]+=Q[k][i]*Q[k][j];    
            }
        }
    }
    printf("B=\n"); matprint(N,N,B);         
    return 0;
    gramschmidt(M,N,A,QT);
    printf("QT=\n"); matprint(M,N,QT);
    bzero(R,sizeof(double)*N*N);
    for(int i=0;i<N;i++){
        for(int j=0;j<N;j++){
            for(int k=0;k<M;k++){
                R[i][j]+=QT[i][k]*A[k][j];
            }
        }
    }
    printf("R=\n"); matprint(N,N,R);
    bzero(QR,sizeof(double)*M*N);
    for(int i=0;i<M;i++){
        for(int j=0;j<N;j++){
            for(int k=0;k<N;k++){
                QR[i][j]+=QT[k][i]*R[k][j];    
            }
        }
    }
    printf("QR=\n"); matprint(M,N,QR);           
    return 0;
}