//
//  subpixel_edge.cpp
//  subpixel_edge
//
//  Created by Mehdi Stapleton on 2015-06-17.
//
//

#include "subpixel_edge.h"

//-------------------------------------------------------------------------
//                     HELPER EDGE DETECTION FUNCTIONS
//-------------------------------------------------------------------------


void select_nbors (const matrix_t angle, int* const nbor1, int* const nbor2, int* const nbor3, matrix_t &locStep, matrix_t * const 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.0 && angle < -0.75 )
    {
        nbor1[0] = -1;
        nbor1[1] = -1;
        nbor2[0] = -1;
        nbor2[1] =  0;
        nbor3[0] = -1;
        nbor3[1] =  1;
        
        locStep  = tanpi(angle);
        locPos[0] = -1;
        locPos[1] = locStep;
        
    }
    else if( angle >= -0.75 && angle < -0.5 )
    {
        nbor1[0] =  1;
        nbor1[1] =  1;
        nbor2[0] =  0;
        nbor2[1] =  1;
        nbor3[0] = -1;
        nbor3[1] =  1;
        
        locStep  = 1/tanpi(angle);
        locPos[0] = -locStep;
        locPos[1] = 1;
        
    }
    else if( angle >= -0.5 && angle < -0.25 )
    {
        nbor1[0] =  1;
        nbor1[1] =  1;
        nbor2[0] =  0;
        nbor2[1] =  1;
        nbor3[0] = -1;
        nbor3[1] =  1;
        
        locStep  = 1/tanpi(angle);
        locPos[0] = -locStep;
        locPos[1] = 1;
        
    }
    else if( angle >= -0.25 && angle < 0.0 )
    {
        nbor1[0] =  1;
        nbor1[1] =  1;
        nbor2[0] =  1;
        nbor2[1] =  0;
        nbor3[0] =  1;
        nbor3[1] = -1;
        
        locStep  = tanpi(angle);
        locPos[0] = 1;
        locPos[1] = -locStep;
        
    }
    else if( angle >= 0.0 && angle < 0.25 )
    {
        nbor1[0] =  1;
        nbor1[1] =  1;
        nbor2[0] =  1;
        nbor2[1] =  0;
        nbor3[0] =  1;
        nbor3[1] = -1;
        
        locStep  = tanpi(angle);
        locPos[0] = 1;
        locPos[1] = -locStep;
        
    }
    else if( angle >= 0.25 && angle < 0.5 )
    {
        nbor1[0] = -1;
        nbor1[1] = -1;
        nbor2[0] =  0;
        nbor2[1] = -1;
        nbor3[0] =  1;
        nbor3[1] = -1;
        
        locStep  = 1/tanpi(angle);
        locPos[0] = locStep;
        locPos[1] = -1;
        
    }
    else if( angle >= 0.5f && angle < 0.75 )
    {
        nbor1[0] = -1;
        nbor1[1] = -1;
        nbor2[0] =  0;
        nbor2[1] = -1;
        nbor3[0] =  1;
        nbor3[1] = -1;
        
        locStep  = 1/tanpi(angle);
        locPos[0] = locStep;
        locPos[1] = -1;
        
    }
    else if( angle >= 0.75 && angle <= 1.0 )
    {
        nbor1[0] = -1;
        nbor1[1] = -1;
        nbor2[0] = -1;
        nbor2[1] =  0;
        nbor3[0] = -1;
        nbor3[1] =  1;
        
        locStep  = tanpi(angle);
        locPos[0] = -1;
        locPos[1] = locStep;
        
    }
    else
    {
        nbor1[0] =  0;
        nbor1[1] =  0;
        nbor2[0] =  0;
        nbor2[1] =  0;
        nbor3[0] =  0;
        nbor3[1] =  0;
        
        locStep   = 0;
        locPos[0] = 0;
        locPos[1] = 0;
        
    }
}


void get_gradient(const matrix_t* xGradImgIn, const matrix_t* yGradImgIn, int rOffset, int cOffset, int r, int c, matrix_t* const grad, int nRows, int nCols)
{
    
    int rNew = r + rOffset;
    int cNew = c + cOffset;
    
    if( rNew > nRows-1 )
    {
        rNew = nRows-1;
    }
    else if( rNew < 0 )
    {
        rNew = 0;
    }
    
    if( cNew > nCols-1 )
    {
        cNew = nCols-1;
    }
    else if( cNew < 0 )
    {
        cNew = 0;
    }
    
    grad[0] = xGradImgIn[rNew + cNew*nRows];
    grad[1] = yGradImgIn[rNew + cNew*nRows];
    
}


matrix_t compute_interpolated_directional_derivative(const matrix_t* normGrad, const matrix_t* nbor1Grad, const matrix_t* nbor2Grad, const matrix_t* nbor3Grad, matrix_t locStep)
{
    matrix_t dirDeriv;
    matrix_t nbor1ProjGrad;
    matrix_t nbor2ProjGrad;
    matrix_t nbor3ProjGrad;
    
    matrix_t locStepSq = pow(locStep, 2.0);
    
    nbor1ProjGrad = normGrad[0]*nbor1Grad[0] + normGrad[1]*nbor1Grad[1];
    nbor2ProjGrad = normGrad[0]*nbor2Grad[0] + normGrad[1]*nbor2Grad[1];
    nbor3ProjGrad = normGrad[0]*nbor3Grad[0] + normGrad[1]*nbor3Grad[1];
    
    dirDeriv = nbor1ProjGrad*(0.5*locStepSq - 0.5*locStep) + nbor2ProjGrad*(-locStepSq + 1.0) +nbor3ProjGrad*(0.5*locStepSq + 0.5*locStep);
    
    return dirDeriv;
}

