#include <cusp/ell_matrix.h>
#include <cusp/print.h>
#include <iostream>

#define SPARSE_MEM_OFFSET 116
#define SOLID 35
const int height = 4;

texture<float, 2>  texLevelSet1;
float* d_levelSet1;

__device__ int oneDimIndexOfXY(int x, int y)
{
return x + y*height;
}

__device__ int getDevPtrForSparse(int x,int y,int i,int offset)
{
return oneDimIndexOfXY(x,y) + i*offset;
}



__global__ void createSparseMatrix(int* column_indices, float* values, bool readFromLevelSet1, const int invalid_index, const int column_diff)
{
//float scale = dt / (rho*dx*dx)
float scale = 1.0f;

int x = threadIdx.x + blockIdx.x * blockDim.x;
    int y = threadIdx.y + blockIdx.y * blockDim.y;

float phi;
if (readFromLevelSet1)
phi = tex2D(texLevelSet1,x,y);
/*else
phi = tex2D(texLevelSet2,x,y);*/

if (phi <= 0)
{
int index = oneDimIndexOfXY(x,y);

float phiLeft,phiRight,phiUp,phiDown;
if (readFromLevelSet1)
{
phiLeft = tex2D(texLevelSet1,x-1,y);
phiRight = tex2D(texLevelSet1,x+1,y);
phiUp = tex2D(texLevelSet1,x,y+1);
phiDown = tex2D(texLevelSet1,x,y-1);
}
/*else
{
phiLeft = tex2D(texLevelSet1,x-1,y);
phiRight = tex2D(texLevelSet1,x+1,y);
phiUp = tex2D(texLevelSet1,x,y+1);
phiDown = tex2D(texLevelSet1,x,y-1);
}*/

if (phiDown <= 0)
{
*(column_indices + getDevPtrForSparse(x,y,0,column_diff)) = oneDimIndexOfXY(x,y-1);
*(values + getDevPtrForSparse(x,y,0,column_diff)) = -scale;
}
else
{
//Om vi vet #N av antal non zeros i A
//*(column_indices + getDevPtrForSparse(x,y,0)) = invalid_index;

*(column_indices + getDevPtrForSparse(x,y,0,column_diff)) = oneDimIndexOfXY(x,y-1);
*(values + getDevPtrForSparse(x,y,0,column_diff)) = 0;
}


if (phiLeft <= 0)
{
*(column_indices + getDevPtrForSparse(x,y,1,column_diff)) = oneDimIndexOfXY(x-1,y);
*(values + getDevPtrForSparse(x,y,1,column_diff)) = -scale;
}
else
{
*(column_indices + getDevPtrForSparse(x,y,1,column_diff)) = oneDimIndexOfXY(x-1,y);
*(values + getDevPtrForSparse(x,y,1,column_diff)) = 0;
}

int diagScale = 0;
if (phiLeft != SOLID)
diagScale++;
if (phiRight != SOLID)
diagScale++;
if (phiUp != SOLID)
diagScale++;
if (phiDown != SOLID)
diagScale++;
*(column_indices + getDevPtrForSparse(x,y,2,column_diff)) = oneDimIndexOfXY(x,y);
*(values + getDevPtrForSparse(x,y,2,column_diff)) = diagScale * scale;


if (phiRight  <= 0)
{
*(column_indices + getDevPtrForSparse(x,y,3,column_diff)) = oneDimIndexOfXY(x+1,y);
*(values + getDevPtrForSparse(x,y,3,column_diff)) = -scale;
}
else
{
*(column_indices + getDevPtrForSparse(x,y,3,column_diff)) = oneDimIndexOfXY(x+1,y);
*(values + getDevPtrForSparse(x,y,3,column_diff)) = 0;
}

if (phiUp  <= 0)
{
*(column_indices + getDevPtrForSparse(x,y,4,column_diff)) = oneDimIndexOfXY(x,y+1);
*(values + getDevPtrForSparse(x,y,4,column_diff)) = -scale;
}
else
{
*(column_indices + getDevPtrForSparse(x,y,4,column_diff)) = oneDimIndexOfXY(x,y+1);
*(values + getDevPtrForSparse(x,y,4,column_diff)) = 0;
}
}
else
{
*(column_indices + getDevPtrForSparse(x,y,0,column_diff)) = 0;
*(values + getDevPtrForSparse(x,y,0,column_diff)) = 0;
*(column_indices + getDevPtrForSparse(x,y,1,column_diff)) = 1;
*(values + getDevPtrForSparse(x,y,1,column_diff)) = 1;
*(column_indices + getDevPtrForSparse(x,y,2,column_diff)) = 2;
*(values + getDevPtrForSparse(x,y,2,column_diff)) = 2;
*(column_indices + getDevPtrForSparse(x,y,3,column_diff)) = 3;
*(values + getDevPtrForSparse(x,y,3,column_diff)) = 3;
*(column_indices + getDevPtrForSparse(x,y,4,column_diff)) = 4;
*(values + getDevPtrForSparse(x,y,4,column_diff)) = 4;
}
}
int main()
{

int imageSize = height*height*sizeof(float);
cudaMalloc((void**)&d_levelSet1, imageSize);
cudaChannelFormatDesc desc = cudaCreateChannelDesc<float>();
cudaBindTexture2D(NULL, texLevelSet1, d_levelSet1, desc, height, height, sizeof(float)*height);

float *tempLevelSet1 = (float*)malloc(imageSize);


/*
S  S  S S
S  1  1 S
S -1 -1 S
S  S  S S
*/

tempLevelSet1[0] = SOLID; tempLevelSet1[1] = SOLID; tempLevelSet1[2] = SOLID; tempLevelSet1[3] = SOLID;
tempLevelSet1[4] = SOLID; tempLevelSet1[5] = -1; tempLevelSet1[6] = -1; tempLevelSet1[7] = SOLID;
tempLevelSet1[8] = SOLID; tempLevelSet1[9] = 1; tempLevelSet1[10] = 1; tempLevelSet1[11] = SOLID;
tempLevelSet1[12] = SOLID; tempLevelSet1[13] = SOLID; tempLevelSet1[14] = SOLID; tempLevelSet1[15] = SOLID;

cudaMemcpy(d_levelSet1, tempLevelSet1,imageSize,cudaMemcpyHostToDevice);
    free(tempLevelSet1);


//A matrix
cusp::ell_matrix<int,float,cusp::device_memory> A(height*height,height*height, 5*height*height, 5);

for (int x= 0;x<5;x++)
for (int y = 0; y<4;y++)
std::cout << "Adress för (" << y  << "," << x << "): "  << int (thrust::raw_pointer_cast(&A.values(y,x)))  << std::endl;



int column_diff = int(thrust::raw_pointer_cast(&A.values(0,1)) - thrust::raw_pointer_cast(&A.values(0,0)));
createSparseMatrix<<<1,16>>>(thrust::raw_pointer_cast(&A.column_indices(0,0)),thrust::raw_pointer_cast(&A.values(0,0)),true,-1,column_diff);


cusp::print_matrix(A);
cusp::ell_matrix<int,float,cusp::host_memory> B = A;

std::cout << "Från A[5, " << B.column_indices(5,0) << "] = " << B.values(5,0) <<std::endl;
std::cout << "Från A[5, " << B.column_indices(5,1) << "] = " << B.values(5,1) <<std::endl;
std::cout << "Från A[5, " << B.column_indices(5,2) << "] = " << B.values(5,2) <<std::endl;
std::cout << "Från A[5, " << B.column_indices(5,3) << "] = " << B.values(5,3) <<std::endl;
std::cout << "Från A[5, " << B.column_indices(5,4) << "] = " << B.values(5,4) <<std::endl;

std::cout << "Från A[6, " << B.column_indices(6,0) << "] = " << B.values(6,0) <<std::endl;
std::cout << "Från A[6, " << B.column_indices(6,1) << "] = " << B.values(6,1) <<std::endl;
std::cout << "Från A[6, " << B.column_indices(6,2) << "] = " << B.values(6,2) <<std::endl;
std::cout << "Från A[6, " << B.column_indices(6,3) << "] = " << B.values(6,3) <<std::endl;
std::cout << "Från A[6,  " << B.column_indices(6,4) << "] = " << B.values(6,4) <<std::endl;

std::cout << "Från A[9, " << B.column_indices(9,0) << "] = " << B.values(9,0) <<std::endl;
std::cout << "Från A[9, " << B.column_indices(9,1) << "] = " << B.values(9,1) <<std::endl;
std::cout << "Från A[9, " << B.column_indices(9,2) << "] = " << B.values(9,2) <<std::endl;
std::cout << "Från A[9, " << B.column_indices(9,3) << "] = " << B.values(9,3) <<std::endl;
std::cout << "Från A[9, " << B.column_indices(9,4) << "] = " << B.values(9,4) <<std::endl;

std::cout << "Från A[10, " << B.column_indices(10,0) << "] = " << B.values(10,0) <<std::endl;
std::cout << "Från A[10, " << B.column_indices(10,1) << "] = " << B.values(10,1) <<std::endl;
std::cout << "Från A[10, " << B.column_indices(10,2) << "] = " << B.values(10,2) <<std::endl;
std::cout << "Från A[10, " << B.column_indices(10,3) << "] = " << B.values(10,3) <<std::endl;
std::cout << "Från A[10, " << B.column_indices(10,4) << "] = " << B.values(10,4) <<std::endl;

cudaUnbindTexture(texLevelSet1);
cudaFree(d_levelSet1);
return 0;
}