#include "opencv2/opencv.hpp"
#include <iostream>
#include <math.h>
#include <string.h>

using namespace cv;
using namespace std;


int thresh = 50, N = 11;
const char* wndname = "Square Detection Demo";


int calcdistance(Point_<int> &point1, Point_<int> &point2) {

    int x1 = point1.x;
    int y1 = point1.y;
    int x2 = point2.x;
    int y2 = point2.y;
    int dist = sqrt(pow(x2-x1,2)+pow(y2-x1,2));

    return dist;
}

// 查找向量之间的角度余弦
// 从pt0->pt1和从pt0->pt2
static double angle( Point pt1, Point pt2, Point pt0 )
{
    double dx1 = pt1.x - pt0.x;
    double dy1 = pt1.y - pt0.y;
    double dx2 = pt2.x - pt0.x;
    double dy2 = pt2.y - pt0.y;
    return (dx1*dx2 + dy1*dy2)/sqrt((dx1*dx1 + dy1*dy1)*(dx2*dx2 + dy2*dy2) + 1e-10);
    //cos<a,b>=(ab的内积)/(|a||b|)
}

// 返回图像上检测到的正方形序列
// 序列存储在指定的内存存储器中
static void findSquares( const Mat& image, vector<vector<Point> >& squares )
{
    int h = image.rows;
    int w = image.cols;
    squares.clear();

    Mat pyr, timg, gray0(image.size(), CV_8U), gray;

    // 缩小和放大图像以滤除噪音
    pyrDown(image, pyr, Size(image.cols/2, image.rows/2));//高斯降噪，并只取奇数行列缩小图片
    pyrUp(pyr, timg, image.size());//插入偶数行列，再次高斯降噪
    vector<vector<Point> > contours;

    // 在图像的每个颜色平面中查找正方形
    for( int c = 0; c < 3; c++ )
    {
        int ch[] = {c, 0};
        mixChannels(&timg, 1, &gray0, 1, ch, 1);

        // 尝试几个阈值级别
        for( int l = 0; l < N; l++ )
        {
            if( l == 0 )
            {
                Canny(gray0, gray, 0, thresh, 5);

                dilate(gray, gray, Mat(), Point(-1,-1));
            }
            else
            {
                // apply threshold if l!=0:
                //     tgray(x,y) = gray(x,y) < (l+1)*255/N ? 255 : 0
                gray = gray0 >= (l+1)*255/N;
            }

            //注意_RETR_LIST参数指明最后只存了角点位置
            findContours(gray, contours, RETR_LIST, CHAIN_APPROX_SIMPLE);

            vector<Point> approx;

            // test each contour
            //一次取一个轮廓判断下是不是矩形
            for( size_t i = 0; i < contours.size(); i++ )
            {

                //approx存储了近似后的轮廓
                approxPolyDP(Mat(contours[i]), approx, arcLength(Mat(contours[i]), true)*0.02, true);

                if( approx.size() == 4 && //矩形必须是四个点
                    fabs(contourArea(Mat(approx))) > 5000 &&
                    isContourConvex(Mat(approx)) )//必须是凸的，咋理解？？？？
                {
                    double maxCosine = 0;

                    for( int j = 2; j < 5; j++ )
                    {
                        // find the maximum cosine of the angle between joint edges
                        double cosine = fabs(angle(approx[j%4], approx[j-2], approx[j-1]));
                        maxCosine = MAX(maxCosine, cosine);
                    }
                    //依次计算1,2,3顶点的角余弦，最大的余弦值，对应三个角中的最小角，也就是三个角中，最不像直角的
                    //为什么是三个呢？三个中最不像直角的都接近直角了，剩下的自然也是直角
                    if( maxCosine < 0.3 )
                        if (calcdistance(approx[0],approx[3]) < h && calcdistance(approx[0],approx[1]) < w){
                            squares.push_back(approx);
                        }
                }
            }
        }
    }
}


// 函数绘制图像中的所有四边形
static void drawSquares(Mat& image, const vector<vector<Point> >& squares)
{
    for( size_t i = 0; i < squares.size(); i++ )
    {
        const Point* p = &squares[i][0];
        int n = (int)squares[i].size();
        circle(image,squares[i][0],3,Scalar(0,0,255),1);
        circle(image,squares[i][1],3,Scalar(0,0,255),1);
        circle(image,squares[i][2],3,Scalar(0,0,255),1);
        circle(image,squares[i][3],3,Scalar(0,0,255),1);
//        polylines(image, &p, &n, 1, true, Scalar(0,255,0), 1, LINE_AA);
//        break;
        Rect rr = boundingRect(squares[i]);

        //通过最小包围盒来获取主要函数区域
        vector<vector<Point>> main;
        main[0][0] = squares[0][0];
        main[0][1] = squares[0][1];
        main[0][2] = squares[1][2];
        main[0][3] = squares[1][3];

        circle(image,main[0][0],1,Scalar(0,255,0),-1);
        circle(image,main[0][1],1,Scalar(0,255,0),-1);
        imshow("main",image);
        RotatedRect minRect = minAreaRect(Mat(main[0]));
        Point2f vertex[4];//用于存放最小矩形的四个顶点
        minRect.points(vertex);//返回矩形的四个顶点给vertex
        //绘制最小面积包围矩形
        vector<Point>min_rectangle;
        for (int i = 0; i < 4; i++)
        {
            line(image, vertex[i], vertex[(i + 1) % 4], Scalar(0, 255, 255), 1, 8);//非常巧妙的表达式
            min_rectangle.push_back(vertex[i]);//将最小矩形的四个顶点以Point的形式放置在vector容器中
        }
//        rectangle(image,Point(rr.x, rr.y), Point(rr.x + rr.width, rr.y + rr.height), 1);
//        float area = contourArea(squares[i], false);
//        cout << "area==" << area << endl;
//        break;
    }

    imshow(wndname, image);
}



int main(int /*argc*/, char** /*argv*/)
{
    Mat image = imread("/home/leoxae/KeekoRobot/TestPic/大班/1.png");
    vector<vector<Point>> squares;

    findSquares(image, squares);

    for (auto itx = squares.begin(); itx != squares.end(); itx++){
        vector<Point> points = *itx;
        cout << "Pts=" << points << endl;
    }
/*    Point b_tl = squares[0][0];
    Point b_tr = squares[0][1];
    Point b_bl = squares[0][2];
    Point b_br = squares[0][3];

    Point s_tl = squares[1][0];
    Point s_tr = squares[1][1];
    Point s_bl = squares[1][2];
    Point s_br = squares[1][3];
    cout << "b_tl==" << b_tl << endl;
    cout << "b_tr==" << b_tr << endl;
    cout << "b_bl==" << b_bl << endl;
    cout << "b_br==" << b_br << endl;

    cout << "s_tl==" << s_tl << endl;
    cout << "s_tr==" << s_tr << endl;
    cout << "s_bl==" << s_bl << endl;
    cout << "s_br==" << s_br << endl;
    circle(image,b_bl,3,Scalar(0,0,255),-1);
    circle(image,b_tr,3,Scalar(0,0,255),-1);
    circle(image,b_bl,3,Scalar(0,0,255),-1);
    circle(image,b_br,3,Scalar(0,0,255),-1);

    circle(image,s_tl,3,Scalar(0,255,0),-1);
    circle(image,s_tr,3,Scalar(0,255,0),-1);
    circle(image,s_bl,3,Scalar(0,255,0),-1);
    circle(image,s_br,3,Scalar(0,255,0),-1);*/

//    imshow("drawcircle",image);
    drawSquares(image, squares);

    waitKey();
    return 0;
}