#include "contour.h"

/**
  @brief 用Sobel算子检测图像梯度边缘，非极大值抑制得到细化边缘
  @param [out] dst 目标输出图像，为二值化图像
  @param [in] auxSubIndex 辅助遍历图像下标数组（加快遍历图像数组速度）
  @param [in] image 输入图像
  @param [in] width 图像宽
  @param [in] height 图像高
  @param [in] sobelThres 梯度模值阈值
*/
void sobel_NMS(uchar *dst, uint *auxSubIndex, uchar *image, ushort width, ushort height, uint sobelThres)
{
    uchar *rowCopy = (uchar *)malloc((width+2)*3);
    uint gradMag[width*height];
    //memset(gradMag, 0, width*height);
    if( NULL == rowCopy)
        return;
    if( NULL == gradMag)
        return;
    uchar *first = rowCopy;
    uchar *second = rowCopy + (width + 2);
    uchar *third = rowCopy + (width + 2)*2;

    memcpy(second, image, 1);
    memcpy(second + 1, image, width);
    memcpy(second + width + 1, image + width-1, 1);

    memcpy(first, second, width + 2);

    memcpy(third, image + width ,1);
    memcpy(third + 1, image + width, width);
    memcpy(third + width +1, image + width*2 -1, 1);

    for(int y=0; y<height; y++)
    {
        if( y!=0)
        {
            uchar *temp=first;
            first = second;
            second = third;
            third = temp;
        }
        if( y==height-1)
            memcpy(third, second, width+2);
        else
        {
            memcpy(third, image+auxSubIndex[y+1], 1);
            memcpy(third +1, image+auxSubIndex[y+1], width);
            memcpy(third +width+1, image + auxSubIndex[y+1]-1, 1);
        }
        for(int x =0; x<width; x++)
        {
            int GX = first[x] - first[x+2] + (second[x] - second[x+2])*2 + third[x] - third[x+2];
            int GY = first[x] + first[x+2] + (first[x+1] - third[x+1])*2 - third[x] - third[x+2];
            gradMag[auxSubIndex[y]+x] = GX*GX + GY*GY;
            //linePD[x] = abs(GX) + abs(GY);
        }
    }
    free(rowCopy);
    /*
    b8 b6 b4
      \|/
   b1-----b2
      /|\
    b3 b5 b7
    循环判断4个方向上的梯度值
    */
    for(int i=1; i<height-1; i++)
    for(int j=1; j<width-1; j++)
    {
        bool b0 = gradMag[auxSubIndex[i]+j] > sobelThres;
        bool b1 = gradMag[auxSubIndex[i]+j] > gradMag[auxSubIndex[i]+j-1];//水平
        bool b2 = gradMag[auxSubIndex[i]+j] > gradMag[auxSubIndex[i]+j+1];

        bool b3 = gradMag[auxSubIndex[i]+j] > gradMag[auxSubIndex[i+1]+j-1];//水平下方45度
        bool b4 = gradMag[auxSubIndex[i]+j] > gradMag[auxSubIndex[i-1]+j+1];

        bool b5 = gradMag[auxSubIndex[i]+j] > gradMag[auxSubIndex[i-1]+j];//垂直
        bool b6 = gradMag[auxSubIndex[i]+j] > gradMag[auxSubIndex[i+1]+j];

        bool b7 = gradMag[auxSubIndex[i]+j] > gradMag[auxSubIndex[i-1]+j-1];//水平上方45度
        bool b8 = gradMag[auxSubIndex[i]+j] > gradMag[auxSubIndex[i+1]+j+1];
        dst[auxSubIndex[i]+j] =  (b0 && ((b1 && b2 && b3 && b4) || (b3 && b4 && b5 && b6) || (b5 && b6 && b7 && b8) || (b7 && b8 && b2 && b1)));
    }
}

/**
  @brief 二值图像转灰度图像
  @param [out] dst 目标输出图像
  @param [in] image 输入图像
  @param []size 图像大小
*/
void bin2gray(uchar *dst, uchar *image, int size)
{
    for(int i=0; i<size; i++)
        dst[i] = 255*image[i];
}

/**
  @brief 对图像进行2倍下采样
  @param [in] image 原图像
  @param [out] dst 目标输出图像
  @param [in] auxSubIndex 辅助遍历图像下标数组（加快遍历图像数组速度）
*/
void downSample_scale2(uchar *dst, uchar *image, uint *preSampleLocat)
{
    uchar *newImage=(uchar *)malloc(sampledImgSize);
    for(uint i=0; i<sampledImgSize; i++)
        newImage[i] = image[preSampleLocat[i]];
    memcpy(dst, newImage, sampledImgSize);
    free(newImage);
}