Removed unsed code

Removed unsed code

#define __CLKERNEL

#ifdef __CLKERNEL
#else
#include <stdio.h>
#define uint unsigned int
#endif

#define OFFSET_BASIS 2166136261
#define FNV_PRIME 16777619
#define FLOAT_T float
#define HASH_T uint
#define N 5

typedef float4 Point;
typedef int4 IntPoint;

// FNV hash: http://isthe.com/chongo/tech/comp/fnv/#FNV-source
HASH_T hash(HASH_T i, HASH_T j, HASH_T k) {
  return (HASH_T)((((((OFFSET_BASIS ^ (HASH_T)i) * FNV_PRIME) ^ (HASH_T)j) * FNV_PRIME) ^ (HASH_T)k) * FNV_PRIME);
}

//~ FLOAT_T our_distance(Point p1, Point p2) {
    //~ Point d = fabs(p1-p2);
    //~ if(d.x > d.y && d.x > d.z)
	//~ return d.x;
    //~ if(d.y > d.z)
	//~ return d.y;
    //~ return d.z;
//~ }

// Return the square of the distance between points p1 and p2
FLOAT_T our_distance(Point p1, Point p2) {
  return (p1.x-p2.x)*(p1.x-p2.x) + (p1.y-p2.y)*(p1.y-p2.y) + (p1.z-p2.z)*(p1.z-p2.z);
}

void insert(FLOAT_T *arr, FLOAT_T value) {
  // Ugly hack to prevent duplicate values
  for(int i=0; i < N; ++i)
    if(arr[i] == value)
  	return;

  float temp;
  for(int i=N-1; i>=0; i--) {
    if(value > arr[i]) break;
    temp = arr[i];
    arr[i] = value;
    if(i+1<N) arr[i+1] = temp;
  }
}

// LCG Random Number Generator
uint rng(uint last) {
  return ((1103515245 * last + 12345) % 0x100000000);
}

// Generated with "AccountingForm[N[Table[CDF[PoissonDistribution[4], i], {i, 1, 9}], 20]*2^32]" //"N[Table[CDF[PoissonDistribution[4], i], {i, 1, 9}], 20]"
uint prob_lookup(uint value)
{
    if(value < 393325350) return 1;
    if(value < 1022645910) return 2;
    if(value < 1861739990) return 3;
    if(value < 2700834071) return 4;
    if(value < 3372109335) return 5;
    if(value < 3819626178) return 6;
    if(value < 4075350088) return 7;
    if(value < 4203212043) return 8;
    return 9;
}

void findDistancesForCube(FLOAT_T *distanceArray, Point p, IntPoint c) {
  uint rngLast = rng( hash(c.x, c.y, c.z) );
  uint rng1,rng2,rng3;
  Point randomDiff,featurePoint;
  uint numFPoints = prob_lookup( rngLast );

  for(uint i = 0; i < numFPoints; ++i) {
	rng1 = rng(rngLast);
	rng2 = rng(rng1);
	rng3 = rng(rng2);
	rngLast = rng3;

	randomDiff.x = (float)rng1 / 0x100000000;
	randomDiff.y = (float)rng2 / 0x100000000;
	randomDiff.z = (float)rng3 / 0x100000000;
	featurePoint = randomDiff + convert_float4(c);

	insert(distanceArray, our_distance(p,featurePoint));
  }
}

void forAll(FLOAT_T *distanceArray, Point p) {
    for(int i=-1; i < 2; ++i) {
	for(int j=-1; j < 2; ++j) {
	    for(int k=-1; k < 2; ++k) {
		IntPoint cube;
		cube.x = p.x + i;
		cube.y = p.y + j;
		cube.z = p.z + k;
		findDistancesForCube(distanceArray, p, cube);
	    }
	}
    }
}

__kernel void WorleyNoise(__global FLOAT_T *arrX, __global FLOAT_T *arrY, __global FLOAT_T *arrZ, __global FLOAT_T *output) {//, int width, int height, int depth) {
    uint idX = get_global_id(0);
    uint idY = get_global_id(1);
    uint idZ = get_global_id(2);
    uint width = get_global_size(0);
    uint height = get_global_size(1);
    uint depth = get_global_size(2);

  // Shall we do work?
  if(idX < width && idY < height && idZ < depth) {
	// Initalize array
	FLOAT_T darr[N];
	for(int i=0; i<N; ++i)
	  darr[i] = 888;

	Point p;
	p.x = arrX[idX];
	p.y = arrY[idY];
	p.z = arrZ[idZ];
	forAll(darr,p);
	//findDistancesForCube(darr, p, convert_int4(p));

	output[depth*height*idX + depth*idY + idZ] = darr[1] - darr[0];
  }
}

#define __CLKERNEL

#ifdef __CLKERNEL
#else
#include <stdio.h>
#define uint unsigned int
#endif

#define OFFSET_BASIS 2166136261
#define FNV_PRIME 16777619
#define FLOAT_T float
#define HASH_T uint
#define N 5

typedef float4 Point;
typedef int4 IntPoint;

// FNV hash: http://isthe.com/chongo/tech/comp/fnv/#FNV-source
HASH_T hash(HASH_T i, HASH_T j, HASH_T k) {
  return (HASH_T)((((((OFFSET_BASIS ^ (HASH_T)i) * FNV_PRIME) ^ (HASH_T)j) * FNV_PRIME) ^ (HASH_T)k) * FNV_PRIME);
}

//~ FLOAT_T our_distance(Point p1, Point p2) {
    //~ Point d = fabs(p1-p2);
    //~ if(d.x > d.y && d.x > d.z)
	//~ return d.x;
    //~ if(d.y > d.z)
	//~ return d.y;
    //~ return d.z;
//~ }

// Return the square of the distance between points p1 and p2
FLOAT_T our_distance(Point p1, Point p2) {
  return (p1.x-p2.x)*(p1.x-p2.x) + (p1.y-p2.y)*(p1.y-p2.y) + (p1.z-p2.z)*(p1.z-p2.z);
}

void insert(FLOAT_T *arr, FLOAT_T value) {
  // Ugly hack to prevent duplicate values
  for(int i=0; i < N; ++i)
    if(arr[i] == value)
  	return;

  float temp;
  for(int i=N-1; i>=0; i--) {
    if(value > arr[i]) break;
    temp = arr[i];
    arr[i] = value;
    if(i+1<N) arr[i+1] = temp;
  }
}

// LCG Random Number Generator
uint rng(uint last) {
  return ((1103515245 * last + 12345) % 0x100000000);
}

// Generated with "AccountingForm[N[Table[CDF[PoissonDistribution[4], i], {i, 1, 9}], 20]*2^32]" //"N[Table[CDF[PoissonDistribution[4], i], {i, 1, 9}], 20]"
uint prob_lookup(uint value)
{
    if(value < 393325350) return 1;
    if(value < 1022645910) return 2;
    if(value < 1861739990) return 3;
    if(value < 2700834071) return 4;
    if(value < 3372109335) return 5;
    if(value < 3819626178) return 6;
    if(value < 4075350088) return 7;
    if(value < 4203212043) return 8;
    return 9;
}

 //~ if(value < 0.091578194443670901469) return 1;
 //~ else if(value < 0.23810330555354434382) return 2;
 //~ else if(value < 0.43347012036670893362) return 3;
 //~ else if(value < 0.62883693517987352342) return 4;
 //~ else if(value < 0.78513038703040519526) return 5;
 //~ else if(value < 0.88932602159742630982) return 6;
 //~ else if(value < 0.94886638420715266099) return 7;
 //~ else if(value < 0.97863656551201583658) return 8;
 //~ else return 9;
