/*
int block_size:			 鄰域大小，相鄰畫素的尺寸，就是視窗函式大小  W(x, y);
int aperture_size:    aperture size  for the Sobel();   因為我們用sobel函式來得到Ix， Iy;
double k         ：   harris 檢測器的自由引數
*/
cvCornerHarris( const CvArr* srcarr, CvArr* dstarr,
                int block_size, int aperture_size, double k )
{
    cv::Mat src = cv::cvarrToMat(srcarr), dst = cv::cvarrToMat(dstarr);

    CV_Assert( src.size() == dst.size() && dst.type() == CV_32FC1 );
    
    //呼叫opencv內部Harris檢測函式
    cv::cornerHarris( src, dst, block_size, aperture_size, k, cv::BORDER_REPLICATE );
}


//分配輸出資料空間，並呼叫harris特徵檢測函式
void cv::cornerHarris( InputArray _src, OutputArray _dst, int blockSize, int ksize, double k, int borderType )
{
    Mat src = _src.getMat();
    _dst.create( src.size(), CV_32F );
    Mat dst = _dst.getMat();
    cornerEigenValsVecs( src, dst, blockSize, ksize, HARRIS, k, borderType );
}


//預處理：sobel檢測獲取x方向和y方向上的邊緣, 濾波處理，準備好Ix*Ix  Iy*Iy  Ix*Iy
//然後呼叫最終的檢測函式
static void
cornerEigenValsVecs( const Mat& src, Mat& eigenv, int block_size,
                     int aperture_size, int op_type, double k=0.,
                     int borderType=BORDER_DEFAULT )
{
#ifdef HAVE_TEGRA_OPTIMIZATION
    if (tegra::cornerEigenValsVecs(src, eigenv, block_size, aperture_size, op_type, k, borderType))
        return;
#endif

    int depth = src.depth();
    double scale = (double)(1 << ((aperture_size > 0 ? aperture_size : 3) - 1)) * block_size;
    if( aperture_size < 0 )
        scale *= 2.;
    if( depth == CV_8U )
        scale *= 255.;
    scale = 1./scale;

    CV_Assert( src.type() == CV_8UC1 || src.type() == CV_32FC1 );

    Mat Dx, Dy;
    if( aperture_size > 0 )  //利用sobel獲取影象邊緣資訊，獲取x和y兩個方向的邊緣資訊
    {
        Sobel( src, Dx, CV_32F, 1, 0, aperture_size, scale, 0, borderType );
        Sobel( src, Dy, CV_32F, 0, 1, aperture_size, scale, 0, borderType );
    }
    else
    {
        Scharr( src, Dx, CV_32F, 1, 0, scale, 0, borderType );
        Scharr( src, Dy, CV_32F, 0, 1, scale, 0, borderType );
    }

    Size size = src.size();
    Mat cov( size, CV_32FC3 );
    int i, j;

    for( i = 0; i < size.height; i++ )
    {
        float* cov_data = (float*)(cov.data + i*cov.step);
        const float* dxdata = (const float*)(Dx.data + i*Dx.step);
        const float* dydata = (const float*)(Dy.data + i*Dy.step);

        for( j = 0; j < size.width; j++ )
        {
            float dx = dxdata[j];
            float dy = dydata[j];

            cov_data[j*3] = dx*dx;
            cov_data[j*3+1] = dx*dy;
            cov_data[j*3+2] = dy*dy;
        }
    }

	  //使用方框濾波
    boxFilter(cov, cov, cov.depth(), Size(block_size, block_size),
        Point(-1,-1), false, borderType );
    
    //這裡有幾種方式
    if( op_type == MINEIGENVAL )
        calcMinEigenVal( cov, eigenv );
    else if( op_type == HARRIS )
        calcHarris( cov, eigenv, k );
    else if( op_type == EIGENVALSVECS )
        calcEigenValsVecs( cov, eigenv );
}


//特徵提取函式
static void
calcHarris( const Mat& _cov, Mat& _dst, double k )
{
    int i, j;
    Size size = _cov.size();
#if CV_SSE
    volatile bool simd = checkHardwareSupport(CV_CPU_SSE);
#endif

    if( _cov.isContinuous() && _dst.isContinuous() )
    {
        size.width *= size.height;
        size.height = 1;
    }

    for( i = 0; i < size.height; i++ )
    {
        const float* cov = (const float*)(_cov.data + _cov.step*i);
        float* dst = (float*)(_dst.data + _dst.step*i);
        j = 0;

    #if CV_SSE
        if( simd )
        {
            __m128 k4 = _mm_set1_ps((float)k);
            for( ; j <= size.width - 5; j += 4 )
            {
                __m128 t0 = _mm_loadu_ps(cov + j*3); // a0 b0 c0 x
                __m128 t1 = _mm_loadu_ps(cov + j*3 + 3); // a1 b1 c1 x
                __m128 t2 = _mm_loadu_ps(cov + j*3 + 6); // a2 b2 c2 x
                __m128 t3 = _mm_loadu_ps(cov + j*3 + 9); // a3 b3 c3 x
                __m128 a, b, c, t;
                t = _mm_unpacklo_ps(t0, t1); // a0 a1 b0 b1
                c = _mm_unpackhi_ps(t0, t1); // c0 c1 x x
                b = _mm_unpacklo_ps(t2, t3); // a2 a3 b2 b3
                c = _mm_movelh_ps(c, _mm_unpackhi_ps(t2, t3)); // c0 c1 c2 c3
                a = _mm_movelh_ps(t, b);
                b = _mm_movehl_ps(b, t);
                t = _mm_add_ps(a, c);
                a = _mm_sub_ps(_mm_mul_ps(a, c), _mm_mul_ps(b, b));
                t = _mm_mul_ps(_mm_mul_ps(k4, t), t);
                a = _mm_sub_ps(a, t);
                _mm_storeu_ps(dst + j, a);
            }
        }
    #endif

        for( ; j < size.width; j++ )
        {
            float a = cov[j*3];
            float b = cov[j*3+1];
            float c = cov[j*3+2];
            
            dst[j] = (float)(a*c - b*b - k*(a + c)*(a + c));  //對應公式
        }
    }
}