/*
VERSION: 2.0 ( Memory leaking debugging version)
AUTHOR: ANDOL LI (i@andol.info)
PROJECT: FAMILY COMMUNITY
ALL COPYRIGHTS @ LIVE:LAB
LAST UPDATED: 03/2009
*/

#include "cv.h"
#include "cxcore.h"
#include "highgui.h"
#include "math.h"
#include <iostream>
#include <stdio.h>
#include <string.h>
#include <conio.h>
#include <sstream>


using namespace std;
/*==========================================*/
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;
    return (dx1*dx2 + dy1*dy2)/sqrt((dx1*dx1 + dy1*dy1)*(dx2*dx2 + dy2*dy2) + 1e-10);
}
/*==========================================*/
CvSeq* findSquares4(  IplImage* img,  CvMemStorage* storage )
{
    double s = 0,  t = 0;
    CvSeq* contours =  NULL;
    CvSeq* result =  NULL;
    CvSeq* squares  =  cvCreateSeq(  0,  sizeof( CvSeq),  sizeof( CvPoint),  storage );
    cvFindContours(  img,  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)) > 1000 && fabs( cvContourArea( result, CV_WHOLE_SEQ)) <( img->height * img->width/2 ) && 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;
                }
            }
            if(  s < 0.5 )for(  int i  =  0; i < 4; i++ )cvSeqPush(  squares,  ( CvPoint*)cvGetSeqElem(  result,  i ));
        }
        contours  =  contours->h_next;
    }
    cout<<">>> rectangles: "<< squares->total/4 <<endl;

    return squares;
}/*==========================================*/
void drawSquares(IplImage *img, CvSeq* squares )
{
CvFont font;
cvInitFont( &font, CV_FONT_HERSHEY_SIMPLEX, 0.4f, 0.4f, 0,1, 8 );
    int i,j = 0;
    CvSeqReader reader;
    cvStartReadSeq( squares, &reader, 0 );
        //cout<<">>> squares: "<<squares->total/3<<endl;
    for( i = 0; i < squares->total; i += 4 )
    {
        j++;
        CvPoint pt[4], *rect = pt;
        int count = 4;
        // read 4 vertices
        CV_READ_SEQ_ELEM( pt[0], reader );
        CV_READ_SEQ_ELEM( pt[1], reader );
        CV_READ_SEQ_ELEM( pt[2], reader );
        CV_READ_SEQ_ELEM( pt[3], reader );
        cvCircle( img, pt[1], 2, CV_RGB(200,0,0),1, 8, 0 );
        //cvCircle( img, pt[1], 3, CV_RGB(200,0,0),1, 8, 0 );
        cvCircle( img, pt[2], 4, CV_RGB(200,0,0),1, 8, 0 );
        //cvCircle( img, pt[3], 3, CV_RGB(200,0,0),1, 8, 0 );
        // draw the square as a closed polyline
        cvLine( img, pt[1], pt[2], CV_RGB(200,0,0),1, 8, 0 );
		
		double angle = abs(pt[1].y-pt[2].y)/sqrt((pt[1].x-pt[2].x)*(pt[1].x-pt[2].x)+(pt[1].y-pt[2].y)*(pt[1].y-pt[2].y)+0.00001);
		char st[255];
		sprintf(st, "%2f", angle);

		//for (int i = 0; i < int( angle*10); i++)   cvRectangle( img, cvPoint((j-1)*30 , i*12+10), cvPoint((j-1)*30+27 , i*12), CV_RGB(255 * angle, 255*(1-angle), 0), -1, 8, 0 );
		
		cvLine( img, cvPoint(0,img->height/2), cvPoint(img->width,img->height/2), CV_RGB(200,200,200),1, 8, 0 );
		cvLine( img, cvPoint(img->width/3,0), cvPoint(img->width/3,img->height), CV_RGB(200,200,200),1, 8, 0 );
		cvLine( img, cvPoint(img->width/3*2,0), cvPoint(img->width/3*2,img->height), CV_RGB(200,200,200),1, 8, 0 );

		cvPutText( img, st, pt[1], &font, CV_RGB(200,0,0));

        cvPolyLine( img, &rect, &count, 1, 1, CV_RGB(200,0,0), 1, CV_AA, 0 );
    }
}
/*==========================================*/
int main(){
    int c = 0;
    CvCapture* capture = cvCaptureFromCAM(0);
		if(!cvQueryFrame(capture)){cout<<"Video capture failed, please check the camera."<<endl;}else{cout<<"Video camera capture status: OK"<<endl;};
    CvSize sz = cvGetSize(cvQueryFrame(capture));
		cvNamedWindow( "out", 0);
		cvNamedWindow( "src", 1);
	//
	IplImage* src = cvCreateImage( sz, 8, 3 );
	IplImage* gray = cvCreateImage( sz, 8, 1 );
	IplImage* pyr = cvCreateImage( cvSize(sz.width/2, sz.height/2), 8, 3 );
	IplImage* tgray = cvCreateImage( sz, 8, 1 );
	//
	CvMemStorage* storage = cvCreateMemStorage(0);
	CvSeq* contours = NULL;
	//
	CvMemStorage* mainStorage = cvCreateMemStorage(0);
	CvMemStorage* cstorage = cvCreateMemStorage( 0);
	CvSeq* circles = NULL;
	//
    while(c != 27)
    {
		IplImage* img = NULL;
		IplImage* out = NULL;
        src = cvQueryFrame( capture);
        img = cvCloneImage( src); out = cvCloneImage( src);
        cvPyrDown( img, pyr, 7 ); cvPyrUp( pyr, img, 7 );
		cvSetImageCOI( img, 2 );
        cvCopy( img, tgray, 0 );
        cvThreshold( tgray, gray, 100, 255, CV_THRESH_BINARY );
			cvShowImage( "src", gray);
		//////
		cvFindContours( gray, storage, &contours, sizeof(CvContour), CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE, cvPoint(0,0) );
		//////
		circles =  cvHoughCircles( gray, cstorage, CV_HOUGH_GRADIENT, 1, gray->height/16, 8, 10, 4, 50 );
			cout <<"Total circles: "<< circles->total <<endl;
		for(  int i  =  0; i < circles-> total; i++ )
		{
			 float* p  =  ( float*)cvGetSeqElem(  circles,  i );
			 cvCircle(  out,  cvPoint( cvRound( p[0]), cvRound( p[1])),  2,  CV_RGB( 200, 0, 0),  -1,  8,  0 );
			 cvCircle(  out,  cvPoint( cvRound( p[0]), cvRound( p[1])),  cvRound( p[2]),  CV_RGB( 200, 0, 0),  1,  8,  0 );
		}
		//////
		drawSquares(out,findSquares4(gray,mainStorage));
		//////
/*
*/
			cvReleaseImage( &img);
			cvShowImage( "out", out);
			cvReleaseImage( &out);
		//
        c = cvWaitKey(10);
		//
    }
    cvReleaseCapture( &capture );
    cvDestroyAllWindows();
}