//~ }

void findDistancesForCube(FLOAT_T *distanceArray, Point p, IntPoint c) {
  uint rngLast = rng( hash(c.x, c.y, c.z) );
  uint rng1,rng2,rng3;
  Point randomDiff,featurePoint;
  uint numFPoints = prob_lookup( rngLast );

  for(uint i = 0; i < numFPoints; ++i) {
	rng1 = rng(rngLast);
	rng2 = rng(rng1);
	rng3 = rng(rng2);
	rngLast = rng3;

	randomDiff.x = (float)rng1 / 0x100000000;
	randomDiff.y = (float)rng2 / 0x100000000;
	randomDiff.z = (float)rng3 / 0x100000000;
	featurePoint = randomDiff + convert_float4(c);

	insert(distanceArray, our_distance(p,featurePoint));
  }
}

void forAll(FLOAT_T *distanceArray, Point p) {
    for(int i=-1; i < 2; ++i) {
	for(int j=-1; j < 2; ++j) {
	    for(int k=-1; k < 2; ++k) {
		IntPoint cube;
		cube.x = p.x + i;
		cube.y = p.y + j;
		cube.z = p.z + k;
		findDistancesForCube(distanceArray, p, cube);
	    }
	}
    }
}

__kernel void WorleyNoise(__global FLOAT_T *arrX, __global FLOAT_T *arrY, __global FLOAT_T *arrZ, __global FLOAT_T *output) {//, int width, int height, int depth) {
    uint idX = get_global_id(0);
    uint idY = get_global_id(1);
    uint idZ = get_global_id(2);
    uint width = get_global_size(0);
    uint height = get_global_size(1);
    uint depth = get_global_size(2);

  // Shall we do work?
  if(idX < width && idY < height && idZ < depth) {
	// Initalize array
	FLOAT_T darr[N];
	for(int i=0; i<N; ++i)
	  darr[i] = 888;

	Point p;
	p.x = arrX[idX];
	p.y = arrY[idY];
	p.z = arrZ[idZ];
	forAll(darr,p);
	//findDistancesForCube(darr, p, convert_int4(p));

	output[depth*height*idX + depth*idY + idZ] = darr[1] - darr[0];
  }
}


added an option for precise calculation with the kahan summation algorithm

// -*- c++ -*-
// UjoImro, 2013
// Experimental code for the CARP Project
// Copyright (c) RealEyes, 2013
// This is a c-implementation of the PCA->MLP response map calculation

typedef struct {
    int rows;
    int cols;
    int step;
    int start;
} clMat; // struct clMatFloat

typedef struct {
    int size;
    int step;
    int start;
} clVector; // struct clVector

typedef struct {
    int m_patchSize;
    clMat /* uint8_t */ alignedImage;
    clMat /* int32_t */ shape;
//        clMat /* float */ m_wIn;
    clMat /* float */ m_wOut;
//        clMat /* float */ m_U;
    clMat /* float */ wIn;
    clMat /* float */ bIn;
} calcinput; // struct

typedef struct {
    int x;
    int y;
} Point2i; // struct Point2i

typedef struct {
    // temporaries
//    clVector /* clMat[] */ patches;
    clVector /* clMat[] */ xOuts;
//    clVector /* clMat[] */ es;
} calctemp; // struct

typedef struct {
    // results
    clMat /*float*/ responseMap;
} calcoutput;

typedef struct {
    calcinput  input;
    calctemp   tmp;
    calcoutput output;
} calcpackage;

typedef struct {
    float shift;
    float stride;
} normalization;

__constant const int MAX_INT = ~(1 << (8*sizeof(int) - 1));
// __constant const int false = (1!=1);
__constant const int gangsize = 128;
__constant const bool precise = false;

clMat GetMatFromVector( __local void * self, clVector /*clMat*/ vec, int idx )
{
//    assert(self);
//    assert(idx>=0);
//    assert(idx<vec.size);

    return ((__local clMat*)self)[ vec.start + vec.step * idx ];
} // GetMatFromVector

void SetMatToVector( __local void * self, clVector /*clMat*/ vec, int idx, clMat val )
{
//    assert(self);
//    assert(idx>=0);
//    assert(idx<vec.size);

    ((__local clMat*)self)[ vec.start + vec.step * idx ] = val;
    return;
} // GetMatFromVector

clMat /*uint8_t*/
GetBlockChar( __local void * self, clMat /*uint8_t*/  smat, int row_from, int row_to, int col_from, int col_to )
{
    // assert(row_from>=0);
    // assert(col_from>=0);
    // assert(row_from<row_to);
    // assert(col_from<col_to);
    // assert(row_to<=smat.rows);
    // assert(col_to<=smat.cols);

    clMat /*uchar*/  result;
    result.rows  = row_to - row_from;
    result.cols  = col_to - col_from;
    result.step  = smat.step;
    result.start = smat.start + row_from * smat.step + col_from;

    return result;
}

clMat
GetBlockFloat( __local void * self, clMat /*float*/smat, int row_from, int row_to, int col_from, int col_to )
{
    // assert(row_from>=0);
    // assert(col_from>=0);
    // assert(row_from<row_to);
    // assert(col_from<col_to);
    // assert(row_to<=smat.rows);
    // assert(col_to<=smat.cols);

    clMat /*float*/result;
    result.rows  = row_to - row_from;
    result.cols  = col_to - col_from;
    result.step  = smat.step;
    result.start = smat.start + row_from * smat.step + col_from;

    return result;
}

// returns alpha*A*B + beta * C
void
gemmFloatDirDirDir(
    __local void * self,
    clMat /*float*/A,
    clMat /*uint8_t*/B,
    float alpha,
    clMat /*float*/C,
    float beta,
    normalization norm,
    clMat /*float*/ result
    )
{
    // assert(A.rows == C.rows);
    // assert(A.cols == B.rows);
    // assert(B.cols == C.cols);
    // assert(C.rows == result.rows);
    // assert(C.cols == result.cols);

    int q, w, e;
    float sum=0.0f;
    float c=0.0f;

    for ( q=0; q<C.rows; q++ )
        for ( w=0; w<C.cols; w++ )
        {
            sum = 0;
            for ( e=0; e<A.cols; e++ )
            {
                float a = ((__local float*)self)[ q * A.step + e + A.start ];
                int b_row = e / B.rows;
                int b_col = e % B.cols;
                float b = ((__local uchar*)self)[ b_row * B.step + b_col + B.start ];
                if (precise) {
                    float y = a * (norm.shift + norm.stride * b) - c;
                    float t = sum + y;
                    c = (t - sum) - y;
                    sum = t;
                }
                else // NOT precise
                {
                    sum += a * (norm.shift + norm.stride * b);
                }
            }

            ((__local float*)self)[ q * result.step + w + result.start ] = alpha * sum  + beta * ((__local float*)self)[ q * C.step + w + C.start ];
        }

    return;
} // gemmFloatDirDirDir

void
activateOutput( __local void * self, clMat /*float*/input )
{
    int q, w;

    for ( q=0; q<input.rows; q++ )
        for ( w=0; w<input.cols; w++ ) {
            float val = ((__local float*)self)[ q * input.step + w + input.start ];
            float result = 2.0f/(exp(val) + 1) - 1;
            ((__local float*)self)[ q * input.step + w + input.start ] = result;
        }

    return;
} // activateOutput

