
float readBuffer(__global float* buf, int2 pos, const int nRows, const int nCols);
void writeBuffer(__global float* buf, float writeVal, int2 pos, const int nRows, const int nCols);
void select_nbors (const float angle, int2* nbor1, int2* nbor2, int2* nbor3, float* locStep, float2* locPos);
float2 get_gradient (__global float* xGradImg, __global float* yGradImg, const int2 pos, const int2 offset, const int nRows, const int nCols);
float compute_interpolated_directional_derivative(float2 grad, float2 nbor1Grad, float2 nbor2Grad, float2 nbor3Grad, float locStep);

float readBuffer(__global float* buf, int2 pos, const int nRows, const int nCols)
{
    //access buffer in row-major format
    return buf[pos.x + nCols*pos.y];
}

void writeBuffer(__global float* buf, float writeVal, int2 pos, const int nRows, const int nCols)
{
    //write to buffer in row-major format
    buf[pos.x + nCols*pos.y] = writeVal;
}


void select_nbors (const float angle, int2* nbor1, int2* nbor2, int2* nbor3, float* locStep, float2* locPos)
{
    //find relevant neighbours and new location for gradient value in direction of center gradient
    //note: (**algorithm designed with +y = -row; requires flip to -ve**)
    //input angle in radians/pi
    if( angle >= -1.0f && angle < -0.75f )
    {
        nbor1->xy = (int2)(-1, -1);
        nbor2->xy = (int2)(-1,  0);
        nbor3->xy = (int2)(-1,  1);
        
        *locStep   = tanpi(angle);
        locPos->xy = (float2)( -1.0f, *locStep );
        
    }
    else if( angle >= -0.75f && angle < -0.5f )
    {
        nbor1->xy = (int2)( 1,  1);
        nbor2->xy = (int2)( 0,  1);
        nbor3->xy = (int2)(-1,  1);
        
        *locStep    = 1/tanpi(angle);
        locPos->xy = (float2)( -(*locStep), 1 );
    }
    else if( angle >= -0.5f && angle < -0.25f )
    {
        nbor1->xy = (int2)( 1,  1);
        nbor2->xy = (int2)( 0,  1);
        nbor3->xy = (int2)(-1,  1);
        
        *locStep    = 1/tanpi(angle);
        locPos->xy = (float2)( -(*locStep), 1 );
    }
    else if( angle >= -0.25f && angle < 0.0f )
    {
        nbor1->xy = (int2)( 1,  1);
        nbor2->xy = (int2)( 1,  0);
        nbor3->xy = (int2)( 1, -1);
        
        *locStep    = tanpi(angle);
        locPos->xy = (float2)(1, -(*locStep));
    }
    else if( angle >= 0.0f && angle < 0.25f )
    {
        nbor1->xy = (int2)( 1,  1);
        nbor2->xy = (int2)( 1,  0);
        nbor3->xy = (int2)( 1, -1);
        
        *locStep    = tanpi(angle);
        locPos->xy = (float2)(1, -(*locStep));
    }
    else if( angle >= 0.25f && angle < 0.5f )
    {
        nbor1->xy = (int2)( -1, -1);
        nbor2->xy = (int2)(  0, -1);
        nbor3->xy = (int2)(  1, -1);
        
        *locStep    = 1/tanpi(angle);
        locPos->xy = (float2)( *locStep, -1 );
    }
    else if( angle >= 0.5f && angle < 0.75f )
    {
        nbor1->xy = (int2)( -1, -1);
        nbor2->xy = (int2)(  0, -1);
        nbor3->xy = (int2)(  1, -1);
        
        *locStep    = 1/tanpi(angle);
        locPos->xy = (float2)( *locStep, -1 );
    }
    else if( angle >= 0.75f && angle <= 1.0f )
    {
        nbor1->xy = (int2)(-1, -1);
        nbor2->xy = (int2)(-1,  0);
        nbor3->xy = (int2)(-1,  1);
        
        *locStep   = tanpi(angle);
        locPos->xy = (float2)( -1.0f, *locStep );
    }
    else
    {
        nbor1->xy = (int2)(0, 0);
        nbor2->xy = (int2)(0, 0);
        nbor3->xy = (int2)(0, 0);
        
        *locStep   = 0.0f;
        locPos->xy = (float2)( 0.0f, 0.0f );

    }
}

float2 get_gradient (  __global float* xGradImg
                     , __global float* yGradImg
                     , const int2 pos, const int2 offset, const int nRows, const int nCols)
{
    int2  newPos;
    float2  grad;
    float  xGrad;
    float  yGrad;
    
    newPos = pos + offset;
    
    if( newPos.x > nCols-1 )
    {
        newPos.x = nCols-1;
    }
    else if( newPos.x < 0 )
    {
        newPos.x = 0;
    }
    
    if( newPos.y > nRows-1 )
    {
        newPos.y = nRows-1;
    }
    else if( newPos.y < 0 )
    {
        newPos.y = 0;
    }
    
    xGrad   = readBuffer(xGradImg, newPos, nRows, nCols);
    yGrad   = readBuffer(yGradImg, newPos, nRows, nCols);
    grad.xy = (float2)(xGrad, yGrad);
    
    return grad;
}

