/********************************************************************
*  sample.cu
*  This is a example of the CUDA program.
*********************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <cuda_runtime.h>
#include <cutil.h>

/************************************************************************/
/* Init CUDA                                                            */
/************************************************************************/
#if __DEVICE_EMULATION__

bool InitCUDA(void){return true;}

#else
bool InitCUDA(void)
{
	int count = 0;
	int i = 0; 

	cudaGetDeviceCount(&count);
	if(count == 0) { 
		fprintf(stderr, "There is no device.\n");
		return false; 
	}

	for(i = 0; i < count; i++) {
		cudaDeviceProp prop; 
		if(cudaGetDeviceProperties(&prop, i) == cudaSuccess) {
			if(prop.major >= 1) {
				break;
			}
		}
	}
	if(i == count) {
		fprintf(stderr, "There is no device supporting CUDA.\n");
		return false;
	}
	cudaSetDevice(i);

	printf("CUDA initialized.\n");
	return true;
}

#endif
/******************************************************************/
/* Denisity Gradient Function                                                            */
/************************************************************************/
__device__ float grad_N (int node, int coord, float x0, float y0, float z0, float x1,
	float y1, float z1, float x2, float y2, float z2, float x3,
	float y3, float z3)
{
  switch (node)
    {
    case 0:
      switch (coord)
	{
	case 0:
	  return (y1 * z2 - y1 * z3 - z1 * y2 + z1 * y3 + y2 * z3 -
		  y3 * z2) / (x0 * y1 * z2 - x0 * y1 * z3 -
			      x0 * z1 * y2 + x0 * z1 * y3 +
			      x0 * y2 * z3 - x0 * y3 * z2 -
			      y0 * x1 * z2 + y0 * x1 * z3 +
			      y0 * z1 * x2 - y0 * z1 * x3 -
			      y0 * x2 * z3 + y0 * x3 * z2 +
			      z0 * x1 * y2 - z0 * x1 * y3 -
			      z0 * y1 * x2 + z0 * y1 * x3 +
			      z0 * x2 * y3 - z0 * x3 * y2 -
			      x1 * y2 * z3 + x1 * y3 * z2 +
			      y1 * x2 * z3 - y1 * x3 * z2 -
			      z1 * x2 * y3 + z1 * x3 * y2);
	  break;
	case 1:
	  return -(x1 * z2 - x1 * z3 - z1 * x2 + z1 * x3 + x2 * z3 -
		   x3 * z2) / (x0 * y1 * z2 - x0 * y1 * z3 -
			       x0 * z1 * y2 + x0 * z1 * y3 +
			       x0 * y2 * z3 - x0 * y3 * z2 -
			       y0 * x1 * z2 + y0 * x1 * z3 +
			       y0 * z1 * x2 - y0 * z1 * x3 -
			       y0 * x2 * z3 + y0 * x3 * z2 +
			       z0 * x1 * y2 - z0 * x1 * y3 -
			       z0 * y1 * x2 + z0 * y1 * x3 +
			       z0 * x2 * y3 - z0 * x3 * y2 -
			       x1 * y2 * z3 + x1 * y3 * z2 +
			       y1 * x2 * z3 - y1 * x3 * z2 -
			       z1 * x2 * y3 + z1 * x3 * y2);
	  break;
	case 2:
	  return (x1 * y2 - x1 * y3 - y1 * x2 + y1 * x3 + x2 * y3 -
		  x3 * y2) / (x0 * y1 * z2 - x0 * y1 * z3 -
			      x0 * z1 * y2 + x0 * z1 * y3 +
			      x0 * y2 * z3 - x0 * y3 * z2 -
			      y0 * x1 * z2 + y0 * x1 * z3 +
			      y0 * z1 * x2 - y0 * z1 * x3 -
			      y0 * x2 * z3 + y0 * x3 * z2 +
			      z0 * x1 * y2 - z0 * x1 * y3 -
			      z0 * y1 * x2 + z0 * y1 * x3 +
			      z0 * x2 * y3 - z0 * x3 * y2 -
			      x1 * y2 * z3 + x1 * y3 * z2 +
			      y1 * x2 * z3 - y1 * x3 * z2 -
			      z1 * x2 * y3 + z1 * x3 * y2);
	}
      break;

    case 1:
      switch (coord)
	{
	case 0:
	  return -(y0 * z2 - y0 * z3 - z0 * y2 + z0 * y3 + y2 * z3 -
		   y3 * z2) / (x0 * y1 * z2 - x0 * y1 * z3 -
			       x0 * z1 * y2 + x0 * z1 * y3 +
			       x0 * y2 * z3 - x0 * y3 * z2 -
			       y0 * x1 * z2 + y0 * x1 * z3 +
			       y0 * z1 * x2 - y0 * z1 * x3 -
			       y0 * x2 * z3 + y0 * x3 * z2 +
			       z0 * x1 * y2 - z0 * x1 * y3 -
			       z0 * y1 * x2 + z0 * y1 * x3 +
			       z0 * x2 * y3 - z0 * x3 * y2 -
			       x1 * y2 * z3 + x1 * y3 * z2 +
			       y1 * x2 * z3 - y1 * x3 * z2 -
			       z1 * x2 * y3 + z1 * x3 * y2);
	  break;
	case 1:
	  return (x0 * z2 - x0 * z3 - z0 * x2 + z0 * x3 + x2 * z3 -
		  x3 * z2) / (x0 * y1 * z2 - x0 * y1 * z3 -
			      x0 * z1 * y2 + x0 * z1 * y3 +
			      x0 * y2 * z3 - x0 * y3 * z2 -
			      y0 * x1 * z2 + y0 * x1 * z3 +
			      y0 * z1 * x2 - y0 * z1 * x3 -
			      y0 * x2 * z3 + y0 * x3 * z2 +
			      z0 * x1 * y2 - z0 * x1 * y3 -
			      z0 * y1 * x2 + z0 * y1 * x3 +
			      z0 * x2 * y3 - z0 * x3 * y2 -
			      x1 * y2 * z3 + x1 * y3 * z2 +
			      y1 * x2 * z3 - y1 * x3 * z2 -
			      z1 * x2 * y3 + z1 * x3 * y2);
	  break;
	case 2:
	  return -(x0 * y2 - x0 * y3 - y0 * x2 + y0 * x3 + x2 * y3 -
		   x3 * y2) / (x0 * y1 * z2 - x0 * y1 * z3 -
			       x0 * z1 * y2 + x0 * z1 * y3 +
			       x0 * y2 * z3 - x0 * y3 * z2 -
			       y0 * x1 * z2 + y0 * x1 * z3 +
			       y0 * z1 * x2 - y0 * z1 * x3 -
			       y0 * x2 * z3 + y0 * x3 * z2 +
			       z0 * x1 * y2 - z0 * x1 * y3 -
			       z0 * y1 * x2 + z0 * y1 * x3 +
			       z0 * x2 * y3 - z0 * x3 * y2 -
			       x1 * y2 * z3 + x1 * y3 * z2 +
			       y1 * x2 * z3 - y1 * x3 * z2 -
			       z1 * x2 * y3 + z1 * x3 * y2);
	  break;
	}

    case 2:
      switch (coord)
	{
	case 0:
	  return (y0 * z1 - y0 * z3 - z0 * y1 + z0 * y3 + y1 * z3 -
		  z1 * y3) / (x0 * y1 * z2 - x0 * y1 * z3 -
			      x0 * z1 * y2 + x0 * z1 * y3 +
			      x0 * y2 * z3 - x0 * y3 * z2 -
			      y0 * x1 * z2 + y0 * x1 * z3 +
			      y0 * z1 * x2 - y0 * z1 * x3 -
			      y0 * x2 * z3 + y0 * x3 * z2 +
			      z0 * x1 * y2 - z0 * x1 * y3 -
			      z0 * y1 * x2 + z0 * y1 * x3 +
			      z0 * x2 * y3 - z0 * x3 * y2 -
			      x1 * y2 * z3 + x1 * y3 * z2 +
			      y1 * x2 * z3 - y1 * x3 * z2 -
			      z1 * x2 * y3 + z1 * x3 * y2);
	  break;
	case 1:
	  return -(x0 * z1 - x0 * z3 - z0 * x1 + z0 * x3 + x1 * z3 -
		   z1 * x3) / (x0 * y1 * z2 - x0 * y1 * z3 -
			       x0 * z1 * y2 + x0 * z1 * y3 +
			       x0 * y2 * z3 - x0 * y3 * z2 -
			       y0 * x1 * z2 + y0 * x1 * z3 +
			       y0 * z1 * x2 - y0 * z1 * x3 -
			       y0 * x2 * z3 + y0 * x3 * z2 +
			       z0 * x1 * y2 - z0 * x1 * y3 -
			       z0 * y1 * x2 + z0 * y1 * x3 +
			       z0 * x2 * y3 - z0 * x3 * y2 -
			       x1 * y2 * z3 + x1 * y3 * z2 +
			       y1 * x2 * z3 - y1 * x3 * z2 -
			       z1 * x2 * y3 + z1 * x3 * y2);
	  break;
	case 2:
	  return (x0 * y1 - x0 * y3 - y0 * x1 + y0 * x3 + x1 * y3 -
		  y1 * x3) / (x0 * y1 * z2 - x0 * y1 * z3 -
			      x0 * z1 * y2 + x0 * z1 * y3 +
			      x0 * y2 * z3 - x0 * y3 * z2 -
			      y0 * x1 * z2 + y0 * x1 * z3 +
			      y0 * z1 * x2 - y0 * z1 * x3 -
			      y0 * x2 * z3 + y0 * x3 * z2 +
			      z0 * x1 * y2 - z0 * x1 * y3 -
			      z0 * y1 * x2 + z0 * y1 * x3 +
			      z0 * x2 * y3 - z0 * x3 * y2 -
			      x1 * y2 * z3 + x1 * y3 * z2 +
			      y1 * x2 * z3 - y1 * x3 * z2 -
			      z1 * x2 * y3 + z1 * x3 * y2);
	  break;
	}

    case 3:
      switch (coord)
	{
	case 0:
	  return -(y0 * z1 - y0 * z2 - z0 * y1 + z0 * y2 + y1 * z2 -
		   z1 * y2) / (x0 * y1 * z2 - x0 * y1 * z3 -
			       x0 * z1 * y2 + x0 * z1 * y3 +
			       x0 * y2 * z3 - x0 * y3 * z2 -
			       y0 * x1 * z2 + y0 * x1 * z3 +
			       y0 * z1 * x2 - y0 * z1 * x3 -
			       y0 * x2 * z3 + y0 * x3 * z2 +
			       z0 * x1 * y2 - z0 * x1 * y3 -
			       z0 * y1 * x2 + z0 * y1 * x3 +
			       z0 * x2 * y3 - z0 * x3 * y2 -
			       x1 * y2 * z3 + x1 * y3 * z2 +
			       y1 * x2 * z3 - y1 * x3 * z2 -
			       z1 * x2 * y3 + z1 * x3 * y2);
	  break;
	case 1:
	  return (x0 * z1 - x0 * z2 - z0 * x1 + z0 * x2 + x1 * z2 -
		  z1 * x2) / (x0 * y1 * z2 - x0 * y1 * z3 -
			      x0 * z1 * y2 + x0 * z1 * y3 +
			      x0 * y2 * z3 - x0 * y3 * z2 -
			      y0 * x1 * z2 + y0 * x1 * z3 +
			      y0 * z1 * x2 - y0 * z1 * x3 -
			      y0 * x2 * z3 + y0 * x3 * z2 +
			      z0 * x1 * y2 - z0 * x1 * y3 -
			      z0 * y1 * x2 + z0 * y1 * x3 +
			      z0 * x2 * y3 - z0 * x3 * y2 -
			      x1 * y2 * z3 + x1 * y3 * z2 +
			      y1 * x2 * z3 - y1 * x3 * z2 -
			      z1 * x2 * y3 + z1 * x3 * y2);
	  break;
	case 2:
	  return -(x0 * y1 - x0 * y2 - y0 * x1 + y0 * x2 + x1 * y2 -
		   y1 * x2) / (x0 * y1 * z2 - x0 * y1 * z3 -
			       x0 * z1 * y2 + x0 * z1 * y3 +
			       x0 * y2 * z3 - x0 * y3 * z2 -
			       y0 * x1 * z2 + y0 * x1 * z3 +
			       y0 * z1 * x2 - y0 * z1 * x3 -
			       y0 * x2 * z3 + y0 * x3 * z2 +
			       z0 * x1 * y2 - z0 * x1 * y3 -
			       z0 * y1 * x2 + z0 * y1 * x3 +
			       z0 * x2 * y3 - z0 * x3 * y2 -
			       x1 * y2 * z3 + x1 * y3 * z2 +
			       y1 * x2 * z3 - y1 * x3 * z2 -
			       z1 * x2 * y3 + z1 * x3 * y2);
	  break;
	}
    }

  return 0;
}