float
GetValueFloat( __local void * self, clMat /*float*/smat, int row, int col )
{
    // assert(self);
    // assert(row >= 0);
    // assert(col >= 0);
    // assert(row < smat.rows);
    // assert(col < smat.cols);
    // assert(smat.step >= smat.cols);
    // assert(smat.step % smat.cols == 0);

    return ((__local float*)self)[ row * smat.step + col + smat.start ];
    // return -1231.;
}

uchar
GetValueChar( __local void * self, clMat /* uint8_t*/ smat, int row, int col )
{
    // assert(self);
    // assert(row >= 0);
    // assert(col >= 0);
    // assert(row < smat.rows);
    // assert(col < smat.cols);
    // assert(smat.step >= smat.cols);

    return ((__local uchar*)self)[ row * smat.step + col + smat.start ];
    // return -1231.;
}

void
SetValueFloat( __local void * self, clMat /*float*/ smat, int row, int col, float value )
{
    // assert(self);
    // assert(row >= 0);
    // assert(col >= 0);
    // assert(row < smat.rows);
    // assert(col < smat.cols);
    // assert(smat.step >= smat.cols);
    // assert(smat.step % smat.cols == 0);

    ((__local float*)self)[ row * smat.step + col + smat.start ] = value;
    return;
}

void
SetValueChar( __local void * self, clMat /*float*/ smat, int row, int col, uchar value )
{
    // assert(self);
    // assert(row >= 0);
    // assert(col >= 0);
    // assert(row < smat.rows);
    // assert(col < smat.cols);
    // assert(smat.step >= smat.cols);
    // assert(smat.step % smat.cols == 0);

    ((__local uchar*)self)[ row * smat.step + col + smat.start ] = value;
    return;
}

float dotProductDirDir( __local void * self, clMat /*float*/A, clMat /*float*/B )
{
  // assert( A.cols == 1 );
  // assert( B.cols == 1 );
  // assert( A.rows == B.rows );

  float result = 0.0f;
  float c = 0.0f;

  int q;
  for (q=0; q<A.rows; q++) {
      float y = ((__local float*)self)[ q * A.step + A.start ] * ((__local float*)self)[ q * B.step + B.start ] - c;
      float t = result + y;
      c = (t - result) - y;
      result = t ;
  }

  return result;
}

float dotProductTransDir( __local void * self, clMat /*float*/A, clMat /*float*/B )
{
  // assert( A.rows == 1 );
  // assert( B.cols == 1 );
  // assert( A.cols == B.rows );

  float result = 0.0f;
  float c = 0.0f;

  int q;
  for (q=0; q<A.cols; q++) {
      float y = ((__local float*)self)[ q + A.start ] * ((__local float*)self)[ q * B.step + B.start ] - c;
      float t = result + y;
      c = (t - result) - y;
      result = t ;
  }

  return result;
}

normalization
normalizeSample( __local void * self, clMat /*uint8_t*/  image ) // , clMat /*float*/ * result )
{
  // assert(result->cols == image.cols);
  // assert(result->rows == image.rows);

    int q,w;
    float sum=0.0f;
    float c = 0.0f; // kahan summation
    uchar minvalue = 255;
    uchar maxvalue = 0;

    for ( q=0; q<image.rows; q++ )
        for ( w=0; w<image.cols; w++ )
        {
	    uchar pixel = ((__local uchar*)self)[ q * image.step + w + image.start ];
	    minvalue = min( minvalue, pixel );
	    maxvalue = max( maxvalue, pixel );
            if (precise) {
                float y = pixel - c;
                float t = sum + y;
                c = (t - sum) - y;
                sum = t;
            }
            else
            {
                sum += pixel;
            }
        }

    normalization nresult;

    float sampleMean = sum / (image.rows * image.cols);
    float sampleMin  = minvalue;
    float sampleMax  = maxvalue;

    sampleMax -= sampleMean;
    sampleMin -= sampleMean;

    sampleMax = fmax( fabs(sampleMin), fabs(sampleMax));

    if (sampleMax == 0.0) sampleMax = 1.0;

    nresult.shift  = -(1.0/sampleMax)*sampleMean;
    nresult.stride = 1.0/sampleMax;

    return nresult;
} // normalizeSample

void
generateResponseMap(
    __local void * self,
    const clMat /*uint8_t*/  image,
    const Point2i center,
    int mapSize,
    int m_patchSize,
    // clMat /*float*/m_wIn,
    clMat /*float*/m_wOut,
    // clMat /*float*/m_U,
    clMat wIn,
    clMat bIn,

    // temporary variables
//    clVector /*cMat float*/ patches,
    clVector /*cMat float*/ xOuts,
//    clVector /*cMat float*/ es,
    // result
    clMat /*float*/ result
    )
{

  // assert( result.rows == 2 * mapSize + 1 );
  // assert( result.cols == 2 * mapSize + 1 );

  //  assert(wIn.rows == wIn_A.rows);
  //  assert(wIn.cols == m_U.rows);

    clMat /*float*/wOut_tmp = GetBlockFloat( self, m_wOut, 0, m_wOut.rows, 0, m_wOut.cols - 1 );

    {
        int localid = get_local_id(0);
//        for ( localid=0; localid<gangsize; localid++ ) {

            int work = 0;
            int worksize = (2 * mapSize + 1) * (2 * mapSize + 1);

            float bOut = GetValueFloat( self, m_wOut, 0, m_wOut.cols - 1 );

            for ( work=0; work < (worksize / gangsize) + 1; work++ )
            {
              int index = work * gangsize + localid;
              if (index>=worksize) continue;
              int ncy = index / (2 * mapSize + 1);
              int cy  = center.y - mapSize + ncy;
              int ncx = index % (2 * mapSize + 1);
              int cx  = center.x - mapSize + ncx;

              clMat /*uint8_t*/   imagePatch = GetBlockChar( self, image, cy - m_patchSize, cy + m_patchSize + 1, cx - m_patchSize, cx + m_patchSize + 1 );

              normalization norm = normalizeSample( self, imagePatch );

              clMat xOut = GetMatFromVector( self, xOuts, localid );

              gemmFloatDirDirDir( self, wIn, imagePatch, -1.0f, bIn, -1.0f, norm, xOut );

              activateOutput( self, xOut );

              SetValueFloat( self, result, ncy, ncx, 1.0f/( 1.0f + exp(- dotProductTransDir( self, wOut_tmp, xOut) - bOut ) ) );

            } // for gangsize
            //       } // for localid
    } // localid block

//  // assert(false);

  // freeMatFloat( self, allocator, &wIn);
  // end of classic impl
    return;
} // generateResponseMap

int
cvRound( float value )
{
    return (int)(value + (value >= 0 ? 0.5f : -0.5f));
} // cvRound

