//#define TEST
#define X	0
#define Y	0

//#define USE_DBL
#ifdef USE_DBL
#define TYPE_FLT  double
#define TYPE_INT  long
#define MASK      0xFFFF
#define SHIFT     16
#define EPSILSON  4.94065645841247E-324
#else
#define TYPE_FLT  float
#define TYPE_INT  int
#define MASK      0x00FF
#define SHIFT     8
#define EPSILSON  1.401298E-45
#endif

constant sampler_t sampler = CLK_NORMALIZED_COORDS_FALSE |
                                CLK_ADDRESS_CLAMP_TO_EDGE |
                                CLK_FILTER_LINEAR;

struct input
{
	int output_width;
  int grid_x_size;
  int grid_z_size;
};

TYPE_FLT read_data(image2d_t input_frame, int x, int z)
{
  int2 gid = (int2)(x, z);
  uint4 pixel = read_imageui(input_frame, sampler, gid);
  TYPE_INT temp =  (TYPE_INT)((TYPE_INT)pixel.x & MASK) | 
                    (TYPE_INT)(((TYPE_INT)pixel.y & MASK) << SHIFT) |
                    (TYPE_INT)(((TYPE_INT)pixel.z & MASK) << (SHIFT * 2)) | 
                    (TYPE_INT)(((TYPE_INT)pixel.w & MASK) << (SHIFT * 3));
  TYPE_FLT raw_data = *((TYPE_FLT*)(&temp));
  return raw_data;
}


TYPE_FLT mabs(TYPE_FLT arg)
{
  if (arg < 0)
    return -arg;
  return arg;    
}

kernel void ScanConversion(read_only image2d_t input_frame, read_write image2d_t output_frame, 
                          global TYPE_FLT* grid_x, global TYPE_FLT* grid_z, 
                          global TYPE_FLT* query_x, global TYPE_FLT* query_z,
                          struct input params) {
  int2 gid = (int2)(get_global_id(0), get_global_id(1));
  TYPE_FLT dx = grid_x[1] - grid_x[0];
  TYPE_FLT dz = grid_z[1] - grid_z[0];
  TYPE_FLT x = query_x[gid.y * params.output_width + gid.x];
  TYPE_FLT z = query_z[gid.y * params.output_width + gid.x];

  int grid_x_index = 0;
  int grid_z_index = 0;

  if(mabs(x - grid_x[params.grid_x_size - 1]) < EPSILSON)
    grid_x_index = params.grid_x_size - 2;
  else
    grid_x_index = floor((x - grid_x[0]) / dx);

  if(mabs(z - grid_z[params.grid_z_size - 1]) < EPSILSON)
    grid_z_index = params.grid_z_size - 2;
  else
    grid_z_index = floor((z - grid_z[0]) / dz);

  TYPE_FLT output_data = 0;
  if(grid_x_index >= 0 && grid_x_index < params.grid_x_size - 1 && 
    grid_z_index >= 0 && grid_z_index < params.grid_z_size - 1)
  {
    //matrix simplification
    TYPE_FLT a11 = grid_z[grid_z_index + 1] - z;
    TYPE_FLT a12 = z - grid_z[grid_z_index];
    TYPE_FLT b11 = read_data(input_frame, grid_x_index, grid_z_index);
    TYPE_FLT b12 = read_data(input_frame, grid_x_index + 1, grid_z_index);
    TYPE_FLT b21 = read_data(input_frame, grid_x_index, grid_z_index + 1);
    TYPE_FLT b22 = read_data(input_frame, grid_x_index + 1, grid_z_index + 1);
    TYPE_FLT c11 = grid_x[grid_x_index + 1] - x;
    TYPE_FLT c21 = x - grid_x[grid_x_index];

#ifdef TEST
    if(gid.x == X && gid.y == Y)
    {
      printf("dx: %.9f | ", dx);
      printf("dz: %.9f | ", dz);
      printf("x: %.9f | ", x);
      printf("z: %.9f | ", z);
      printf("grid_x_index: %d | ", grid_x_index);
      printf("grid_z_index: %d | ", grid_z_index);
      printf("a11: %.9f | ", a11);
      printf("a12: %.9f | ", a12);
      printf("b11: %.9f | ", b11);
      printf("b12: %.9f | ", b12);
      printf("b21: %.9f | ", b21);
      printf("b22: %.9f | ", b22);
      printf("c11: %.9f | ", c11);
      printf("c21: %.9f | ", c21);
    }
    #endif
    
    output_data = 1 / dx / dz * (c11 * (a11 * b11 + a12 * b21) + c21 * (a11 * b12 + a12 * b22));
  }

  TYPE_INT out = *((TYPE_INT*)(&output_data));
  uint4 pixel;
  pixel.x = (TYPE_INT)(out & MASK);
  pixel.y = (TYPE_INT)((out >> SHIFT) & MASK);
  pixel.z = (TYPE_INT)((out >> (SHIFT *2)) & MASK);
  pixel.w = (TYPE_INT)((out >> (SHIFT * 3)) & MASK);

#ifdef TEST
  if(gid.x == X && gid.y == Y)
  {
    printf("out: %.15f | ", output_data);
  }
#endif

  write_imageui(output_frame, gid, pixel);
}