
#include "stdafx.h"
#include "FaceAnim.h"

#include "FaceAnimDoc.h"
#include "FaceAnimView.h"

#include "klt/klt.h"

#ifdef _DEBUG
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////////
inline float sqr(float fVal) { return(fVal*fVal);}

//////////////////////////////////////////////////////////////////////////
void CFaceAnimDoc::allocateOnDemand2( IplImage **img, CvSize size, int depth, int channels )
{
	if ( *img != NULL )	return;

	*img = cvCreateImage( size, depth, channels );
	if ( *img == NULL )
	{
		fprintf(stderr, "Error: Couldn't allocate image.  Out of memory?\n");
		exit(-1);
	}
}

//////////////////////////////////////////////////////////////////////////
bool CFaceAnimDoc::CalibrationAVI3(const char *_szFilename)
{	
	// Create an object that decodes the input video stream. 
	CvCapture *input_video = cvCaptureFromAVI(_szFilename);
	if (!input_video)
	{
		// Either the video didn't exist OR it uses a codec OpenCV
		// doesn't support.					
		MyError("Cannot open file %s \n",_szFilename);
		return false;
	}

	// This is a hack. If we don't call this first then getting capture
	// properties (below) won't work right. This is an OpenCV bug. We
	// ignore the return value here. But it's actually a video frame.	
	cvQueryFrame( input_video );

	// Read the video's frame size out of the AVI. 
	CvSize frame_size;
	frame_size.height =(int) cvGetCaptureProperty( input_video, CV_CAP_PROP_FRAME_HEIGHT );
	frame_size.width =(int) cvGetCaptureProperty( input_video, CV_CAP_PROP_FRAME_WIDTH );
	//CvSize frame_size_small;
	//frame_size_small.width=320;
	//frame_size_small.height=240;

	float fScaleX=1.0f; //(float)(frame_size_small.width)/(float)(frame_size.width);
	float fScaleY=1.0f; //(float)(frame_size_small.height)/(float)(frame_size.height);

	// Determine the number of frames in the AVI. 
	long number_of_frames;
	// Go to the end of the AVI (ie: the fraction is "1") 
	cvSetCaptureProperty( input_video, CV_CAP_PROP_POS_AVI_RATIO, 1. );
	// Now that we're at the end, read the AVI position in frames 
	number_of_frames = (int) cvGetCaptureProperty( input_video, CV_CAP_PROP_POS_FRAMES );
	// Return to the beginning 
	cvSetCaptureProperty( input_video, CV_CAP_PROP_POS_FRAMES, 0. );

	cvNamedWindow("FaceAnim", CV_WINDOW_AUTOSIZE);

	
	//////////////////////////////////////////////////////////////////////////
	CvMemStorage* storage = 0;
	CvHaarClassifierCascade* cascadeface = 0;
	//const char *cascade_name = "data/haarcascades/haarcascade_frontalface_alt2.xml";
	const char *cascadeface_name = "E:/Dev/Main/Code/Tools/FaceAnim/Bin/Data/haarcascades/haarcascade_frontalface_alt2.xml";
	cascadeface = (CvHaarClassifierCascade*)cvLoad( cascadeface_name, 0, 0, 0 );
	storage = cvCreateMemStorage(0);

	const char *cascadeeyes_name = "E:/Dev/Main/Code/Tools/FaceAnim/Bin/Data/haarcascades/parojos.xml";
	CvHaarClassifierCascade*cascadeeyes = (CvHaarClassifierCascade*)cvLoad( cascadeeyes_name, 0, 0, 0 );

	const char *cascade_righteye_name = "E:/Dev/Main/Code/Tools/FaceAnim/Bin/Data/haarcascades/ojoD.xml";
	CvHaarClassifierCascade *cascade_righteye = (CvHaarClassifierCascade*)cvLoad( cascade_righteye_name, 0, 0, 0 );

	const char *cascade_lefteye_name = "E:/Dev/Main/Code/Tools/FaceAnim/Bin/Data/haarcascades/ojoI.xml";
	CvHaarClassifierCascade *cascade_lefteye = (CvHaarClassifierCascade*)cvLoad( cascade_lefteye_name, 0, 0, 0 );

	const char *cascade_mouth_name = "E:/Dev/Main/Code/Tools/FaceAnim/Bin/Data/haarcascades/boca.xml";
	CvHaarClassifierCascade *cascade_mouth = (CvHaarClassifierCascade*)cvLoad( cascade_mouth_name, 0, 0, 0 );

	//////////////////////////////////////////////////////////////////////////

	static IplImage *frame = NULL, *frame1 = NULL, *frame1_1C = NULL, *frame2_1C = NULL, *eig_image = NULL, *temp_image = NULL, *pyramid1 = NULL, *pyramid2 = NULL;

	CvScalar colors[] = 
	{
		{{0,0,255}},
		{{0,128,255}},
		{{0,255,255}},
		{{0,255,0}},
		{{255,128,0}},
		{{255,255,0}},
		{{255,0,0}},
		{{255,0,255}}
	};

	KLT_TrackingContext m_tc;
	KLT_FeatureList m_fl;
	KLT_FeatureTable m_ft;

	const int nMaxFeatures=64;

	m_tc = KLTCreateTrackingContext();		
	m_fl = KLTCreateFeatureList(nMaxFeatures);
	m_ft = KLTCreateFeatureTable(1, nMaxFeatures);  

	CvFont font1;
	cvInitFont( &font1, CV_FONT_HERSHEY_SIMPLEX, 0.35, 0.35);  		
	
	long current_frame = 0;
	char szText[256];
	while(true)
	{		

		cvSetCaptureProperty( input_video, CV_CAP_PROP_POS_FRAMES, current_frame );

		// first frame
		frame = cvQueryFrame( input_video );

		// We'll make a full color backup of this frame so that we can draw on it.
		// (It's not the best idea to draw on the static memory space of cvQueryFrame().)
		allocateOnDemand2( &frame1, frame_size, 8, 3 );
		//cvConvertImage(frame, frame1, CV_CVTIMG_FLIP);		
		cvConvertImage(frame, frame1, 1);		

		//allocateOnDemand2( &temp_image, frame_size, 8, 1 );
		//cvConvertImage(frame, temp_image, 1);
		allocateOnDemand2( &frame1_1C, frame_size, 8, 1 );
		//cvCvtColor( frame, frame1_1C, CV_RGB2GRAY );
		cvConvertImage(frame, frame1_1C, 1);				
		
		//cvCanny( frame1_1C, frame1_1C, 50, 200);		
		//cvConvertImage(frame1_1C, frame1, 0);				
		// copy again
		//cvConvertImage(frame, frame1_1C, 1);				

		uchar *pGray1=(uchar *)frame1_1C->imageData;

		//KLTSelectGoodFeatures(m_tc, pGray1, frame_size.width, frame_size.height, m_fl);			
		//KLTStoreFeatureList(m_fl, m_ft, 0);

		// equalize for face detection etc.
		cvEqualizeHist( frame1_1C, frame1_1C );
		cvClearMemStorage( storage );

		cvConvertImage(frame1_1C, frame1, 0);				

		sprintf(szText,"%d/%d",current_frame,number_of_frames);
		cvPutText( frame1, szText,cvPoint(10,20), &font1, cvScalar(255,0,0) );

		//////////////////////////////////////////////////////////////////////////
		// face

		double t = (double)cvGetTickCount();
		CvSeq* faces = cvHaarDetectObjects( frame1_1C, cascadeface, storage,
			//1.1, 2, 0/*CV_HAAR_DO_CANNY_PRUNING*/,
			1.1, 3, 0/*CV_HAAR_DO_CANNY_PRUNING*/
			//,cvSize(30, 30) );
			//,cvSize(20, 20) );
			);
		t = (double)cvGetTickCount() - t;
		//MyOutputDebugString( "detection time = %gms\n", t/((double)cvGetTickFrequency()*1000.) );
		if (faces && faces->total==1)
		{		
			float scale=1.0f;
			//for(int i = 0; i < (faces ? faces->total : 0); i++ )

			CvRect* r = (CvRect*)cvGetSeqElem( faces, 0 );
			// copy cos its sharing data pointers
			CvRect	face_rect=cvRect(r->x,r->y,r->width,r->height);
			CvPoint face_center;
			int radius;
			face_center.x = cvRound((r->x + r->width*0.5)*fScaleX);
			face_center.y = cvRound((r->y + r->height*0.5)*fScaleY);
			radius = cvRound((r->width + r->height)*0.25*fScaleX);
			cvCircle( frame1, face_center, radius, colors[0], 3, 8, 0 );

			cvDrawRect(frame1,cvPoint(r->x,r->y),cvPoint(r->x+r->width,r->y+r->height),colors[0]);

			CvSize face_size;
			face_size.width=r->width;
			face_size.height=r->height;
			IplImage *pTemp=cvCreateImage( face_size, 8, 1 );			
			cvGetRectSubPix(frame1_1C,pTemp,cvPoint2D32f(face_center.x,face_center.y));
			//IplImage *pTemp=cvCreateImage( frame_size, 8, 1 );			
			//cvCopyImage(frame1_1C,pTemp);
 
			//////////////////////////////////////////////////////////////////////////
			// features
			for (int k=0;k<m_ft->nFeatures;k++)  
			{				
				if (m_ft->feature[k][0]->val<0)
					continue;		

				float fX=m_ft->feature[k][0]->x*fScaleX;
				float fY=m_ft->feature[k][0]->y*fScaleY;		

				float fDist2=sqr(fX-face_center.x)+sqr(fY-face_center.y);
				if (fDist2>radius*radius)
					continue;

				cvRectangle(frame1,cvPoint((int)(fX)-1,(int)(fY)-1),cvPoint((int)(fX)+1,(int)(fY)+1),CV_RGB(0,255,0));
			} //k


			//////////////////////////////////////////////////////////////////////////
			// left eye

			cvClearMemStorage( storage );
			t = (double)cvGetTickCount();
			CvSeq* eyeL = cvHaarDetectObjects( pTemp, cascade_lefteye, storage,
				//1.1, 2, 0//CV_HAAR_DO_CANNY_PRUNING,
				1.1, 3, 0//CV_HAAR_DO_CANNY_PRUNING
				//,cvSize(30, 30) );
				//,cvSize(20, 20) );
				);
 
			CvRect	eye_bestL;
			int		bestyposL=-1000;
			bool bFoundL=false;
			for(int i = 0; i < (eyeL ? eyeL->total : 0); i++ )
			{			
				CvRect* r2 = (CvRect*)cvGetSeqElem( eyeL, i );

				CvPoint eye_center;				
				eye_center.x = face_rect.x+cvRound((r2->x + r2->width*0.5)*fScaleX);
				eye_center.y = face_rect.y+cvRound((r2->y + r2->height*0.5)*fScaleY);
				if (eye_center.y>face_center.y)
					continue; // mouth or nose

				if (eye_center.x>face_center.x)
					continue; // right eye

				// copy cos its sharing data pointers
				CvRect	eyeRect=cvRect(r2->x,r2->y,r2->width,r2->height);
				if (eye_center.y>bestyposL)
				{
					eye_bestL=eyeRect;
					bestyposL=eye_center.y;
				}			
				bFoundL=true;
			} //i

			if (bFoundL)
			{	
				cvDrawRect(frame1,cvPoint(face_rect.x+eye_bestL.x,face_rect.y+eye_bestL.y),cvPoint(face_rect.x+eye_bestL.x+eye_bestL.width,face_rect.y+eye_bestL.y+eye_bestL.height),colors[1]);
				CvRect	eyeRect2=cvRect(face_rect.x+eye_bestL.x,face_rect.y+eye_bestL.y,eye_bestL.width,eye_bestL.height);
			}

			//////////////////////////////////////////////////////////////////////////
			// right eye
						
			cvClearMemStorage( storage );
			t = (double)cvGetTickCount();
			CvSeq* eyeR = cvHaarDetectObjects( pTemp, cascade_righteye, storage,
				//1.1, 2, 0//CV_HAAR_DO_CANNY_PRUNING,
				1.1, 3, 0//CV_HAAR_DO_CANNY_PRUNING
				//,cvSize(30, 30) );
				//,cvSize(20, 20) );
				);

			CvRect	eye_bestR;
			int		bestyposR=-1000;
			bool bFoundR=false;

			for(int i = 0; i < (eyeR ? eyeR->total : 0); i++ )
			{			
				CvRect* r2 = (CvRect*)cvGetSeqElem( eyeR, i );

				CvPoint eye_center;				
				eye_center.x = face_rect.x+cvRound((r2->x + r2->width*0.5)*fScaleX);
				eye_center.y = face_rect.y+cvRound((r2->y + r2->height*0.5)*fScaleY);
				if (eye_center.y>face_center.y)
					continue; // mouth or nose

				if (eye_center.x<face_center.x)
					continue; // left eye

				// copy cos its sharing data pointers
				CvRect	eyeRect=cvRect(r2->x,r2->y,r2->width,r2->height);
				if (eye_center.y>bestyposR)
				{
					eye_bestR=eyeRect;
					bestyposR=eye_center.y;
				}			
				bFoundR=true;				
			} //i
			
			if (bFoundR)
			{	
				cvDrawRect(frame1,cvPoint(face_rect.x+eye_bestR.x,face_rect.y+eye_bestR.y),cvPoint(face_rect.x+eye_bestR.x+eye_bestR.width,face_rect.y+eye_bestR.y+eye_bestR.height),colors[2]);
			}

			//////////////////////////////////////////////////////////////////////////
			// mouth

			cvClearMemStorage( storage );
			t = (double)cvGetTickCount();
			CvSeq* mouth = cvHaarDetectObjects( pTemp, cascade_mouth, storage,
				//1.1, 2, 0//CV_HAAR_DO_CANNY_PRUNING,
				1.1, 3, 0//CV_HAAR_DO_CANNY_PRUNING
				//,cvSize(30, 30) );
				//,cvSize(20, 20) );
				);

			CvRect	mouth_best;
			int		bestypos_mouth=-1000;
			bool bFoundMouth=false;

			for(int i = 0; i < (mouth ? mouth->total : 0); i++ )
			{			
				CvRect* r2 = (CvRect*)cvGetSeqElem( mouth, i );

				CvPoint mouth_center;				
				mouth_center.x = face_rect.x+cvRound((r2->x + r2->width*0.5)*fScaleX);
				mouth_center.y = face_rect.y+cvRound((r2->y + r2->height*0.5)*fScaleY);
				//if (mouth_center.y<face_center.y)
				if (face_rect.y+r2->y<face_center.y)
					continue; // nose

				// copy cos its sharing data pointers
				CvRect	mouthRect=cvRect(r2->x,r2->y,r2->width,r2->height);
				if (mouth_center.y>bestypos_mouth)
				{
					mouth_best=mouthRect;
					bestypos_mouth=mouth_center.y;
				}			
				bFoundMouth=true;				
			} //i
 
			if (bFoundMouth)
			{	
				cvDrawRect(frame1,cvPoint(face_rect.x+mouth_best.x,face_rect.y+mouth_best.y),cvPoint(face_rect.x+mouth_best.x+mouth_best.width,face_rect.y+mouth_best.y+mouth_best.height),colors[4]);
			}

			if (bFoundR && bFoundL)
			{
				CvPoint eye_centerL;				
				eye_centerL.x = face_rect.x+cvRound((eye_bestL.x + eye_bestL.width*0.5)*fScaleX);
				eye_centerL.y = face_rect.y+cvRound((eye_bestL.y + eye_bestL.height*0.5)*fScaleY);

				CvPoint eye_centerR;				
				eye_centerR.x = face_rect.x+cvRound((eye_bestR.x + eye_bestR.width*0.5)*fScaleX);
				eye_centerR.y = face_rect.y+cvRound((eye_bestR.y + eye_bestR.height*0.5)*fScaleY);

				int w=frame_size.width;
				int h=frame_size.height;

				float fDirX=(float)(eye_centerR.x-eye_centerL.x);
				float fDirY=(float)(eye_centerR.y-eye_centerL.y);
				float fLen=sqrt(fDirX*fDirX+fDirY*fDirY);
				fDirX/=fLen;
				fDirY/=fLen;
				CvPoint eye_dirL;				
				eye_dirL.x=eye_centerL.x-(int)(fDirX*w/4);
				eye_dirL.y=eye_centerL.y-(int)(fDirY*h/4);
				CvPoint eye_dirR;				
				eye_dirR.x=eye_centerR.x+(int)(fDirX*w/4);
				eye_dirR.y=eye_centerR.y+(int)(fDirY*h/4);

				cvDrawLine(frame1,eye_dirL,eye_dirR,colors[3]);
				
				if (bFoundMouth)				
				{
					//////////////////////////////////////////////////////////////////////////
					// face orientation

					CvPoint eye_middle;				
					eye_middle.x = (int)((eye_centerR.x+eye_centerL.x)*0.5f);
					eye_middle.y = (int)((eye_centerR.y+eye_centerL.y)*0.5f);
					
					CvPoint mouth_center;				
					mouth_center.x = face_rect.x+cvRound((mouth_best.x + mouth_best.width*0.5)*fScaleX);
					mouth_center.y = face_rect.y+cvRound((mouth_best.y + mouth_best.height*0.5)*fScaleY);

					fDirX=(float)(mouth_center.x-eye_middle.x);
					fDirY=(float)(mouth_center.y-eye_middle.y);
					fLen=sqrt(fDirX*fDirX+fDirY*fDirY);
					fDirX/=fLen;
					fDirY/=fLen;
					eye_dirL;				
					eye_dirL.x=eye_middle.x-(int)(fDirX*w/4);
					eye_dirL.y=eye_middle.y-(int)(fDirY*h/4);
					eye_dirR;				
					eye_dirR.x=mouth_center.x+(int)(fDirX*w/4);
					eye_dirR.y=mouth_center.y+(int)(fDirY*h/4);

					cvDrawLine(frame1,eye_dirL,eye_dirR,colors[3]);
				}
				
				/*
				cvClearMemStorage( storage );
				// smooth it, otherwise a lot of false circles may be detected
				cvSmooth( pTemp, pTemp, CV_GAUSSIAN, 9, 9 ); 
				CvSeq* circles = cvHoughCircles( pTemp, storage, CV_HOUGH_GRADIENT, 2, pTemp->height/16  );
				int i;
				for( i = 0; i < circles->total; i++ )
				{
					float* p = (float*)cvGetSeqElem( circles, i );
					cvCircle( frame1, cvPoint(cvRound(p[0]),cvRound(p[1])), 3, CV_RGB(0,255,0), -1, 8, 0 );
					cvCircle( frame1, cvPoint(cvRound(p[0]),cvRound(p[1])), cvRound(p[2]), CV_RGB(255,0,0), 3, 8, 0 );
				}	
				*/

			}


			cvReleaseImage(&pTemp);
		}
		
		cvShowImage("FaceAnim", frame1);
		//cvShowImage("FaceAnim", frame1_1C);

		int key_pressed;
		key_pressed = cvWaitKey(0);

		if (key_pressed == 'b' || key_pressed == 'B')	
			current_frame--;
		else											
			//current_frame++;
			current_frame+=10;
		// Don't run past the front/end of the AVI. 
		if (current_frame < 0)						
			current_frame = 0;
		if (current_frame >= number_of_frames - 1)	
		{
			//break;
			//current_frame = number_of_frames - 2;
			current_frame = 0;
		}
		if (key_pressed=='x' || key_pressed=='X')
			break;
	}

	KLTFreeFeatureTable(m_ft);	
	KLTFreeFeatureList(m_fl);	
	KLTFreeTrackingContext(m_tc);		

	cvReleaseMemStorage(&storage);

	// (It's not the best idea to draw on the static memory space of cvQueryFrame().)
	//cvReleaseImage(&frame);
	cvReleaseImage(&frame1);
	cvReleaseImage(&frame1_1C);
	//cvReleaseImage(&frame2_1C);

	cvvDestroyWindow("FaceAnim");
	cvReleaseCapture(&input_video);


	return (true);
}