__kernel void
calculateMaps(
    // global variables
    __global void * self,
    __constant int memory_segments[], // representing the start of the gang's memory segment
    int m_visibleLandmarks_size,
    int m_mapSize,

    __constant calcpackage packages[],

    // local buffer
    int buffersize,
    __local void * buffer
    )
{

    int idx = get_group_id(0);
    int localid = get_local_id(0);

    // Copying the calculation into the local memory
    {
        __global int * glob = ((__global int*)(self + memory_segments[idx]));
        __local  int * loc  = ((__local  int*) buffer);

        int iter = 0;
        for ( iter = 0; iter < (buffersize/4) / gangsize + 1; iter++ ) {
            int index = gangsize * iter + localid;
            loc[index]=glob[index];
        }
    }
    barrier(CLK_LOCAL_MEM_FENCE);

    // Calculating
    Point2i center;

    float shape_x;
    float shape_y;
    calcpackage package = packages[idx];
    shape_x = GetValueFloat( buffer, package.input.shape, 2*idx, 0 );
    shape_y = GetValueFloat( buffer, package.input.shape, 2*idx+1, 0 );

    center.x = cvRound(shape_x);
    center.y = cvRound(shape_y);

    generateResponseMap(
        buffer,
        package.input.alignedImage,
        center,
        m_mapSize,
        package.input.m_patchSize,
        package.input.m_wOut,
        package.input.wIn,
        package.input.bIn,

        // temporary
        // package.tmp.patches,
        package.tmp.xOuts,
        // package.tmp.es,
        // result
        package.output.responseMap );

    barrier(CLK_LOCAL_MEM_FENCE);
    // Copying back the result into the global memory
    {
        int start = package.output.responseMap.start;
        int size  = package.output.responseMap.step * package.output.responseMap.rows;

        __global float * glob = ((__global float*)(self + memory_segments[idx]));
        __local  float * loc  = ((__local  float*) buffer);

        int iter = 0;
        for ( iter = 0; iter < size / gangsize + 1; iter++ ) {
            int index = start + gangsize * iter + localid;
            glob[index]=loc[index];
        }
    }

    return;
} // calculateMaps

// LuM end of file

// -*- c++ -*-
// UjoImro, 2013
// Experimental code for the CARP Project
// Copyright (c) RealEyes, 2013
// This is a c-implementation of the PCA->MLP response map calculation

typedef struct {
    int rows;
    int cols;
    int step;
    int start;
} clMat; // struct clMatFloat

typedef struct {
    int size;
    int step;
    int start;
} clVector; // struct clVector

typedef struct {
    int m_patchSize;
    clMat /* uint8_t */ alignedImage;
    clMat /* int32_t */ shape;
//        clMat /* float */ m_wIn;
    clMat /* float */ m_wOut;
//        clMat /* float */ m_U;
    clMat /* float */ wIn;
    clMat /* float */ bIn;
} calcinput; // struct

typedef struct {
    int x;
    int y;
} Point2i; // struct Point2i

typedef struct {
    // temporaries
//    clVector /* clMat[] */ patches;
    clVector /* clMat[] */ xOuts;
//    clVector /* clMat[] */ es;
} calctemp; // struct

typedef struct {
    // results
    clMat /*float*/ responseMap;
} calcoutput;

typedef struct {
    calcinput  input;
    calctemp   tmp;
    calcoutput output;
} calcpackage;

typedef struct {
    float shift;
    float stride;
} normalization;

__constant const int MAX_INT = ~(1 << (8*sizeof(int) - 1));
// __constant const int false = (1!=1);
__constant const int gangsize = 128;

clMat GetMatFromVector( __local void * self, clVector /*clMat*/ vec, int idx )
{
//    assert(self);
//    assert(idx>=0);
//    assert(idx<vec.size);

    return ((__local clMat*)self)[ vec.start + vec.step * idx ];
} // GetMatFromVector

void SetMatToVector( __local void * self, clVector /*clMat*/ vec, int idx, clMat val )
{
//    assert(self);
//    assert(idx>=0);
//    assert(idx<vec.size);

    ((__local clMat*)self)[ vec.start + vec.step * idx ] = val;
    return;
} // GetMatFromVector

clMat /*uint8_t*/
GetBlockChar( __local void * self, clMat /*uint8_t*/  smat, int row_from, int row_to, int col_from, int col_to )
{
    // assert(row_from>=0);
    // assert(col_from>=0);
    // assert(row_from<row_to);
    // assert(col_from<col_to);
    // assert(row_to<=smat.rows);
    // assert(col_to<=smat.cols);

    clMat /*uchar*/  result;
    result.rows  = row_to - row_from;
    result.cols  = col_to - col_from;
    result.step  = smat.step;
    result.start = smat.start + row_from * smat.step + col_from;

    return result;
}

clMat
GetBlockFloat( __local void * self, clMat /*float*/smat, int row_from, int row_to, int col_from, int col_to )
{
    // assert(row_from>=0);
    // assert(col_from>=0);
    // assert(row_from<row_to);
    // assert(col_from<col_to);
    // assert(row_to<=smat.rows);
    // assert(col_to<=smat.cols);

    clMat /*float*/result;
    result.rows  = row_to - row_from;
    result.cols  = col_to - col_from;
    result.step  = smat.step;
    result.start = smat.start + row_from * smat.step + col_from;

    return result;
}

// returns alpha*A*B + beta * C
void
gemmFloatDirDirDir(
    __local void * self,
    clMat /*float*/A,
    clMat /*uint8_t*/B,
    float alpha,
    clMat /*float*/C,
    float beta,
    normalization norm,
    clMat /*float*/ result
    )
{
    // assert(A.rows == C.rows);
    // assert(A.cols == B.rows);
    // assert(B.cols == C.cols);
    // assert(C.rows == result.rows);
    // assert(C.cols == result.cols);

    int q, w, e;
    float sum=0.0f;
    float c=0.0f;

    for ( q=0; q<C.rows; q++ )
        for ( w=0; w<C.cols; w++ )
        {
            sum = 0;
            for ( e=0; e<A.cols; e++ )
            {
                float a = ((__local float*)self)[ q * A.step + e + A.start ];
                int b_row = e / B.rows;
                int b_col = e % B.cols;
                float b = ((__local uchar*)self)[ b_row * B.step + b_col + B.start ];
                float y = a * (norm.shift + norm.stride * b) - c;
                float t = sum + y;
                c = (t - sum) - y;
                sum = t;
            }

            ((__local float*)self)[ q * result.step + w + result.start ] = alpha * sum  + beta * ((__local float*)self)[ q * C.step + w + C.start ];
        }

    return;
} // gemmFloatDirDirDir

void
activateOutput( __local void * self, clMat /*float*/input )
{
    int q, w;

    for ( q=0; q<input.rows; q++ )
        for ( w=0; w<input.cols; w++ ) {
            float val = ((__local float*)self)[ q * input.step + w + input.start ];
            float result = 2.0f/(exp(val) + 1) - 1;
            ((__local float*)self)[ q * input.step + w + input.start ] = result;
        }

    return;
} // activateOutput

float
GetValueFloat( __local void * self, clMat /*float*/smat, int row, int col )
{
    // assert(self);
    // assert(row >= 0);
    // assert(col >= 0);
    // assert(row < smat.rows);
    // assert(col < smat.cols);
    // assert(smat.step >= smat.cols);
    // assert(smat.step % smat.cols == 0);

    return ((__local float*)self)[ row * smat.step + col + smat.start ];
    // return -1231.;
}

uchar
GetValueChar( __local void * self, clMat /* uint8_t*/ smat, int row, int col )
{
    // assert(self);
    // assert(row >= 0);
    // assert(col >= 0);
    // assert(row < smat.rows);
    // assert(col < smat.cols);
    // assert(smat.step >= smat.cols);

    return ((__local uchar*)self)[ row * smat.step + col + smat.start ];
    // return -1231.;
}

void
SetValueFloat( __local void * self, clMat /*float*/ smat, int row, int col, float value )
{
    // assert(self);
    // assert(row >= 0);
    // assert(col >= 0);
    // assert(row < smat.rows);
    // assert(col < smat.cols);
    // assert(smat.step >= smat.cols);
    // assert(smat.step % smat.cols == 0);

    ((__local float*)self)[ row * smat.step + col + smat.start ] = value;
    return;
}