float compute_interpolated_directional_derivative(float2 grad, float2 nbor1Grad, float2 nbor2Grad, float2 nbor3Grad, float locStep)
{
    float dirDeriv;
    float nbor1ProjGrad;
    float nbor2ProjGrad;
    float nbor3ProjGrad;
    
    float2 normGrad;
    float3 projections;
    float3 applicator;
    
    normGrad = normalize(grad);
    
    nbor1ProjGrad = dot(normGrad, nbor1Grad);
    nbor2ProjGrad = dot(normGrad, nbor2Grad);
    nbor3ProjGrad = dot(normGrad, nbor3Grad);
    
    projections.xyz = (float3)(nbor1ProjGrad, nbor2ProjGrad, nbor3ProjGrad);
    
    applicator.xyz  = (float3)(  0.5f*pow(locStep,2.0f) - 0.5f*locStep
                               , -pow(locStep,2.0f) + 1.0f
                               , 0.5f*pow(locStep,2.0f) + 0.5f*locStep);
    
    dirDeriv = dot(applicator, projections);
    
    return dirDeriv;
}

__kernel void subpixel_edge (
                      __global   float* xGradImg,
                      __global   float* yGradImg,
                      __global   float* edgeImg,
                      __global   float* xDeltaImg,
                      __global   float* yDeltaImg,
                                 int    nRows,
                                 int    nCols)
{
    const int2 pos = {get_global_id(0), get_global_id(1)};
    
    float posXGrad;
    float posYGrad;
    float2 posGrad;
    float posGradL2Norm;
    
    int2 nbor1;
    int2 nbor2;
    int2 nbor3;
    
    float2 nbor1Grad;
    float2 nbor2Grad;
    float2 nbor3Grad;
    
    float  locStep;
    
    float dirDerivB;
    float dirDerivC;
    float2 locPosB;
    float2 locPosC;
    
    float angle;
    float maxPos;
    float maxGrad;
    
    posXGrad      = readBuffer(xGradImg, pos, nRows, nCols);
    posYGrad      = readBuffer(yGradImg, pos, nRows, nCols);
    posGrad       = (float2)(posXGrad, posYGrad);
    posGradL2Norm = length(posGrad);
    
    //compute gradient angle (**algorithm designed with +y = -row; requires flip to -ve**)
    angle = atan2pi(-posYGrad, posXGrad);
    
    //select relevant neighbours for estimating gradient in direction of angle (i.e. 'C')
    select_nbors(angle, &nbor1, &nbor2, &nbor3, &locStep, &locPosC);
    
    nbor1Grad = get_gradient ( xGradImg, yGradImg, pos, nbor1, nRows, nCols);
    nbor2Grad = get_gradient ( xGradImg, yGradImg, pos, nbor2, nRows, nCols);
    nbor3Grad = get_gradient ( xGradImg, yGradImg, pos, nbor3, nRows, nCols);
    
    //quadratic interpolation to find value ('C' gradient projection)
    dirDerivC = compute_interpolated_directional_derivative(posGrad, nbor1Grad, nbor2Grad, nbor3Grad, locStep);
    
    angle = angle + 1.0f;
    if( angle > 1.0f )
    {
        //wrap-around [-1,1]
        angle = angle - 2.0f;
    }
    
    //select relevant neighbours for estimating gradient in opposite direction of angle (i.e. 'B')
    select_nbors(angle, &nbor1, &nbor2, &nbor3, &locStep, &locPosB);
    
    nbor1Grad = get_gradient ( xGradImg, yGradImg, pos, nbor1, nRows, nCols);
    nbor2Grad = get_gradient ( xGradImg, yGradImg, pos, nbor2, nRows, nCols);
    nbor3Grad = get_gradient ( xGradImg, yGradImg, pos, nbor3, nRows, nCols);

    //quadratic interpolation to find value ('B' gradient projection)
    dirDerivB = compute_interpolated_directional_derivative(posGrad, nbor1Grad, nbor2Grad, nbor3Grad, locStep);

    //check if center is maximum (NMS)
    if( dirDerivB >= posGradL2Norm || dirDerivC >= posGradL2Norm )
    {
        writeBuffer(edgeImg,   0.0f, pos, nRows, nCols);
        writeBuffer(xDeltaImg, 0.0f, pos, nRows, nCols);
        writeBuffer(yDeltaImg, 0.0f, pos, nRows, nCols);
    }
    else
    {
        maxPos  = (dirDerivB - dirDerivC)/(2.0f*dirDerivB - 4.0f*posGradL2Norm + 2.0f*dirDerivC);
        maxGrad = -pow(maxPos,2.0f)*(0.5f*dirDerivB - posGradL2Norm + 0.5f*dirDerivC) + posGradL2Norm;
        
        writeBuffer(edgeImg,   maxGrad, pos, nRows, nCols);
        writeBuffer(xDeltaImg, maxPos*locPosC.x, pos, nRows, nCols);
        writeBuffer(yDeltaImg, maxPos*locPosC.y, pos, nRows, nCols);
    }
}









