#include "BlahoDetector.h"
#include <windows.h>

using namespace std;
using namespace ee;

TBlahoDetector::TBlahoDetector()
: faceDetectorFilename( "C:/skola/videoeditor/haarcascade_frontalface_alt.xml"),
	skinDetectorFilename( "C:/skola/videoeditor/skin.dat"),
	skinThreshold( 170),
	areaThreshold( 1000),
	tracker( areaThreshold, skinThreshold, true),
	faceDetectorCascade( (CvHaarClassifierCascade*)cvLoad( faceDetectorFilename.c_str(),0,0,0)),
	storage( NULL),
	skin( skinDetectorFilename.c_str()),
	frameGray( NULL),
	frameSkin( NULL),
	frameSmall( NULL),
	faceDetectionImageScale( 0.66),
	camera(1)
{
	if( faceDetectorCascade == NULL){
		cerr<< "Unable to open face cascade: " << faceDetectorFilename << endl;
//		return -1;
	}
	storage = cvCreateMemStorage( 0);
	cvNamedWindow( AnsiString(AnsiString("skin") + IntToStr(camera)).c_str() );
	cvNamedWindow( AnsiString(AnsiString("result") + IntToStr(camera)).c_str());
}

TBlahoDetector::TBlahoDetector(int Camera)
: faceDetectorFilename( "C:/skola/videoeditor/haarcascade_frontalface_alt.xml"),
	skinDetectorFilename( "C:/skola/videoeditor/skin.dat"),
	skinThreshold( 170),
	areaThreshold( 1000),
	tracker( areaThreshold, skinThreshold, true),
	faceDetectorCascade( (CvHaarClassifierCascade*)cvLoad( faceDetectorFilename.c_str(),0,0,0)),
	storage( NULL),
	skin( skinDetectorFilename.c_str()),
	frameGray( NULL),
	frameSkin( NULL),
	frameSmall( NULL),
	faceDetectionImageScale( 0.66),
	camera(Camera)
{
	if( faceDetectorCascade == NULL){
		cerr<< "Unable to open face cascade: " << faceDetectorFilename << endl;
//		return -1;
	}
	storage = cvCreateMemStorage( 0);
	cvNamedWindow( AnsiString(AnsiString("skin") + IntToStr(camera)).c_str());
	cvNamedWindow( AnsiString(AnsiString("result") + IntToStr(camera)).c_str());
}

void TBlahoDetector::Init()
{
	TParameters Parameters;
	Parameters.push_back(TParameter("Person", "A"));
	InsertMessage(0, "Start Speaking", Parameters);

	Parameters.clear();
	Parameters.push_back(TParameter("X1", "0"));
	Parameters.push_back(TParameter("Y1", "0"));
	Parameters.push_back(TParameter("X2", "640"));
	Parameters.push_back(TParameter("Y2", "0"));
	Parameters.push_back(TParameter("X3", "640"));
	Parameters.push_back(TParameter("Y3", "480"));
	Parameters.push_back(TParameter("X4", "0"));
	Parameters.push_back(TParameter("Y4", "480"));
	InsertMessage(0, "Prezentation Board", Parameters);
}