void
SetValueChar( __local void * self, clMat /*float*/ smat, int row, int col, uchar value )
{
    // assert(self);
    // assert(row >= 0);
    // assert(col >= 0);
    // assert(row < smat.rows);
    // assert(col < smat.cols);
    // assert(smat.step >= smat.cols);
    // assert(smat.step % smat.cols == 0);

    ((__local uchar*)self)[ row * smat.step + col + smat.start ] = value;
    return;
}

float dotProductDirDir( __local void * self, clMat /*float*/A, clMat /*float*/B )
{
  // assert( A.cols == 1 );
  // assert( B.cols == 1 );
  // assert( A.rows == B.rows );

  float result = 0.0f;
  float c = 0.0f;

  int q;
  for (q=0; q<A.rows; q++) {
      float y = ((__local float*)self)[ q * A.step + A.start ] * ((__local float*)self)[ q * B.step + B.start ] - c;
      float t = result + y;
      c = (t - result) - y;
      result = t ;
  }

  return result;
}

float dotProductTransDir( __local void * self, clMat /*float*/A, clMat /*float*/B )
{
  // assert( A.rows == 1 );
  // assert( B.cols == 1 );
  // assert( A.cols == B.rows );

  float result = 0.0f;
  float c = 0.0f;

  int q;
  for (q=0; q<A.cols; q++) {
      float y = ((__local float*)self)[ q + A.start ] * ((__local float*)self)[ q * B.step + B.start ] - c;
      float t = result + y;
      c = (t - result) - y;
      result = t ;
  }

  return result;
}

normalization
normalizeSample( __local void * self, clMat /*uint8_t*/  image ) // , clMat /*float*/ * result )
{
  // assert(result->cols == image.cols);
  // assert(result->rows == image.rows);

    int q,w;
    float sum=0.0f;
    float c = 0.0f; // kahan summation
    uchar minvalue = 255;
    uchar maxvalue = 0;

    for ( q=0; q<image.rows; q++ )
        for ( w=0; w<image.cols; w++ )
        {
	    uchar pixel = ((__local uchar*)self)[ q * image.step + w + image.start ];
	    minvalue = min( minvalue, pixel );
	    maxvalue = max( maxvalue, pixel );
            float y = pixel - c;
            float t = sum + y;
            c = (t - sum) - y;
            sum = t;
        }

    normalization nresult;

    float sampleMean = sum / (image.rows * image.cols);
    float sampleMin  = minvalue;
    float sampleMax  = maxvalue;

    sampleMax -= sampleMean;
    sampleMin -= sampleMean;

    sampleMax = fmax( fabs(sampleMin), fabs(sampleMax));

    if (sampleMax == 0.0) sampleMax = 1.0;

    nresult.shift  = -(1.0/sampleMax)*sampleMean;
    nresult.stride = 1.0/sampleMax;

    return nresult;
} // normalizeSample

void
generateResponseMap(
    __local void * self,
    const clMat /*uint8_t*/  image,
    const Point2i center,
    int mapSize,
    int m_patchSize,
    // clMat /*float*/m_wIn,
    clMat /*float*/m_wOut,
    // clMat /*float*/m_U,
    clMat wIn,
    clMat bIn,

    // temporary variables
//    clVector /*cMat float*/ patches,
    clVector /*cMat float*/ xOuts,
//    clVector /*cMat float*/ es,
    // result
    clMat /*float*/ result
    )
{

  // assert( result.rows == 2 * mapSize + 1 );
  // assert( result.cols == 2 * mapSize + 1 );

  //  assert(wIn.rows == wIn_A.rows);
  //  assert(wIn.cols == m_U.rows);

    clMat /*float*/wOut_tmp = GetBlockFloat( self, m_wOut, 0, m_wOut.rows, 0, m_wOut.cols - 1 );

    {
        int localid = get_local_id(0);
//        for ( localid=0; localid<gangsize; localid++ ) {

            int work = 0;
            int worksize = (2 * mapSize + 1) * (2 * mapSize + 1);

            float bOut = GetValueFloat( self, m_wOut, 0, m_wOut.cols - 1 );

            for ( work=0; work < (worksize / gangsize) + 1; work++ )
            {
              int index = work * gangsize + localid;
              if (index>=worksize) continue;
              int ncy = index / (2 * mapSize + 1);
              int cy  = center.y - mapSize + ncy;
              int ncx = index % (2 * mapSize + 1);
              int cx  = center.x - mapSize + ncx;

              clMat /*uint8_t*/   imagePatch = GetBlockChar( self, image, cy - m_patchSize, cy + m_patchSize + 1, cx - m_patchSize, cx + m_patchSize + 1 );

              normalization norm = normalizeSample( self, imagePatch );

              clMat xOut = GetMatFromVector( self, xOuts, localid );

              gemmFloatDirDirDir( self, wIn, imagePatch, -1.0f, bIn, -1.0f, norm, xOut );

              activateOutput( self, xOut );

              SetValueFloat( self, result, ncy, ncx, 1.0f/( 1.0f + exp(- dotProductTransDir( self, wOut_tmp, xOut) - bOut ) ) );

            } // for gangsize
            //       } // for localid
    } // localid block

//  // assert(false);

  // freeMatFloat( self, allocator, &wIn);
  // end of classic impl
    return;
} // generateResponseMap

int
cvRound( float value )
{
    return (int)(value + (value >= 0 ? 0.5f : -0.5f));
} // cvRound

__kernel void
calculateMaps(
    // global variables
    __global void * self,
    __constant int memory_segments[], // representing the start of the gang's memory segment
    int m_visibleLandmarks_size,
    int m_mapSize,

    __constant calcpackage packages[],

    // local buffer
    int buffersize,
    __local void * buffer
    )
{

    int idx = get_group_id(0);
    int localid = get_local_id(0);

    // Copying the calculation into the local memory
    {
        __global int * glob = ((__global int*)(self + memory_segments[idx]));
        __local  int * loc  = ((__local  int*) buffer);

        int iter = 0;
        for ( iter = 0; iter < (buffersize/4) / gangsize + 1; iter++ ) {
            int index = gangsize * iter + localid;
            loc[index]=glob[index];
        }
    }
    barrier(CLK_LOCAL_MEM_FENCE);

    // Calculating
    Point2i center;

    float shape_x;
    float shape_y;
    calcpackage package = packages[idx];
    shape_x = GetValueFloat( buffer, package.input.shape, 2*idx, 0 );
    shape_y = GetValueFloat( buffer, package.input.shape, 2*idx+1, 0 );

    center.x = cvRound(shape_x);
    center.y = cvRound(shape_y);

    generateResponseMap(
        buffer,
        package.input.alignedImage,
        center,
        m_mapSize,
        package.input.m_patchSize,
        package.input.m_wOut,
        package.input.wIn,
        package.input.bIn,

        // temporary
        // package.tmp.patches,
        package.tmp.xOuts,
        // package.tmp.es,
        // result
        package.output.responseMap );

    barrier(CLK_LOCAL_MEM_FENCE);
    // Copying back the result into the global memory
    {
        int start = package.output.responseMap.start;
        int size  = package.output.responseMap.step * package.output.responseMap.rows;

        __global float * glob = ((__global float*)(self + memory_segments[idx]));
        __local  float * loc  = ((__local  float*) buffer);

        int iter = 0;
        for ( iter = 0; iter < size / gangsize + 1; iter++ ) {
            int index = start + gangsize * iter + localid;
            glob[index]=loc[index];
        }
    }

    return;
} // calculateMaps

// LuM end of file


remove local caching

remove local caching

