#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"

#define N 5

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;
        }
    }
}
double A[N][N],QT[N][N],R[N][N];
int main(){
    setrlimit(RLIMIT_STACK,
        &(const struct rlimit)
        {RLIM_INFINITY,RLIM_INFINITY});
    printf("N=%d\n",N);
    for(int i=0;i<N;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(N,N,A);
    gramschmidt(N,N,A,QT);
    printf("QT=\n"); matprint(N,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<N;k++){
                R[i][j]+=QT[i][k]*A[k][j];
            }
        }
    }
    printf("R=\n"); matprint(N,N,R);            
    return 0;
}