#include "cv.h"
#include "highgui.h"
#include <stdio.h>
#include <math.h>
#include <string.h>


//////////////////////////////////////////////////////////////////
//函式功能：用向量來做COSα=兩向量之積/兩向量模的乘積求兩條線段夾角
//輸入：   線段3個點座標pt1,pt2,pt0,最後一個引數為公共點
//輸出：   線段夾角，單位為角度
//////////////////////////////////////////////////////////////////
double angle( CvPoint* pt1, CvPoint* pt2, CvPoint* 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;    
    double angle_line = (dx1*dx2 + dy1*dy2)/sqrt((dx1*dx1 + dy1*dy1)*(dx2*dx2 + dy2*dy2) + 1e-10);//餘弦值
    return acos(angle_line)*180/3.141592653; 
}
//////////////////////////////////////////////////////////////////
//函式功能：採用多邊形檢測，通過約束條件尋找矩形
//輸入：   img 原影象
//          storage 儲存
 

//          minarea，maxarea 檢測矩形的最小/最大面積
//          minangle,maxangle 檢測矩形邊夾角範圍，單位為角度
//輸出：   矩形序列
//////////////////////////////////////////////////////////////////
CvSeq* findSquares4( IplImage* img, CvMemStorage* storage ,int minarea, int maxarea, int minangle, int maxangle)
{  
    CvSeq* contours;//邊緣
    int N = 6;  //閾值分級
    CvSize sz = cvSize( img->width & -2
 
, img->height & -2 );
    IplImage* timg = cvCloneImage( img );//拷貝一次img
    IplImage* gray = cvCreateImage( sz, 8, 1 ); //img灰度圖
    IplImage* pyr = cvCreateImage( cvSize(sz.width/2, sz.height/2), 8, 3 );  //金字塔濾波3通道影象中間變數
    IplImage* tgray = cvCreateImage( sz, 8, 1 ); ;   
    CvSeq* result;  
    double s, t;  
    CvSeq* squares = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvPoint), storage );   

    cvSetImageROI( timg, cvRect( 0, 0, sz.width, sz.height ));   
    //金字塔濾波 
    cvPyrDown( timg, pyr, 7 );  
    cvPyrUp( pyr, timg, 7 );   
    //在3個通道中尋找矩形 
    for( int c = 0; c < 3; c++ ) //對3個通道分別進行處理 
    {       
        cvSetImageCOI( timg, c+1 );     
        cvCopy( timg, tgray, 0 );  //依次將BGR通道送入tgray         
        for( int l = 0; l < N; l++ )     
        {         
            //不同閾值下二值化
            cvThreshold( tgray, gray, (l+1)*255/N, 255, CV_THRESH_BINARY );

            cvFindContours( gray, storage, &contours, sizeof(CvContour),CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE, cvPoint(0,0) );          
            while( contours )    
            { //多邊形逼近             
              result = cvApproxPoly( contours, sizeof(CvContour), storage,CV_POLY_APPROX_DP, cvContourPerimeter(contours)*0.02, 0 ); 
                //如果是凸四邊形並且面積在範圍內
              if( result->total == 4 && fabs(cvContourArea(result,CV_WHOLE_SEQ)) > minarea  && fabs(cvContourArea(result,CV_WHOLE_SEQ)) < maxarea &&  cvCheckContourConvexity(result) )  
                {               
                    s = 0;      
                    //判斷每一條邊
                    for( int i = 0; i < 5; i++ )  
                    {                   
                        if( i >= 2 )           
                        {   //角度            
                            t = fabs(angle( (CvPoint*)cvGetSeqElem( result, i ),(CvPoint*)cvGetSeqElem( result, i-2 ),(CvPoint*)cvGetSeqElem( result, i-1 )));   
                            s = s > t ? s : t;     
                        }         
                    }   
                    //這裡的S為直角判定條件 單位為角度
                    if( s > minangle && s < maxangle )                      
                        for( int i = 0; i < 4; i++ )              
                            cvSeqPush( squares,(CvPoint*)cvGetSeqElem( result, i ));     
                }                                      
                contours = contours->h_next;      
            }   
        } 
    }
    cvReleaseImage( &gray );   
    cvReleaseImage( &pyr );  
    cvReleaseImage( &tgray );  
    cvReleaseImage( &timg );   
    return squares;
}  
//////////////////////////////////////////////////////////////////
//函式功能：畫出所有矩形
//輸入：   img 原影象
//          squares 矩形序列
//          wndname 視窗名稱
//輸出：   影象中標記矩形
//////////////////////////////////////////////////////////////////
void drawSquares( IplImage* img, CvSeq* squares ,const char* wndname)
{   
    CvSeqReader reader;   
    IplImage* cpy = cvCloneImage( img );   
    CvPoint pt[4];
    int i;       
    cvStartReadSeq( squares, &reader, 0 );     
    for( i = 0; i < squares->total; i += 4 )  
    {       
        CvPoint* rect = pt;    
        int count = 4;      
        memcpy( pt, reader.ptr, squares->elem_size ); 
        CV_NEXT_SEQ_ELEM( squares->elem_size, reader ); 
        memcpy( pt + 1, reader.ptr, squares->elem_size );     
        CV_NEXT_SEQ_ELEM( squares->elem_size, reader );   
        memcpy( pt + 2, reader.ptr, squares->elem_size );   
        CV_NEXT_SEQ_ELEM( squares->elem_size, reader );     
        memcpy( pt + 3, reader.ptr, squares->elem_size );  
        CV_NEXT_SEQ_ELEM( squares->elem_size, reader );         
        //cvPolyLine( cpy, &rect, &count, 1, 1, CV_RGB(0,255,0), 3, CV_AA, 0 );
        cvPolyLine( cpy, &rect, &count, 1, 1, CV_RGB(rand()&255,rand()&255,rand()&255), 1, CV_AA, 0 );//彩色繪製
    }        
    cvShowImage( wndname, cpy );  
    cvReleaseImage( &cpy );
}

int main()
{    
    CvCapture* capture = cvCreateCameraCapture(0);
    IplImage* img0 = 0;
    CvMemStorage* storage = 0;
    int c; 
    const char* wndname = "Square Detection Demo"; //視窗名稱
    storage = cvCreateMemStorage(0);    
    cvNamedWindow( wndname, 1 );    
    while (true)
    {
        img0 = cvQueryFrame(capture);      
        drawSquares( img0, findSquares4( img0, storage, 100, 2000, 80, 100), wndname );
        cvClearMemStorage( storage );  //清空儲存
        c = cvWaitKey(10); 
        if( c == 27 )       
        break;  
    }

    cvReleaseImage( &img0 );       
    cvClearMemStorage( storage );  

    cvDestroyWindow( wndname );   
    return 0;
}