#include <iostream>
#include <opencv2\opencv.hpp>
#include <XnCppWrapper.h>
#include "BlobResult.h"
using namespace xn;
using namespace cv;
using namespace std;

Context context;
ImageGenerator imageGen;
const Size frameSize(640, 480);
Mat bgrMat(frameSize, CV_8UC3);

IplImage* getThresholdedImageHSV(IplImage* img)
{
	IplImage* imgHSV = cvCreateImage(cvGetSize(img), 8, 3);
	cvCvtColor(img, imgHSV, CV_BGR2HSV);

	IplImage* imgThreshHSV = cvCreateImage(cvGetSize(img), 8, 1);
	cvInRangeS(imgHSV, cvScalar(170,160,60), cvScalar(180,256,256), imgThreshHSV);
	cvReleaseImage(&imgHSV);
	return imgThreshHSV;
}

int main()
{
	/*
	The algorithm is quite simple:
	1.Resize the captured image (width/4,height/4, this will reduce a lot of the noise and will speed up the processing time)
	2. Obtain the binary image with cvInRangeS (not with cvThreshold)
	3. Dilate the image a couple of times 
	4. Use the functions that cvblobslib library provide to detect blobs,
		filter small blobs and so on... (you can find sample code on how to do that)
	5. Draw a rectangle around the biggest blob on the original image(taking in consideration the resizing factor
	*/
	// Initialize context object.
	XnStatus nRetVal = XN_STATUS_OK;
	nRetVal = context.Init();

	// Defauly output mode.
	XnMapOutputMode outputMode;
	outputMode.nXRes = 640;
	outputMode.nYRes = 480;
	outputMode.nFPS = 30;

	// Create an imageGenerator node.
	nRetVal = imageGen.Create(context);
	nRetVal = imageGen.SetMapOutputMode(outputMode);
	nRetVal = imageGen.GetMirrorCap().SetMirror(true);

	// Start generating.
	nRetVal = context.StartGeneratingAll();

	// Get the first frame from the camera.
	Mat mat(frameSize, CV_8UC3, (unsigned char*)imageGen.GetImageMap());

	// Window.
	cvNamedWindow("ColorBasedTracking");
	cvNamedWindow("HSV");
	cvNamedWindow("Result");

	// To hold each frame.
	IplImage *frame ;

	while(true)
	{
		Mat mat(frameSize, CV_8UC3, (unsigned char*) imageGen.GetImageMap());
		cvtColor(mat, bgrMat, CV_RGB2BGR);
		frame = cvCloneImage(&(IplImage)bgrMat);
		
		//smooth the original image using Gaussian kernel
		cvSmooth(frame, frame, CV_GAUSSIAN,3,3); 
		
		if(!frame)
			break;

		
		
		IplImage *imgThresh = getThresholdedImageHSV(frame);
		//smooth the binary image using Gaussian kernel.
		cvSmooth(imgThresh, imgThresh, CV_GAUSSIAN, 3, 3); 
		

		// Find the blobs.
		IplImage *blobsImage = cvCreateImage(cvGetSize(frame), 8, 3);
		CBlobResult blobs;
		
		// Extract the blobs using a threshold, discard the blobs with area less than 200 pixels.
		blobs = CBlobResult(imgThresh, NULL, 0);
		blobs.Filter(blobs, B_EXCLUDE, CBlobGetArea(), B_LESS, 200);
		
				
		// Get the biggest blob
		CBlob biggestBlob;		
		blobs.GetNthBlob(CBlobGetArea(), 0, biggestBlob);		
		biggestBlob.FillBlob(blobsImage, CV_RGB(0, 0, 255));

		// Get the bounding box.
		CvRect rect = biggestBlob.GetBoundingBox();
		cvRectangle(frame, cvPoint(rect.x, rect.y), cvPoint(rect.x + rect.width, rect.y + rect.height), cvScalar(0, 0, 255), 1, 8, 0);
		
		
		/*
		// display filtered blobs
		CBlob *currentBlob = blobs.GetBlob(0);for (int i = 0; i < blobs.GetNumBlobs(); i++)
		{
			currentBlob = blobs.GetBlob(i);
			currentBlob->FillBlob(blobsImage, CV_RGB(0, 255, 0));
			currentBlob->GetEllipse();
		}
		*/

		// Display images inside windows.
		cvShowImage("ColorBasedTracking", frame);
		cvShowImage("Result", blobsImage);	
		cvShowImage("HSV", imgThresh);

		// Wait for a keypress.
		int c = cvWaitKey(10);
		if(c != -1)
			break;	


		// Release the images.
		cvReleaseImage(&frame);
		cvReleaseImage(&imgThresh);
		cvReleaseImage(&blobsImage);
	}

	return 0;
}