/******************************************************************/
/* Denisity Kernel Function                                                            */
/************************************************************************/
__global__ void calcDivergenceDensity(int * ids, int * unique_neighbours_array, int * unique_number_neighbours,
									int * unique_neighbours_index, int * neighbours_array, int * neighbours_array_ids,
									int * neighbours_array_index, int * number_neighbours,
									float * displacements, float * vertices, float * initialDensity, float * divigden, int as, int t)
{
	
	int idx=blockIdx.x*blockDim.x+threadIdx.x;
    int idy=blockIdx.y*blockDim.y+threadIdx.y;
	int index = idy*as+idx;
	//int neighbours = unique_number_neighbours[index];
	int j=0;
	int j_prime=0;
	int number_tetra;
	int k,m,l;
	int shape ,chunkoffset;
	float displ[3];
	//int kId;
	float divig=0;
	float delta_rho=0;
	int node0, node1, node2, node3, tetraNo;
	for(j=unique_neighbours_index[ids[index]];j<unique_neighbours_index[ids[index]]+unique_number_neighbours[index];j++){
		//cout<<"in j loop"<<endl;
		for(j_prime=unique_neighbours_index[ids[index]];j_prime<unique_neighbours_index[ids[index]]+unique_number_neighbours[index];j_prime++){
			//cout<<"in j prime loop"<<endl;
			number_tetra=0;
			if(ids[index]==ids[unique_neighbours_array[j]] || 
				ids[index]==ids[unique_neighbours_array[j_prime]]){
				//cout<<"Found i that matches j or j prime"<<endl;
				if(ids[index]==ids[unique_neighbours_array[j]]){
					k=unique_neighbours_array[j_prime];
				}else{
					k=unique_neighbours_array[j];
				}
				//cout<<"K is "<<k<<endl;
				//timeoffset = (t*4*3); // Which time step should we be looking at 
				shape = (k*3)/(4*3); //Which shape number we are working with
				chunkoffset = (shape*10*3*4); // Which section of the array should we be looking at based on the shape number remember to change timestep!!!
				//cout<<"worked out disp offsets"<<endl;
				displ[0] = displacements[((k*3)%(3*4))+chunkoffset+(t*4*3)];
				displ[1] = displacements[((k*3)%(3*4))+1+chunkoffset+(t*4*3)];
				displ[2]= displacements[((k*3)%(3*4))+2+chunkoffset+(t*4*3)];
				//cout<<"found displacements"<<endl;
				//cout<<"found K ID "<<kId<<endl;
				for(m=neighbours_array_index[ids[index]];m<neighbours_array_index[ids[index]]+number_neighbours[ids[index]];m++){
					if(ids[k]==neighbours_array_ids[m]){
						switch(neighbours_array[m]%4)
						{
							case 0:
								tetraNo = 0;
								node0=neighbours_array[m];
								node1=(neighbours_array[m]+1);
								node2=(neighbours_array[m]+2);
								node3=(neighbours_array[m]+3);
								break;
							case 1:
								tetraNo = 1;
								node0 = (neighbours_array[m]-1);
								node1 =neighbours_array[m]; 
								node2=(neighbours_array[m]+1);
								node3 = (neighbours_array[m]+2);
								break;
							case 2:
								tetraNo = 2;
								node0 = (neighbours_array[m]-2);
								node1 = (neighbours_array[m]-1);
								node2 =neighbours_array[m];
								node3 = (neighbours_array[m]+1);
								break;
							case 3:
								tetraNo = 3;
								node0 = (neighbours_array[m]-3);
								node1 = (neighbours_array[m]-2);
								node2 = (neighbours_array[m]-1);
								node3= neighbours_array[m];
								break;
							default:
								break;
						}					
						//cout<<"set Nodes"<<endl;
						number_tetra++;
						for(l=0;l<3;l++){
									  divig += grad_N (tetraNo, l,
				 							vertices[node0*3],
				 							vertices[(node0*3)+1],
				 							vertices[(node0*3)+2],
				 							vertices[node1*3],
				 							vertices[(node1*3)+1],
				 							vertices[(node1*3)+2],
				 							vertices[node2*3],
				 							vertices[(node2*3)+1],
				 							vertices[(node2*3)+2],
				 							vertices[node3*3],
				 							vertices[(node3*3)+1],
				 							vertices[(node3*3)+2]) *
											displ[l];
									  //cout<<"calc divergence"<<endl;
						}
					}
					//cout<<"finished seraching array for k tetrahedron"<<endl;
				}
				if(ids[unique_neighbours_array[j]]==ids[unique_neighbours_array[j_prime]]){ 
					divig=divig*2;
				}
				delta_rho += -(initialDensity[unique_neighbours_array[j]]*(divig/number_tetra));
				number_tetra=0;
				divig=0;
			}else{
				//cout<<"I does not match j or j prime divig 0"<<endl;
				divig=0;
			}

		}

	}
	divigden[index] = delta_rho;

}
/******************************************************************/
/* Denisity Set Up Fucntion                                                          */
/************************************************************************/