#ifdef HALF_AS_FLOAT
#define half float
#define half2 float2
#define half3 float3
#define half4 float4
#else
#pragma OPENCL EXTENSION cl_khr_fp16 : enable
#endif

#define UV_WIDTH RGB_WIDTH / 2
#define UV_HEIGHT RGB_HEIGHT / 2
#define U_OFFSET RGB_WIDTH * RGB_HEIGHT
#define V_OFFSET RGB_WIDTH * RGB_HEIGHT + UV_WIDTH * UV_HEIGHT

#define RGB_TO_Y(r, g, b) ((((mul24(b, 13) + mul24(g, 65) + mul24(r, 33)) + 64) >> 7) + 16)
#define RGB_TO_U(r, g, b) ((mul24(b, 56) - mul24(g, 37) - mul24(r, 19) + 0x8080) >> 8)
#define RGB_TO_V(r, g, b) ((mul24(r, 56) - mul24(g, 47) - mul24(b, 9) + 0x8080) >> 8)
#define AVERAGE(x, y, z, w) ((convert_ushort(x) + convert_ushort(y) + convert_ushort(z) + convert_ushort(w) + 1) >> 1)

// post wb CCM
const __constant half3 color_correction_0 = (half3)(1.82717181, -0.31231438, 0.07307673);
const __constant half3 color_correction_1 = (half3)(-0.5743977, 1.36858544, -0.53183455);
const __constant half3 color_correction_2 = (half3)(-0.25277411, -0.05627105, 1.45875782);

// tone mapping params
const half gamma_k = 0.75;
const half gamma_b = 0.125;
const half mp = 0.01; // ideally midpoint should be adaptive
const half rk = 9 - 100*mp;

inline half3 gamma_apply(half3 x) {
  // poly approximation for s curve
  return (x > mp) ?
    ((rk * (x-mp) * (1-(gamma_k*mp+gamma_b)) * (1+1/(rk*(1-mp))) / (1+rk*(x-mp))) + gamma_k*mp + gamma_b) :
    ((rk * (x-mp) * (gamma_k*mp+gamma_b) * (1+1/(rk*mp)) / (1-rk*(x-mp))) + gamma_k*mp + gamma_b);
}

inline half3 color_correct(half3 rgb) {
  half3 ret = (half)rgb.x * color_correction_0;
  ret += (half)rgb.y * color_correction_1;
  ret += (half)rgb.z * color_correction_2;
  return gamma_apply(ret);
}

inline half get_vignetting_s(float r) {
  if (r < 62500) {
    return (half)(1.0f + 0.0000008f*r);
  } else if (r < 490000) {
    return (half)(0.9625f + 0.0000014f*r);
  } else if (r < 1102500) {
    return (half)(1.26434f + 0.0000000000016f*r*r);
  } else {
    return (half)(0.53503625f + 0.0000000000022f*r*r);
  }
}

inline half val_from_10(const uchar * source, int gx, int gy, half black_level) {
  // parse 12bit
  int start = gy * FRAME_STRIDE + (3 * (gx / 2)) + (FRAME_STRIDE * FRAME_OFFSET);
  int offset = gx % 2;
  uint major = (uint)source[start + offset] << 4;
  uint minor = (source[start + 2] >> (4 * offset)) & 0xf;
  half pv = ((half)(major + minor)) / 4.0;

  // normalize
  pv = max((half)0.0, pv - black_level);
  pv /= (1024.0 - black_level);

  // correct vignetting
  if (CAM_NUM == 1) { // fcamera
    gx = (gx - RGB_WIDTH/2);
    gy = (gy - RGB_HEIGHT/2);
    pv *= get_vignetting_s(gx*gx + gy*gy);
  }

  pv = clamp(pv, (half)0.0, (half)1.0);
  return pv;
}

inline half get_k(half a, half b, half c, half d) {
  return 2.0 - (fabs(a - b) + fabs(c - d));
}

__kernel void debayer10(const __global uchar * in,
                        __global uchar * out,
                        float black_level
                       )
{
  const int gid_x = get_global_id(0);
  const int gid_y = get_global_id(1);

  half3 rgb;
  uchar3 rgb_out[4];

  half4 va, vb, vc, vd;

  va.s0 = val_from_10(in, gid_x*2-1, gid_y*2-1, black_level);
  va.s1 = val_from_10(in, gid_x*2+0, gid_y*2-1, black_level);
  va.s2 = val_from_10(in, gid_x*2+1, gid_y*2-1, black_level);
  va.s3 = val_from_10(in, gid_x*2+2, gid_y*2-1, black_level);

  vb.s0 = val_from_10(in, gid_x*2-1, gid_y*2+0, black_level);
  vb.s1 = val_from_10(in, gid_x*2+0, gid_y*2+0, black_level); // G(R)
  vb.s2 = val_from_10(in, gid_x*2+1, gid_y*2+0, black_level); // R
  vb.s3 = val_from_10(in, gid_x*2+2, gid_y*2+0, black_level);

  vc.s0 = val_from_10(in, gid_x*2-1, gid_y*2+1, black_level);
  vc.s1 = val_from_10(in, gid_x*2+0, gid_y*2+1, black_level); // B
  vc.s2 = val_from_10(in, gid_x*2+1, gid_y*2+1, black_level); // G(B)
  vc.s3 = val_from_10(in, gid_x*2+2, gid_y*2+1, black_level);

  vd.s0 = val_from_10(in, gid_x*2-1, gid_y*2+2, black_level);
  vd.s1 = val_from_10(in, gid_x*2+0, gid_y*2+2, black_level);
  vd.s2 = val_from_10(in, gid_x*2+1, gid_y*2+2, black_level);
  vd.s3 = val_from_10(in, gid_x*2+2, gid_y*2+2, black_level);

  // a simplified version of https://opensignalprocessingjournal.com/contents/volumes/V6/TOSIGPJ-6-1/TOSIGPJ-6-1.pdf
  const half k01 = get_k(va.s0, vb.s1, va.s2, vb.s1);
  const half k02 = get_k(va.s2, vb.s1, vc.s2, vb.s1);
  const half k03 = get_k(vc.s0, vb.s1, vc.s2, vb.s1);
  const half k04 = get_k(va.s0, vb.s1, vc.s0, vb.s1);
  rgb.x = (k02*vb.s2+k04*vb.s0)/(k02+k04); // R_G1
  rgb.y = vb.s1; // G1(R)
  rgb.z = (k01*va.s1+k03*vc.s1)/(k01+k03); // B_G1
  rgb_out[0] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0);

  const half k11 = get_k(va.s1, vc.s1, va.s3, vc.s3);
  const half k12 = get_k(va.s2, vb.s1, vb.s3, vc.s2);
  const half k13 = get_k(va.s1, va.s3, vc.s1, vc.s3);
  const half k14 = get_k(va.s2, vb.s3, vc.s2, vb.s1);
  rgb.x = vb.s2; // R
  rgb.y = (k11*(va.s2+vc.s2)*0.5+k13*(vb.s3+vb.s1)*0.5)/(k11+k13); // G_R
  rgb.z = (k12*(va.s3+vc.s1)*0.5+k14*(va.s1+vc.s3)*0.5)/(k12+k14); // B_R
  rgb_out[1] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0);

  const half k21 = get_k(vb.s0, vd.s0, vb.s2, vd.s2);
  const half k22 = get_k(vb.s1, vc.s0, vc.s2, vd.s1);
  const half k23 = get_k(vb.s0, vb.s2, vd.s0, vd.s2);
  const half k24 = get_k(vb.s1, vc.s2, vd.s1, vc.s0);
  rgb.x = (k22*(vb.s2+vd.s0)*0.5+k24*(vb.s0+vd.s2)*0.5)/(k22+k24); // R_B
  rgb.y = (k21*(vb.s1+vd.s1)*0.5+k23*(vc.s2+vc.s0)*0.5)/(k21+k23); // G_B
  rgb.z = vc.s1; // B
  rgb_out[2] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0);

  const half k31 = get_k(vb.s1, vc.s2, vb.s3, vc.s2);
  const half k32 = get_k(vb.s3, vc.s2, vd.s3, vc.s2);
  const half k33 = get_k(vd.s1, vc.s2, vd.s3, vc.s2);
  const half k34 = get_k(vb.s1, vc.s2, vd.s1, vc.s2);
  rgb.x = (k31*vb.s2+k33*vd.s2)/(k31+k33); // R_G2
  rgb.y = vc.s2; // G2(B)
  rgb.z = (k32*vc.s3+k34*vc.s1)/(k32+k34); // B_G2
  rgb_out[3] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0);

  // write ys
  uchar2 yy = (uchar2)(
    RGB_TO_Y(rgb_out[0].s0, rgb_out[0].s1, rgb_out[0].s2),
    RGB_TO_Y(rgb_out[1].s0, rgb_out[1].s1, rgb_out[1].s2)
  );
  vstore2(yy, 0, out + mad24(gid_y * 2, RGB_WIDTH, gid_x * 2));
  yy = (uchar2)(
    RGB_TO_Y(rgb_out[2].s0, rgb_out[2].s1, rgb_out[2].s2),
    RGB_TO_Y(rgb_out[3].s0, rgb_out[3].s1, rgb_out[3].s2)
  );
  vstore2(yy, 0, out + mad24(gid_y * 2 + 1, RGB_WIDTH, gid_x * 2));

  // write uvs
  const short ar = AVERAGE(rgb_out[0].s0, rgb_out[1].s0, rgb_out[2].s0, rgb_out[3].s0);
  const short ag = AVERAGE(rgb_out[0].s1, rgb_out[1].s1, rgb_out[2].s1, rgb_out[3].s1);
  const short ab = AVERAGE(rgb_out[0].s2, rgb_out[1].s2, rgb_out[2].s2, rgb_out[3].s2);
  out[U_OFFSET + mad24(gid_y, UV_WIDTH, gid_x)] = RGB_TO_U(ar, ag, ab);
  out[V_OFFSET + mad24(gid_y, UV_WIDTH, gid_x)] = RGB_TO_V(ar, ag, ab);
}

