#include "cv.h"
#include "highgui.h"
#include <stdio.h>
#include <ctype.h>

#include "./robot.h"
#include "./helper.h"

int main() { 

	initserial();

    CvCapture* capture = 0;
	capture = cvCaptureFromCAM(0);
	cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH, 960);
	cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT, 720);

    IplImage* frame = cvQueryFrame(capture);
	IplImage* hsv = cvCreateImage( cvGetSize(frame), 8, 3 );
	IplImage* work = cvCreateImage( cvGetSize(frame), 8, 1 );
	IplImage* work2 = cvCreateImage( cvGetSize(frame), 8, 1 );

	int scale = 3;

	IplImage* h_plane = cvCreateImage( cvGetSize(frame), 8, 1 );
	IplImage* h_plane_a[] = {h_plane};
	int h_bins = 180;
	int h_bins_a[] = {h_bins};
	float h_ranges[] = {0, 180}; /* hue varies from 0 (~0°red) to 180 (~360°red again) */
	float* h_ranges_a[] = {h_ranges};
	IplImage* h_hist_img = cvCreateImage( cvSize(540, 400), 8, 3 );
	CvHistogram* h_hist;

	IplImage* s_plane = cvCreateImage( cvGetSize(frame), 8, 1 );
	IplImage* s_plane_a[] = {s_plane};
	int s_bins = 255;
	int s_bins_a[] = {s_bins};
	float s_ranges[] = {0, 255}; /* hue varies from 0 (~0°red) to 180 (~360°red again) */
	float* s_ranges_a[] = {s_ranges};
	IplImage* s_hist_img = cvCreateImage( cvSize(scale*s_bins, 400), 8, 3 );
	CvHistogram* s_hist;

	int h, i, j;
	char txt[8];

	int sep_dist, sep_ang, abs_r, abs_phi;
	int radius = frame->height/10;

	CvMemStorage* storage = cvCreateMemStorage(0);
	CvMemStorage* storage1 = cvCreateMemStorage(0);
	CvMemStorage* storage2 = cvCreateMemStorage(0);
	CvSeq* contour = 0;
	int x1, x2, y1, y2, xtot, ytot;
	CvSeq* locations_unsorted = cvCreateSeq(CV_SEQ_ELTYPE_POINT, sizeof(CvSeq), sizeof(CvPoint), storage1);
	CvSeq* locations = cvCreateSeq(CV_SEQ_ELTYPE_POINT, sizeof(CvSeq), sizeof(CvPoint), storage2);

	#define GET_POINT(seq, index) (CvPoint*)cvGetSeqElem( (CvSeq*)(seq), (index) )

	int h_thresh_min = 103, h_thresh_max = 128, s_thresh_min = 102, s_thresh_max = 255;
	int gauss_rad = 5, gauss_dev = 15;
	int canny_1 = 1, canny_2 = 1, canny_ap = 1;
	int approx_poly = 10;

	CvFont fontstyle;
	cvInitFont(&fontstyle, CV_FONT_HERSHEY_SIMPLEX, 0.5, 0.5, 0, 1, 8);

    if( !capture )
    {
        fprintf(stderr,"Could not initialize capturing...\n");
        return -1;
    }
        
    cvNamedWindow("Raw", 0);
    cvNamedWindow("Raw1", 0);
    cvNamedWindow("Raw2", 0);
    cvNamedWindow("Hist Hue", 0);
    cvNamedWindow("Hist Sat", 0);
	cvResizeWindow("Raw", 960, 720); cvMoveWindow("Raw", 1920, 0);
	cvResizeWindow("Raw1", 800, 720);
	cvResizeWindow("Raw2", 800, 720); cvMoveWindow("Raw2", 800, 0);
	cvResizeWindow("Hist Hue", 300, 300); cvMoveWindow("Hist Hue", 0, 800);
	cvResizeWindow("Hist Sat", 300, 300); cvMoveWindow("Hist Sat", 800, 800);
	

	cvCreateTrackbar("Min", "Hist Hue", &h_thresh_min, 180, NULL);
	cvCreateTrackbar("Max", "Hist Hue", &h_thresh_max, 180, NULL);
	cvCreateTrackbar("Min", "Hist Sat", &s_thresh_min, 255, NULL);
	cvCreateTrackbar("Max", "Hist Sat", &s_thresh_max, 255, NULL);
	cvCreateTrackbar("Gaussian Radius", "Raw1", &gauss_rad, 10, NULL);
	cvCreateTrackbar("Gaussian Deviation", "Raw1", &gauss_dev, 30, NULL);
	cvCreateTrackbar("Canny P1", "Raw2", &canny_1, 300, NULL);
	cvCreateTrackbar("Canny P2", "Raw2", &canny_2, 300, NULL);
	cvCreateTrackbar("Canny App", "Raw2", &canny_ap, 6, NULL);
	cvCreateTrackbar("Approx Poly", "Raw", &approx_poly, 100, NULL);

    while (1) {
		
		
        frame = cvQueryFrame(capture);
		cvHorizFlip(frame, frame);


		// Get Hue matrix, and calculate it's histogram
		cvCvtColor(frame, hsv, CV_BGR2HSV);
		cvCvtPixToPlane(hsv, h_plane, 0, 0, 0);
		cvCvtPixToPlane(hsv, 0, s_plane, 0, 0);
		h_hist = cvCreateHist(1, h_bins_a, CV_HIST_ARRAY, h_ranges_a, 1);
		cvCalcHist(h_plane_a, h_hist, 0, 0);
		cvZero(h_hist_img);
		s_hist = cvCreateHist(1, s_bins_a, CV_HIST_ARRAY, s_ranges_a, 1);
		cvCalcHist(s_plane_a, s_hist, 0, 0);
		cvZero(s_hist_img);

		// Smoothing helps a lot
		cvSmooth(h_plane, h_plane, CV_GAUSSIAN, gauss_rad*2+1, gauss_rad*2+1, (double)(gauss_dev)/10+0.1, 0);
		cvSmooth(s_plane, s_plane, CV_GAUSSIAN, gauss_rad*2+1, gauss_rad*2+1, (double)(gauss_dev)/10+0.1, 0);

		// Draw the Histogram, along with threshhold lines
		cvHistogramDraw(h_hist, h_hist_img, h_bins, CV_RGB(0xFF, 0, 0));
		cvHistogramMarker(h_hist_img, h_thresh_min, scale, CV_RGB(0, 0xFF, 0));
		cvHistogramMarker(h_hist_img, h_thresh_max, scale, CV_RGB(0, 0xFF, 0));
		cvHistogramDraw(s_hist, s_hist_img, s_bins, CV_RGB(0xFF, 0, 0));
		cvHistogramMarker(s_hist_img, s_thresh_min, scale, CV_RGB(0, 0xFF, 0));
		cvHistogramMarker(s_hist_img, s_thresh_max, scale, CV_RGB(0, 0xFF, 0));

		// Filter out not blue
		cvDoubleThreshold(h_plane, h_plane, h_thresh_min, h_thresh_max, 0xFF, 180);
		cvDoubleThreshold(s_plane, s_plane, s_thresh_min, s_thresh_max, 0xFF, 255);
		cvMax(h_plane, s_plane, work);

		// This gets rid of a lot of noise
		cvSmooth(work, work, CV_GAUSSIAN, gauss_rad*2+1, gauss_rad*2+1, (double)(gauss_dev)/10+0.1, 0);
		cvThreshold(work, work, 100, 0xFF, CV_THRESH_BINARY);
		cvSmooth(work, work, CV_GAUSSIAN, gauss_rad*2+1, gauss_rad*2+1, (double)(gauss_dev)/10+0.1, 0);
		cvThreshold(work, work, 100, 0xFF, CV_THRESH_BINARY);
		cvSmooth(work, work, CV_GAUSSIAN, gauss_rad*2+1, gauss_rad*2+1, (double)(gauss_dev)/10+0.1, 0);
		cvThreshold(work, work, 100, 0xFF, CV_THRESH_BINARY);
		cvSmooth(work, work, CV_GAUSSIAN, gauss_rad*2+1, gauss_rad*2+1, (double)(gauss_dev)/10+0.1, 0);
		cvThreshold(work, work, 100, 0xFF, CV_THRESH_BINARY);

		// Find edges
		cvCanny(work, work2, canny_1, canny_2, canny_ap*2+1);

		cvClearMemStorage(storage); 
		cvFindContours(work2, storage, &contour, sizeof(CvContour), CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE, cvPoint(0,0));
		CvScalar color = CV_RGB(0xFF, 0x00, 0x00);
		cvZero(hsv);
		cvClearSeq(locations_unsorted);
		cvClearSeq(locations);
		cvMerge(work, 0, 0, 0, hsv);
		for( ; contour != 0; contour = contour->h_next ) {
			CvSeq* result = cvApproxPoly(contour, sizeof(CvContour), storage, CV_POLY_APPROX_DP, (double)(approx_poly)/10, 0);
			if (fabs(cvContourArea(result,CV_WHOLE_SEQ)) > 60) {
				xtot = 0;
				ytot = 0;
				for (h = 0; h < result->total; h ++) {
					x1 = (int)(((CvPoint*)cvGetSeqElem(result, h-1))->x);
					y1 = (int)(((CvPoint*)cvGetSeqElem(result, h-1))->y);
					x2 = (int)(((CvPoint*)cvGetSeqElem(result, h))->x);
					y2 = (int)(((CvPoint*)cvGetSeqElem(result, h))->y);
					xtot += x1;
					ytot += y1;
					cvLine(hsv, cvPoint(x1,y1), cvPoint(x2,y2), color, 1, 8, 0);
				}
				xtot /= result->total;
				ytot /= result->total;

				CvPoint p = cvPoint(xtot, ytot);
				cvSeqPush(locations_unsorted, (void*)(&p));

			}
		}
		
		cvDrawRegions(frame, radius);
		cvSeqSortByDist(locations_unsorted, locations);
		if (locations->total >= 2) {
			CvPoint O = cvPoint(0,0);
			CvPoint* p1 = (CvPoint*)cvGetSeqElem(locations, 0);
			CvPoint* p2 = (CvPoint*)cvGetSeqElem(locations, 1);
			CvPoint p3 = cvPoint((p1->x + p2->x - frame->width)/2, (p1->y + p2->y - frame->height)/2);
			cvDrawLocation(frame, p1, p2);
			sep_dist = cvDistance(p1, p2);	
			sep_ang = cvAngle(p1, p2);	
			abs_r = cvDistance(&O, &p3);
			abs_phi = cvAngle(&O, &p3);
			cvPutTextInt(frame, sep_ang, cvPoint(4, 30), &fontstyle, color);
			cvPutTextInt(frame, sep_dist, cvPoint(300, 30), &fontstyle, color);
			cvPutTextInt(frame, abs_phi, cvPoint(4, 50), &fontstyle, color);
			cvPutTextInt(frame, abs_r, cvPoint(300, 50), &fontstyle, color);

			if		(sep_dist > 50)		pincer_open();
			else						pincer_close();
	
			if 		(sep_ang > 90+20)	set_base_angle_mov(1);
			else if (sep_ang < 90-20)	set_base_angle_mov(255);
			else 						stop_base_angle_mov();
			
			if 		(between(abs_phi, 22.5, 157.5) && abs_r > radius)	set_upper_mov(256-decide_speed_arm(radius, abs_r));
			else if (between(abs_phi, 202.5, 337.5) && abs_r > radius) 	set_upper_mov(decide_speed_arm(radius, abs_r));
			else														stop_upper_mov();
	
			if		(between(abs_phi, 292.5, 67.5) && abs_r > radius)		set_lower_mov(256-decide_speed_arm(radius, abs_r)); 
			else if (between(abs_phi, 112.5, 247.5) && abs_r > radius)		set_lower_mov(decide_speed_arm(radius, abs_r));
			else															stop_lower_mov();

			move();
		}
		

        cvShowImage("Raw", frame);
        cvShowImage("Raw1", work);
        cvShowImage("Raw2", hsv);

        cvShowImage("Hist Hue", h_hist_img);
        cvShowImage("Hist Sat", s_hist_img);

        if( cvWaitKey(10) >= 0 )
            break;
    }

    cvReleaseCapture( &capture );
	cvReleaseMemStorage(&storage);
	cvReleaseMemStorage(&storage1);
	cvReleaseMemStorage(&storage2);
	cvDestroyAllWindows();

    return 0;
}