float * denisityInitialisation(int * idArray, int * una, int * unn, int * uni, int * na, int * nai, int * naindx, int * nn, int idLength,
									   float * disp, float * vert, float * initDensities, float * divigden, int nodes, int t)
{
	

	int * ids_d;
	int * unique_neighbours_array_d;
	int * unique_neighbours_index_d;
	int * unique_number_neighbours_d;
	int * neighbours_array_d; 
	int * neighbours_array_ids_d; 
	int * neighbours_array_index_d;
	int * number_neighbours_d;


	float * disp_d;
	float * vert_d;
	float * initDensities_d;
	float * divigden_d;


	printf("doing mallocs\n");
	CUDA_SAFE_CALL(cudaMalloc((void**)&ids_d, nodes*sizeof(int)));
	CUDA_SAFE_CALL(cudaMalloc((void**)&unique_neighbours_array_d, nodes*3*sizeof(int)));
	CUDA_SAFE_CALL(cudaMalloc((void**)&unique_number_neighbours_d, idLength*sizeof(int)));
	CUDA_SAFE_CALL(cudaMalloc((void**)&unique_neighbours_index_d, idLength*sizeof(int)));

	CUDA_SAFE_CALL(cudaMalloc((void**)&neighbours_array_d, nodes*4*sizeof(int)));
	CUDA_SAFE_CALL(cudaMalloc((void**)&neighbours_array_ids_d, nodes*4*sizeof(int)));
	CUDA_SAFE_CALL(cudaMalloc((void**)&neighbours_array_index_d, idLength*sizeof(int)));
	CUDA_SAFE_CALL(cudaMalloc((void**)&number_neighbours_d, idLength*sizeof(int)));

	CUDA_SAFE_CALL(cudaMalloc((void**)&disp_d, nodes*30*sizeof(float)));
	CUDA_SAFE_CALL(cudaMalloc((void**)&vert_d, nodes*3*sizeof(float)));
	CUDA_SAFE_CALL(cudaMalloc((void**)&initDensities_d, nodes*sizeof(float)));
	CUDA_SAFE_CALL(cudaMalloc((void**)&divigden_d, nodes*sizeof(float)));

	printf("doing memcpy\n");
	CUDA_SAFE_CALL(cudaMemcpy(ids_d, idArray,nodes*sizeof(int), cudaMemcpyHostToDevice));
	CUDA_SAFE_CALL(cudaMemcpy(unique_neighbours_array_d, una,nodes*3*sizeof(int), cudaMemcpyHostToDevice));
	CUDA_SAFE_CALL(cudaMemcpy(unique_number_neighbours_d, unn,idLength*sizeof(int), cudaMemcpyHostToDevice));
	CUDA_SAFE_CALL(cudaMemcpy(unique_neighbours_index_d, uni,idLength*sizeof(int), cudaMemcpyHostToDevice));

	CUDA_SAFE_CALL(cudaMemcpy(neighbours_array_d, na,nodes*4*sizeof(int), cudaMemcpyHostToDevice));
	CUDA_SAFE_CALL(cudaMemcpy(neighbours_array_ids_d, nai,nodes*4*sizeof(int), cudaMemcpyHostToDevice));
	CUDA_SAFE_CALL(cudaMemcpy(neighbours_array_index_d, naindx,idLength*sizeof(int), cudaMemcpyHostToDevice));
	CUDA_SAFE_CALL(cudaMemcpy(number_neighbours_d, unn,idLength*sizeof(int), cudaMemcpyHostToDevice));

	CUDA_SAFE_CALL(cudaMemcpy(disp_d, disp,nodes*30*sizeof(float), cudaMemcpyHostToDevice));
	CUDA_SAFE_CALL(cudaMemcpy(vert_d, vert,nodes*3*sizeof(float), cudaMemcpyHostToDevice));
	CUDA_SAFE_CALL(cudaMemcpy(initDensities_d, initDensities,nodes*sizeof(float), cudaMemcpyHostToDevice));

	printf("before block size\n");
	int N = nodes;
	int block_size = 512;
	int n_blocks = N/block_size + (N%block_size == 0 ? 0:1);
	//KERNEL CALL HERE!!!!!
	unsigned int timer = 0;
	CUT_SAFE_CALL( cutCreateTimer( &timer));
	CUT_SAFE_CALL( cutStartTimer( timer));

	calcDivergenceDensity<<<n_blocks,block_size>>>(ids_d, unique_neighbours_array_d, unique_number_neighbours_d,
									unique_neighbours_index_d, neighbours_array_d, neighbours_array_ids_d,
									neighbours_array_index_d, number_neighbours_d,
									disp_d, vert_d, initDensities_d, divigden_d, nodes, t);

	
	CUDA_SAFE_CALL( cudaThreadSynchronize() );
	CUT_CHECK_ERROR("Kernel execution failed\n");
	CUT_SAFE_CALL( cutStopTimer( timer));
	printf("Processing time: %f (ms)\n", cutGetTimerValue( timer));
	CUT_SAFE_CALL( cutDeleteTimer( timer));

	CUDA_SAFE_CALL(cudaMemcpy(divigden, divigden_d, sizeof(float)*nodes, cudaMemcpyDeviceToHost));
	



	printf("doing free\n");
	CUDA_SAFE_CALL(cudaFree(ids_d));
	CUDA_SAFE_CALL(cudaFree(unique_neighbours_array_d));
	CUDA_SAFE_CALL(cudaFree(unique_number_neighbours_d));
	CUDA_SAFE_CALL(cudaFree(unique_neighbours_index_d));

	CUDA_SAFE_CALL(cudaFree(neighbours_array_d));
	CUDA_SAFE_CALL(cudaFree(neighbours_array_ids_d));
	CUDA_SAFE_CALL(cudaFree(neighbours_array_index_d));
	CUDA_SAFE_CALL(cudaFree(number_neighbours_d));

	CUDA_SAFE_CALL(cudaFree(disp_d));
	CUDA_SAFE_CALL(cudaFree(vert_d));
	CUDA_SAFE_CALL(cudaFree(initDensities_d));
	CUDA_SAFE_CALL(cudaFree(divigden_d));
	
	return divigden;


}



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

	if(!InitCUDA()) {
		return 0;
	}
	int * idArray;
	int * una; 
	int * unn; 
	int * uni; 
	int * na; 
	int * nai; 
	int * naindx; 
	int * nn; 
	float * disp;
	float * vert;
	float * initDensities;
	float * divigden; 
	int nodes = 19224*4; 
	int t =0;
	int idLength = 8700;

	idArray = new int [nodes];
	una = new int [nodes*3]; 
	unn = new int [idLength]; 
	uni = new int [idLength]; 
	na = new int [nodes*4]; 
	nai= new int [nodes*4];  
	naindx= new int [idLength]; ; 
	nn = new int [idLength];  ; 
	disp = new float[nodes*30];
	vert = new float[nodes*3];
	initDensities = new float[nodes];
	divigden = new float[nodes]; 

	denisityInitialisation(idArray, una, unn, uni, na, nai,naindx, nn, idLength, disp,vert, initDensities, divigden, nodes, t);

	return 0;
}