#ifdef HALF_AS_FLOAT
#define half float
#define half2 float2
#define half3 float3
#define half4 float4
#else
#pragma OPENCL EXTENSION cl_khr_fp16 : enable
#endif

#define UV_WIDTH RGB_WIDTH / 2
#define UV_HEIGHT RGB_HEIGHT / 2
#define U_OFFSET RGB_WIDTH * RGB_HEIGHT
#define V_OFFSET RGB_WIDTH * RGB_HEIGHT + UV_WIDTH * UV_HEIGHT

#define RGB_TO_Y(r, g, b) ((((mul24(b, 13) + mul24(g, 65) + mul24(r, 33)) + 64) >> 7) + 16)
#define RGB_TO_U(r, g, b) ((mul24(b, 56) - mul24(g, 37) - mul24(r, 19) + 0x8080) >> 8)
#define RGB_TO_V(r, g, b) ((mul24(r, 56) - mul24(g, 47) - mul24(b, 9) + 0x8080) >> 8)
#define AVERAGE(x, y, z, w) ((convert_ushort(x) + convert_ushort(y) + convert_ushort(z) + convert_ushort(w) + 1) >> 1)

// post wb CCM
const __constant half3 color_correction_0 = (half3)(1.82717181, -0.31231438, 0.07307673);
const __constant half3 color_correction_1 = (half3)(-0.5743977, 1.36858544, -0.53183455);
const __constant half3 color_correction_2 = (half3)(-0.25277411, -0.05627105, 1.45875782);

// tone mapping params
const half gamma_k = 0.75;
const half gamma_b = 0.125;
const half mp = 0.01; // ideally midpoint should be adaptive
const half rk = 9 - 100*mp;

inline half3 gamma_apply(half3 x) {
  // poly approximation for s curve
  return (x > mp) ?
    ((rk * (x-mp) * (1-(gamma_k*mp+gamma_b)) * (1+1/(rk*(1-mp))) / (1+rk*(x-mp))) + gamma_k*mp + gamma_b) :
    ((rk * (x-mp) * (gamma_k*mp+gamma_b) * (1+1/(rk*mp)) / (1-rk*(x-mp))) + gamma_k*mp + gamma_b);
}

inline half3 color_correct(half3 rgb) {
  half3 ret = (half)rgb.x * color_correction_0;
  ret += (half)rgb.y * color_correction_1;
  ret += (half)rgb.z * color_correction_2;
  return gamma_apply(ret);
}

inline half get_vignetting_s(float r) {
  if (r < 62500) {
    return (half)(1.0f + 0.0000008f*r);
  } else if (r < 490000) {
    return (half)(0.9625f + 0.0000014f*r);
  } else if (r < 1102500) {
    return (half)(1.26434f + 0.0000000000016f*r*r);
  } else {
    return (half)(0.53503625f + 0.0000000000022f*r*r);
  }
}

inline half val_from_10(const uchar * source, int gx, int gy, half black_level) {
  // parse 12bit
  int start = gy * FRAME_STRIDE + (3 * (gx / 2)) + (FRAME_STRIDE * FRAME_OFFSET);
  int offset = gx % 2;
  uint major = (uint)source[start + offset] << 4;
  uint minor = (source[start + 2] >> (4 * offset)) & 0xf;
  half pv = ((half)(major + minor)) / 4.0;

  // normalize
  pv = max((half)0.0, pv - black_level);
  pv /= (1024.0 - black_level);

  // correct vignetting
  if (CAM_NUM == 1) { // fcamera
    gx = (gx - RGB_WIDTH/2);
    gy = (gy - RGB_HEIGHT/2);
    pv *= get_vignetting_s(gx*gx + gy*gy);
  }

  pv = clamp(pv, (half)0.0, (half)1.0);
  return pv;
}

inline half get_k(half a, half b, half c, half d) {
  return 2.0 - (fabs(a - b) + fabs(c - d));
}

