#include "detecposition.h"
#include <iostream>
const int move4[4][2] = {0, 1, 0, -1, 1, 0, -1, 0};

/*深度搜索最大连通块
 * 标记idx
 */
int
DetecPosition::backtrack(int x, int y, int idx)
{
    data[x * width + y] = 255 - idx;
    int ret = 1;
    int nx, ny;
    for(int i = 0; i < 4; i++)
    {
        nx = x + move4[i][0];
        ny = y + move4[i][1];
        if(nx < height && ny < width && nx >= 0 && ny >= 0 && data[nx*width + ny] == 255)
        {
            ret += backtrack(nx, ny, idx);
        }
    }
    return ret;
}

/*
 * 保留最大连通块
 * 确定最大连通块的重心位置
 *
 */
CvPoint
DetecPosition::Detect(IplImage *src, IplImage *dst)
{
    assert(src->nChannels == dst->nChannels);
    cvCopy(src, dst);
    data = (uchar*)dst->imageData;
    width = src->width;
    height = src->height;
    int idx = 0;
    int mx = 0, mxidx;
    uchar *p = data;
    for(int i = 0; i < height; i++)
        for(int j = 0; j < width; j++)
        {
            if(*p == 255) ++mx;
            p++;
        }
    if(mx > (height * width) / 2) return cvPoint(-1, -1);
    mx = 0;
    for(int i = 10; i < height - 10; i++)
    {
        for(int j = 10; j < width - 10; j++)
        {
            if(data[i*width + j] == 255)
            {cnt[idx] = backtrack(i, j, idx + 1);
                if(mx < cnt[idx]){mx = cnt[idx]; mxidx = idx;}
                if(idx == 254 || mx > width * height /3)
                {
                    return cvPoint(-1, -1);
                }
                idx ++;
                cnt[idx] = backtrack(i, j, idx + 1);
                if(mx < cnt[idx]){mx = cnt[idx]; mxidx = idx;}
                if(idx == 254 || mx > width * height /3)
                {
                    return cvPoint(-1, -1);
                }
                idx ++;
            }
        }
    }
    if(mx < 18000) return cvPoint(-1, -1);
    p = data;
    long long ax, ay;
    ax = ay = 0;
    for(int i = 0; i < height; i++)
    {
        for(int j = 0; j < width; j++)
        {
            if(*p == 254 - mxidx) {
                ax += i;
                ay += j;
                *p = 255;
            }
            else *p = 0;
            p++;
        }
    }
    ax /= mx;
    ay /= mx;
    return cvPoint(ax, ay);
}
/*
 *  确定点在视角的位置
 */
uchar
detectCode(CvPoint point, int width, int height)
{
    uchar code = 0;
    if((point.x - 240) * (point.x - 240) + (point.y - 240) * (point.y - 240) < 3600)
        return 4;
    code |= 1&(point.x < point.y?1:0);
    code<<=1;
    code |= 1&(-point.x + 480 < point.y?1:0);
    return code;
}

/*
 *  自动阔值算法二值化
 *
 */
int
otsu(IplImage *src, IplImage *dst)
{
    int width = src->width;
    int height = src->height;
    IplImage *imgYCrCb = cvCreateImage(cvSize(width, height),
                                       src->depth, src->nChannels);
    cvCvtColor(src, imgYCrCb, CV_BGR2YCrCb);
    cvSplit(imgYCrCb, 0, dst, 0, 0);
    cvFlip(dst, dst, 1);
    float p[256] = {0};
    uchar *data = (uchar *)dst->imageData;
    for(int i = 0; i < height; i++)
        for(int j = 0; j < width; j++)
            p[data[i*dst->widthStep + j]] ++;
    float w0, w1, u0, u1, deltaMax, deltaTmp;
    float w[256] = {0}, tmp[256] = {0};
    w[0] = p[0];
    for(int i = 1; i < 256; i++)
    {
        w[i] = p[i] + w[i-1];
        tmp[i] = i * p[i] + tmp[i-1];
    }
    int threshold = 0;
    for(int i = 0; i < 255; i++)
    {
        w0 = w[i];
        w1 = w[255] - w[i];
        u0 = tmp[i]/w[i];
        u1 = (tmp[255] - tmp[i])/(w[255] - w[i]);
        deltaTmp = w0 * w1 *(u0 - u1) * (u0 - u1);
        if(deltaTmp > deltaMax)
        {
            deltaMax = deltaTmp;
            threshold = i;
        }
    }
    cvThreshold(dst, dst, threshold, 255, CV_THRESH_BINARY);
    IplImage * temp = cvCreateImage(cvGetSize(dst), 8,1);
    cvMorphologyEx(dst, dst, temp, NULL, CV_MOP_CLOSE, 1);
    cvMorphologyEx(dst, dst, temp, NULL, CV_MOP_OPEN, 1);
    cvReleaseImage(&temp);
    cvReleaseImage(&imgYCrCb);
    return threshold;
}
inline int suitPoint(int x, int goal, int flag)
{
    if(flag)
    {
        if(x < goal)
            x = goal;
    }
    else if(x > goal)
        x = goal;
    return x;
}

inline double distance(CvPoint a, CvPoint b)
{
    return sqrt((a.x - b.x) * (a.x - b.x) + (a.y + b.y) * (a.y + b.y));
}

int findContours(IplImage *src)
{
      IplImage *dst;
      //src = cvLoadImage("pic.jpg", -1);
      dst = cvCreateImage(cvGetSize(src), 8, 3);
      CvMemStorage *storage = cvCreateMemStorage();
      CvSeq *contour = NULL, *hull = NULL;
      //find contours 8-connects or 4-connects
      CvContourScanner scanner = cvStartFindContours(src, storage);
      cvDrawContours(dst, contour, CV_RGB(255, 0, 0), CV_RGB(0, 255, 0), 0);
      int fingerCount = 0;
      while((contour = cvFindNextContour(scanner))!= NULL)
      {
          //set black background
          cvDrawContours(dst, contour, CV_RGB(255, 0, 0), CV_RGB(0, 255, 0), 0);
          hull = cvConvexHull2(contour, 0, CV_CLOCKWISE, 0);
//          CvPoint pt0 = **(CvPoint**) cvGetSeqElem(hull, hull->total - 1);
//          for(int i = 0; i < hull->total; ++i)
//          {
//              CvPoint pt1 = **(CvPoint**)cvGetSeqElem(hull, i);
//              cvLine(dst, pt0, pt1, CV_RGB(0, 0, 255));
//              pt0 = pt1;
//          }
          CvSeq *defect = cvConvexityDefects(contour, hull);
          for(int i = 0; i < defect->total; ++i)
          {
              CvConvexityDefect df = *(CvConvexityDefect*)cvGetSeqElem(defect, i);
              cvCircle(dst, *df.start, 2, CV_RGB(255, 255, 0), -1);
              cvCircle(dst, *df.end, 2, CV_RGB(255, 255, 0), -1);
              cvCircle(dst, *df.depth_point, 2, CV_RGB(0, 255, 255), -1);
              if(df.depth >= 19)
                fingerCount++;

          }
          cvShowImage("dst", dst);
          //cvShowImage("src", src);
          cvWaitKey(10);
          cvReleaseImage(&dst);
      }
    return fingerCount;
}