#include <omp.h>
#include <cstdlib>
#include <cstdio>
#include <cmath>
#include "ResourceManager.h"
#include "FullMatrix.h"

void BACKSB(int n, FullMatrix &a, double *b)
{
  int i,ii=-1,ip,j;
  double sum;
  
  for(ip=0; ip< n; ip++){
    if(ii != -1)
      for(j=ii; j <= i-1; j++) sum -= a(i,j)*b[j];
    else if (sum) ii=i;
    b[i]=sum;
  }
  for(i=n-1; i>=0; i--){
    sum=b[i];
    for(j=i+1;j<n;j++) sum -= a(i,j)*b[j];
    b[i]=sum/a(i,i);
  }
}


void LU_OMP ( int n, FullMatrix &a) 
{
  int iam,nt;
#pragma omp parallel private (iam)
  {
    for (int k=0;k<n-1;k++){
      {
	iam = omp_get_thread_num();
	nt  = omp_get_num_threads();
	for (int i=k+1+iam;i<n;i+=nt){
	  double lik = a(i,k)/a(k,k);
	  for (int j=k+1;j<n;j++){
	    a(i,j) -= lik * a(k,j);
	  }
	  a(i,k) = lik;
	  //	b[i] -= lik * b[k];
	}
#pragma omp barrier
      }
    }
  }// end of the scope of the omp paragma
}


void INVERT_OMP(int n, FullMatrix &a, FullMatrix &y)
{

  double d,*col;
  int i,j,*indx;

  col = (double *)malloc(n*sizeof(double));
  indx = (int *)malloc(n*sizeof(int));
  LU_OMP(n,a);

  for(j=0;j<n;j++){
    for(i=0; i < n; i++) col[i] = 0.0;
    col[j]=1.0;
    BACKSB(n,a,col);
    for(i=0;i<n;i++) y(i,j)=col[i];
  }
  free(col);
  free(indx);
}


int main (int argc, char *argv[]){

  const int N = atoi(argv[1]);
  FullMatrix a(N,N), ainv(N,N), b(N,N);
  double * rhs = new double [N];
  double * x = new double [N];

  for (int i=0;i<N;i++){
    rhs[i] = (double)rand()/RAND_MAX;
    for (int j=0;j<N;j++){
      a(i,j) = (double)rand()/RAND_MAX;
      b(i,j) = (double)rand()/RAND_MAX;
    }
  }  

  double t = 0;
  double t1 = ResourceManager::instance().wallTime();
  LU_OMP(N,a);
  double t2 = ResourceManager::instance().wallTime();

  printf("LU time = %12.5E\n",t2-t1);

  b.lu_solve(rhs,x);
  double t3 = ResourceManager::instance().wallTime();
  printf("LU_LAPACK time = %12.5E\n",t3-t2);

  //  delete [] b;
  
}