__kernel void debayer10(const __global uchar * in,
                        __global uchar * out,
                        __local half * cached,
                        float black_level
                       )
{
  const int gid_x = get_global_id(0);
  const int gid_y = get_global_id(1);

  const int lid_x = get_local_id(0);
  const int lid_y = get_local_id(1);

  const int localRowLen = mad24(get_local_size(0), 2, 2); // 2 padding
  const int localColLen = mad24(get_local_size(1), 2, 2);

  const int x_global = mul24(gid_x, 2);
  const int y_global = mul24(gid_y, 2);

  const int x_local = mad24(lid_x, 2, 1);
  const int y_local = mad24(lid_y, 2, 1);

  const int x_global_mod = (gid_x == 0 || gid_x == get_global_size(0) - 1) ? -1: 1;
  const int y_global_mod = (gid_y == 0 || gid_y == get_global_size(1) - 1) ? -1: 1;

  int localColOffset = 0;
  int globalColOffset;

  cached[mad24(y_local + 0, localRowLen, x_local + 0)] = val_from_10(in, x_global + 0, y_global + 0, black_level);
  cached[mad24(y_local + 0, localRowLen, x_local + 1)] = val_from_10(in, x_global + 1, y_global + 0, black_level);
  cached[mad24(y_local + 1, localRowLen, x_local + 0)] = val_from_10(in, x_global + 0, y_global + 1, black_level);
  cached[mad24(y_local + 1, localRowLen, x_local + 1)] = val_from_10(in, x_global + 1, y_global + 1, black_level);

  if (lid_x == 0) {  // left edge
    localColOffset = -1;
    globalColOffset = -x_global_mod;
    cached[mad24(y_local + 0, localRowLen, x_local - 1)] = val_from_10(in, x_global - x_global_mod, y_global + 0, black_level);
    cached[mad24(y_local + 1, localRowLen, x_local - 1)] = val_from_10(in, x_global - x_global_mod, y_global + 1, black_level);
  } else if (lid_x == get_local_size(0) - 1) {  // right edge
    localColOffset = 2;
    globalColOffset = x_global_mod + 1;
    cached[mad24(y_local + 0, localRowLen, x_local + 2)] = val_from_10(in, x_global + x_global_mod + 1, y_global + 0, black_level);
    cached[mad24(y_local + 1, localRowLen, x_local + 2)] = val_from_10(in, x_global + x_global_mod + 1, y_global + 1, black_level);
  }

  if (lid_y == 0) {  // top row
    cached[mad24(y_local - 1, localRowLen, x_local + 0)] = val_from_10(in, x_global + 0, y_global - y_global_mod, black_level);
    cached[mad24(y_local - 1, localRowLen, x_local + 1)] = val_from_10(in, x_global + 1, y_global - y_global_mod, black_level);
    if (localColOffset != 0) {  // cache corners
      cached[mad24(y_local - 1, localRowLen, x_local + localColOffset)] = val_from_10(in, x_global + globalColOffset, y_global - y_global_mod, black_level);
    }
  } else if (lid_y == get_local_size(1) - 1) {  // bottom row
    cached[mad24(y_local + 2, localRowLen, x_local + 0)] = val_from_10(in, x_global + 0, y_global + y_global_mod + 1, black_level);
    cached[mad24(y_local + 2, localRowLen, x_local + 1)] = val_from_10(in, x_global + 1, y_global + y_global_mod + 1, black_level);
    if (localColOffset != 0) {  // cache corners
      cached[mad24(y_local + 2, localRowLen, x_local + localColOffset)] = val_from_10(in, x_global + globalColOffset, y_global + y_global_mod + 1, black_level);
    }
  }

  // sync
  barrier(CLK_LOCAL_MEM_FENCE);

  half3 rgb;
  uchar3 rgb_out[4];

  const half4 va = vload4(0, cached + mad24(lid_y * 2 + 0, localRowLen, lid_x * 2));
  const half4 vb = vload4(0, cached + mad24(lid_y * 2 + 1, localRowLen, lid_x * 2));
  const half4 vc = vload4(0, cached + mad24(lid_y * 2 + 2, localRowLen, lid_x * 2));
  const half4 vd = vload4(0, cached + mad24(lid_y * 2 + 3, localRowLen, lid_x * 2));

  // a simplified version of https://opensignalprocessingjournal.com/contents/volumes/V6/TOSIGPJ-6-1/TOSIGPJ-6-1.pdf
  const half k01 = get_k(va.s0, vb.s1, va.s2, vb.s1);
  const half k02 = get_k(va.s2, vb.s1, vc.s2, vb.s1);
  const half k03 = get_k(vc.s0, vb.s1, vc.s2, vb.s1);
  const half k04 = get_k(va.s0, vb.s1, vc.s0, vb.s1);
  rgb.x = (k02*vb.s2+k04*vb.s0)/(k02+k04); // R_G1
  rgb.y = vb.s1; // G1(R)
  rgb.z = (k01*va.s1+k03*vc.s1)/(k01+k03); // B_G1
  rgb_out[0] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0);

  const half k11 = get_k(va.s1, vc.s1, va.s3, vc.s3);
  const half k12 = get_k(va.s2, vb.s1, vb.s3, vc.s2);
  const half k13 = get_k(va.s1, va.s3, vc.s1, vc.s3);
  const half k14 = get_k(va.s2, vb.s3, vc.s2, vb.s1);
  rgb.x = vb.s2; // R
  rgb.y = (k11*(va.s2+vc.s2)*0.5+k13*(vb.s3+vb.s1)*0.5)/(k11+k13); // G_R
  rgb.z = (k12*(va.s3+vc.s1)*0.5+k14*(va.s1+vc.s3)*0.5)/(k12+k14); // B_R
  rgb_out[1] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0);

  const half k21 = get_k(vb.s0, vd.s0, vb.s2, vd.s2);
  const half k22 = get_k(vb.s1, vc.s0, vc.s2, vd.s1);
  const half k23 = get_k(vb.s0, vb.s2, vd.s0, vd.s2);
  const half k24 = get_k(vb.s1, vc.s2, vd.s1, vc.s0);
  rgb.x = (k22*(vb.s2+vd.s0)*0.5+k24*(vb.s0+vd.s2)*0.5)/(k22+k24); // R_B
  rgb.y = (k21*(vb.s1+vd.s1)*0.5+k23*(vc.s2+vc.s0)*0.5)/(k21+k23); // G_B
  rgb.z = vc.s1; // B
  rgb_out[2] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0);

  const half k31 = get_k(vb.s1, vc.s2, vb.s3, vc.s2);
  const half k32 = get_k(vb.s3, vc.s2, vd.s3, vc.s2);
  const half k33 = get_k(vd.s1, vc.s2, vd.s3, vc.s2);
  const half k34 = get_k(vb.s1, vc.s2, vd.s1, vc.s2);
  rgb.x = (k31*vb.s2+k33*vd.s2)/(k31+k33); // R_G2
  rgb.y = vc.s2; // G2(B)
  rgb.z = (k32*vc.s3+k34*vc.s1)/(k32+k34); // B_G2
  rgb_out[3] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0);

  // write ys
  uchar2 yy = (uchar2)(
    RGB_TO_Y(rgb_out[0].s0, rgb_out[0].s1, rgb_out[0].s2),
    RGB_TO_Y(rgb_out[1].s0, rgb_out[1].s1, rgb_out[1].s2)
  );
  vstore2(yy, 0, out + mad24(gid_y * 2, RGB_WIDTH, gid_x * 2));
  yy = (uchar2)(
    RGB_TO_Y(rgb_out[2].s0, rgb_out[2].s1, rgb_out[2].s2),
    RGB_TO_Y(rgb_out[3].s0, rgb_out[3].s1, rgb_out[3].s2)
  );
  vstore2(yy, 0, out + mad24(gid_y * 2 + 1, RGB_WIDTH, gid_x * 2));

  // write uvs
  const short ar = AVERAGE(rgb_out[0].s0, rgb_out[1].s0, rgb_out[2].s0, rgb_out[3].s0);
  const short ag = AVERAGE(rgb_out[0].s1, rgb_out[1].s1, rgb_out[2].s1, rgb_out[3].s1);
  const short ab = AVERAGE(rgb_out[0].s2, rgb_out[1].s2, rgb_out[2].s2, rgb_out[3].s2);
  out[U_OFFSET + mad24(gid_y, UV_WIDTH, gid_x)] = RGB_TO_U(ar, ag, ab);
  out[V_OFFSET + mad24(gid_y, UV_WIDTH, gid_x)] = RGB_TO_V(ar, ag, ab);
}