void TBlahoDetector::Do(int Time, TImg &Img)
{
	IplImage *frame = cvCreateImageHeader( cvSize( Img.GetWidth(), Img.GetHeight()), IPL_DEPTH_8U, 4);
	frame->imageDataOrigin = frame->imageData = (char *)Img.GetBitmap();

	if( !frameGray)
		frameGray = cvCreateImage(cvSize(frame->width,frame->height),IPL_DEPTH_8U,1);

	if(!frameSmall)
		frameSmall = cvCreateImage(cvSize(cvRound(frame->width * faceDetectionImageScale),cvRound(frame->height * faceDetectionImageScale)),IPL_DEPTH_8U,1);

	if(!frameSkin)
		frameSkin = cvCreateImage( cvSize( frame->width, frame->height), IPL_DEPTH_8U, 1);

	cvCvtColor( frame, frameGray, CV_BGRA2GRAY);

	cvResize( frameGray, frameSmall, CV_INTER_LINEAR);


	/* Show skin */
	skin.Eval( frame, frameSkin);
	cvShowImage( AnsiString(AnsiString("skin") + IntToStr(camera)).c_str(), frameSkin);

	/*Track blobs*/
	vector< TrackedState> trackedObjects = tracker.NewFrame( frameSkin);
	
	/*draw blobs*/
	for( vector<TrackedState>::iterator i = trackedObjects.begin(); i != trackedObjects.end(); i++){
		CvPoint p1=cvPoint( i->x - i->rx, i->y - i->ry);
		CvPoint p2=cvPoint( i->x + i->rx, i->y + i->ry);
		
		cvRectangle( frame, p1, p2, cvScalar(0,0,255), 2);
		
		p1=cvPoint( i->x, i->y);
		p2=cvPoint( i->x - i->dx, i->y - i->dy);
			
		cvLine( frame, p1, p2, cvScalar(0,0,255), 2);
	}

	/* Detect faces */
	cvClearMemStorage( storage);
//	double t = (double)cvGetTickCount();
	CvSeq *faces = cvHaarDetectObjects( frameSmall, faceDetectorCascade, storage, 1.1, 2, 0/*CV_HAAR_DO_CANNY_PRUNING*/,cvSize(100, 100));
//	t = (double)cvGetTickCount() - t;
//    cout<<"detection time = "<<t/((double)cvGetTickFrequency()*1000.0)<<endl;
	
	if(faces && faces->total>0){
		for(int i=0;i<(faces?faces->total:0);i++){
			CvRect* r=(CvRect*)cvGetSeqElem(faces, i);
			CvPoint p[2];
			p[0]=cvPoint(cvRound(r->x/faceDetectionImageScale),cvRound(r->y/faceDetectionImageScale));
			p[1]=cvPoint(cvRound((r->x+r->width)/faceDetectionImageScale),cvRound((r->y+r->height)/faceDetectionImageScale));

			for(int k=0;k<2;k++){
				if(p[k].x<0)p[k].x=0;
				if(p[k].y<0)p[k].y=0;
				if(p[k].x>=frame->width)p[k].x=frame->width-1;
				if(p[k].y>=frame->height)p[k].y=frame->height-1;
			}

			int x_border=cvRound(r->width/faceDetectionImageScale*0.25);
			int y_border=cvRound(r->height/faceDetectionImageScale*0.25);

			/*draw rectangle where face was detected*/
			cvRectangle( frame, p[0], p[1], cvScalar(255,0,0), 2);

			/* Add skin color */
			for(int y=p[0].y+y_border;y<p[1].y-y_border;y += 4)
				for(int x=p[0].x+x_border;x<p[1].x-x_border;x++){
					unsigned char *pixel=IMG_ELEM_NU(frame,y,x);

					skin.AddSkin(pixel[R_OFFSET],pixel[G_OFFSET],pixel[B_OFFSET]);
				}
		}

		/* Add background color */
	/*	for(int y=0;y<frame_copy->height;y += 8)
			for(int x=0;x<frame_copy->width;x += 8){
				unsigned char *pixel=IMG_ELEM_NU(frame_copy,y,x);

				skin.AddBackground(pixel[R_OFFSET],pixel[G_OFFSET],pixel[B_OFFSET]);
			}
			*/
		/* Adapt skin model */
		skin.Adapt(1000);
		skin.RememberRatio(0.90);
	}
	cvShowImage( AnsiString(AnsiString("result") + IntToStr(camera)).c_str(), frame);
		if ((camera >= 1 && camera <= 11) && (GetTickCount() % 391 < 7))
		{
			TParameters Parameters;
			Parameters.push_back(TParameter("Object", FloatToStr(float(rand() % 3 + 1))));
			Parameters.push_back(TParameter("CenterX", FloatToStr(float(rand() % 320 + 1))));
			Parameters.push_back(TParameter("CenterY", FloatToStr(float(rand() % 240 + 1))));
			Parameters.push_back(TParameter("DivergenceX", FloatToStr(float(rand() % 100 + 1))));
			Parameters.push_back(TParameter("DivergenceY", FloatToStr(float(rand() % 100 + 1))));
			Parameters.push_back(TParameter("Size", FloatToStr(float(rand() % 300 + 1))));
			InsertMessage(Time, "Teacher", Parameters);
		}

		if ((camera >= 1 && camera <= 11) && (GetTickCount() % 519 < 3))
		{
			TParameters Parameters;
			InsertMessage(Time, "Prezentation Exchange", Parameters);
		}
	cvWaitKey( 10);
}

TBlahoDetector::~TBlahoDetector(){

	if( faceDetectorCascade != NULL){
		//cvRelease( &faceDetectorCascade);
	}

	if( storage != NULL){
		//cvReleasesorage( &storage);
	}

	cvReleaseImage( &frameGray);
	cvReleaseImage( &frameSmall);
	cvReleaseImage( &frameSkin);
}