double atan2pi(double y, double x)
{
    double ang;
    ang = atan2(y,x);
    ang /= PI;
    return ang;
}

double tanpi(double ang)
{
    return tan(ang*PI);
}

//-------------------------------------------------------------------------
//                         SUBPIXEL EDGE DETECTION
//-------------------------------------------------------------------------

#ifndef XCODE_COMPILATION

/*\brief apply subpixel edge detection (Devernay1995)
  1) xGradImg: x-component (column-wise) of gradient
  2) yGradImg: y-component (row-wise) of gradient 
 */
void subpixel_edge_c(matrix_t* xGradImgIn, matrix_t* yGradImgIn, matrix_t* edgeImgOut, matrix_t* xDeltaImgOut, matrix_t* yDeltaImgOut, int nRows, int nCols)
{
    
    matrix_t locStep, dirDerivC, dirDerivB, maxPos, maxGrad;
    matrix_t gradMag, gradAng, xGrad, yGrad;
    matrix_t locPosC[2];
    matrix_t locPosB[2];
    int nbor1[2];
    int nbor2[2];
    int nbor3[2];
    matrix_t ctrGrad[2];
    matrix_t nbor1Grad[2];
    matrix_t nbor2Grad[2];
    matrix_t nbor3Grad[2];
    
    index_t idx;
    for( index_t r = 0; r < nRows; ++r)
    {
        for( index_t c = 0; c < nCols; ++c)
        {
            
            idx = r + c*nRows;
            
            xGrad   = xGradImgIn[idx];
            yGrad   = yGradImgIn[idx];
            gradMag = hypot(xGrad, yGrad);
            //compute gradient angle (**algorithm designed with +y = -row; requires flip to -ve**)
            gradAng = atan2pi( -yGrad, xGrad);
            
            //normalized gradient at (r,c) => (x=c, y=r)
            ctrGrad[0] = xGrad/gradMag;
            ctrGrad[1] = yGrad/gradMag;
            
            //select relevant neighbours for estimating gradient in direction of angle (i.e. 'C')
            select_nbors (gradAng, nbor1, nbor2, nbor3, locStep, locPosC);
            
            get_gradient(xGradImgIn, yGradImgIn, nbor1[1], nbor1[0], r, c, nbor1Grad, nRows, nCols);
            get_gradient(xGradImgIn, yGradImgIn, nbor2[1], nbor2[0], r, c, nbor2Grad, nRows, nCols);
            get_gradient(xGradImgIn, yGradImgIn, nbor3[1], nbor3[0], r, c, nbor3Grad, nRows, nCols);
            
            //quadratic interpolation to find value ('C' gradient projection)
            dirDerivC = compute_interpolated_directional_derivative(ctrGrad, nbor1Grad, nbor2Grad, nbor3Grad, locStep);
            
            gradAng += 1.0;
            if( gradAng > 1.0 )
            {
                //wrap-around [-1,1]
                gradAng -= 2.0;
            }

            //select relevant neighbours for estimating gradient in opposite direction of angle (i.e. 'B')
            select_nbors (gradAng, nbor1, nbor2, nbor3, locStep, locPosB);
            
            get_gradient(xGradImgIn, yGradImgIn, nbor1[1], nbor1[0], r, c, nbor1Grad, nRows, nCols);
            get_gradient(xGradImgIn, yGradImgIn, nbor2[1], nbor2[0], r, c, nbor2Grad, nRows, nCols);
            get_gradient(xGradImgIn, yGradImgIn, nbor3[1], nbor3[0], r, c, nbor3Grad, nRows, nCols);
            
            //quadratic interpolation to find value ('C' gradient projection)
            dirDerivB = compute_interpolated_directional_derivative(ctrGrad, nbor1Grad, nbor2Grad, nbor3Grad, locStep);
            
            //check if center is maximum (NMS)
            if( dirDerivB >= gradMag || dirDerivC >= gradMag)
            {
                edgeImgOut[idx]   = 0.0;
                xDeltaImgOut[idx] = 0.0;
                yDeltaImgOut[idx] = 0.0;
            }
            else
            {
                maxPos  = (dirDerivB - dirDerivC)/(2.0*dirDerivB - 4.0*gradMag + 2.0*dirDerivC);
                maxGrad = -pow(maxPos,2.0)*(0.5*dirDerivB - gradMag + 0.5*dirDerivC) + gradMag;
                
                edgeImgOut[idx]   = maxGrad;
                xDeltaImgOut[idx] = maxPos*locPosC[0];
                yDeltaImgOut[idx] = maxPos*locPosC[1];
                
            }
            
        }
    }
    
    
}//end of subpixel edge

#endif//XCODE_COMPILATION



