#include <stdio.h>
#include <cv.h>
#include <highgui.h>
#include <math.h>
#include <highgui.h>

// 学習結果のデータファイル
#define CASCADE_FILE "data/haarcascade_frontalface_alt.xml"
#define GRAPH_SCALE 0.03
#define NOT_ENOUGH(x)	(x > 30)

// 追跡ようのデータ
typedef struct MyTracker{
	CvSize size;		    // 追跡

	IplImage *graph;

	CvPanTilt dangle;
	CvPanTiltRange range;

	int catch_flag,find_flag;
	int vpan,vtilt;
	int lost_count;
	CvMemStorage* storage ;
	CvSeq *seq;
} MyTracker;

typedef struct FaceDetect{
	// src image
	IplImage *frame;
	CvPoint result;
	
	// for face-tracker
	CvHaarClassifierCascade *cascade ;
	CvMemStorage* storage ;
} FaceDetect;

int pantilt_linear_predict(CvSeq *points,int *vpan,int *vtilt,CvPanTilt *p){
	CvPanTilt p1,p2;

	if(points->total < 2 ){
		return -1;
	}else{
		p1 = *(CvPanTilt *)cvGetSeqElem(points,points->total - 1);
		p2 = *(CvPanTilt *)cvGetSeqElem(points,points->total - 2);

		*vpan = ((p1.pan - p2.pan) < 500)? p1.pan-p2.pan: 0;
		*vtilt =((p1.tilt - p2.tilt)<500)? p1.tilt-p2.tilt: 0 ;
		p->pan = p1.pan + *vpan ;
		p->tilt = p1.tilt + *vtilt;
		
		return 0;
	}

}
int detect_and_draw( struct FaceDetect *detect)
{
	int scale = 1;
	int i;
	CvPoint pt1,pt2;
	int area = 0;
	
	cvClearMemStorage( detect->storage );

	if( detect->cascade )
	{
		// 顔発見
		CvSeq* faces = cvHaarDetectObjects( detect->frame, 
						    detect->cascade, 
						    detect->storage,
						    1.1, 2, CV_HAAR_DO_CANNY_PRUNING,
						    cvSize(40, 40) );
		//発見したものを四角で囲む
		for( i = 0; i < (faces ? faces->total : 0); i++ )
		{
			CvRect* r = (CvRect*)cvGetSeqElem( faces, i );
			pt1.x = r->x*scale;
			pt2.x = (r->x+r->width)*scale;
			pt1.y = r->y*scale;
			pt2.y = (r->y+r->height)*scale;
			cvRectangle( detect->frame, pt1, pt2, 
				     CV_RGB(255,0,0), 3, 8, 0 );

			if(area < (r->width * r->height)){
				area = r->width * r->height;
				detect->result.x = r->x + r->width/2;
				detect->result.y = r->y + r->height/2;
			}
		}
		if(faces->total){
			// 発見した顔領域の中心点に印
			cvCircle(detect->frame,detect->result,5,
				 CV_RGB(0,255,0),CV_FILLED,8,0);
		}
		return faces->total;
	}
}

//現在の角度、顔の点、画像の大きさ　から パン角、チルト角を求める
CvPanTilt calc_angle(CvPoint point,CvSize size){
	int dx,dy;
	CvPanTilt dangle = cvPanTilt(0,0);
	
	dx = (size.width/2 - point.x);
	dy = (size.height/2 - point.y);

	if( NOT_ENOUGH(abs(dx)) ) 
		dangle.pan = - 1 * dx * 1.5;
	if( NOT_ENOUGH(abs(dy)))
		dangle.tilt =  dy * 1.5;

	return dangle;
}
void pantilt_graph_draw(IplImage *image,CvPanTilt angle,
			CvPanTiltRange range,CvScalar color){
	cvCircle(image,
		 cvPoint((angle.pan + range.pan_max)*GRAPH_SCALE,
			 (range.tilt_max - angle.tilt)*GRAPH_SCALE),
		 3,color,-1,8, 0);
}

// パンチルト角度の変移をグラフに描く
IplImage *pantilt_locus_draw(IplImage *image,CvSeq *points,
			     CvPanTiltRange *range){
	int i;
	CvPoint p;
	CvPanTilt angle;
	//水平線
	cvLine(image,cvPoint(0,range->tilt_max*GRAPH_SCALE), 
	       cvPoint(2.0*range->pan_max*GRAPH_SCALE,
		       range->tilt_max*GRAPH_SCALE),
	       CV_RGB(255,0,255),1,8,0);
	//垂直線
	cvLine(image,cvPoint(range->pan_max*GRAPH_SCALE,8), 
	       cvPoint(range->pan_max*GRAPH_SCALE,
		       2*range->tilt_max*GRAPH_SCALE),
	       CV_RGB(255,0,255),1,8,0);
	
	// 軌跡の描画
	for(i = 0 ; i < points->total ; i++){
		int level = (255/points->total)*i ;
		angle = *(CvPanTilt *)cvGetSeqElem(points,i);
		pantilt_graph_draw(image,angle,*range,
				   CV_RGB(level,level,level));
				  
	}
}
FaceDetect *init_facedetect(CvSize size){
	FaceDetect *detect;

	detect = (FaceDetect *)malloc(sizeof(FaceDetect));
	printf("initialize detect structre ... ");
	detect->cascade = (CvHaarClassifierCascade *)cvLoad(CASCADE_FILE, 0, 0, 0);
	detect->storage = cvCreateMemStorage(0);
	detect->frame = cvCreateImage(size,IPL_DEPTH_8U,3);
	detect->result = cvPoint(0,0);
	printf("done.\n");
	return detect;
}
void free_facedetect(FaceDetect *detect){
	cvReleaseImage( &detect->frame);
	free(detect);
}
void init_pantilt(CvCapture *capture,CvPanTiltRange range){
	CvPanTilt angle;

	cvAbsolutePanTilt(capture,cvPanTilt(range.pan_max-1,
					    range.tilt_max-1), range);
	sleep(1);

	cvAbsolutePanTilt(capture,cvPanTilt(0,0),range);
	sleep(1);

	cvAbsolutePanTilt(capture,cvPanTilt(range.pan_min+1,
					    range.tilt_min+1),range);
	sleep(1);

	cvAbsolutePanTilt(capture,cvPanTilt(0,0),range);
}
MyTracker *init_tracker(CvCapture *capture,CvSize size){
	MyTracker *track;
	CvPanTiltRange range;

	printf("initialize tracker structure ...");
	track = (MyTracker *)malloc(sizeof(MyTracker));

	track->size = size;

	// 点の保存をするための、シーケンスデータ構造(CvSeq)
	track->storage = cvCreateMemStorage(0);
	track->seq = cvCreateSeq(0,sizeof(CvSeq),
				 sizeof(CvPanTilt),track->storage);

	track->catch_flag =  track->lost_count= 0;
	range = cvRangePanTilt(capture);
	track->range = range;

	//最大／最小角へ振ってゼロ点を合わせる
	init_pantilt(capture,range);
	
	// パンチルトの軌跡、出力画像のイメージ生成
	track->graph = cvCreateImage(cvSize(range.pan_max*2*GRAPH_SCALE,
					    range.tilt_max*2*GRAPH_SCALE),
				     IPL_DEPTH_8U,3);
	printf("done.\n");
	return track;
}
void free_mytracker(MyTracker *track){
	cvReleaseImage(&track->graph);
	cvClearMemStorage( track->storage );
	free(track);
	
}
void move_camera(CvCapture *capture,CvPoint point,MyTracker *track)
{
	CvPanTilt angle;

	track->catch_flag = 1;
	
	// 顔の中心へむけるためのカメラの角度を計算
	track->dangle = calc_angle(point,track->size);
	
	// 相対角度指定で動作
	cvRelativePanTilt(capture,track->dangle,track->range);

	//現在角度の取得
	cvGetPanTilt(capture,&angle);

	//パンチルト角を記録
	cvSeqPush(track->seq,&angle);

	if( track->seq->total > 10)
		cvSeqPopFront(track->seq,NULL);

	pantilt_linear_predict(track->seq,
			       &track->vpan,&track->vtilt,
			       &angle);
	
	pantilt_graph_draw(track->graph,angle,
			   track->range,CV_RGB(255,0,0));

}
void predict_move_camera(CvCapture *capture,MyTracker *track)
{
	CvPanTilt angle;
	if(track->catch_flag == 1){ 
		// 10回だけ、認識結果の過去2点から線形予測する
		if(++track->lost_count> 10) 
			track->catch_flag = 0;
		cvRelativePanTilt(capture,
				  cvPanTilt(track->vpan,track->vtilt),
				  track->range);
		printf("predict v = %d ,%d\n", track->vpan,track->vtilt);
	}else{
		//初期位置に戻って待機
		printf("init\n");
		track->lost_count=0;
		track->vpan = track->vtilt = 0;
		cvClearSeq( track->seq );
	}
}
int main(int argc,char *argv[]){
	CvCapture	*capture = 0;
	IplImage	*image = 0;
	CvSize		size = cvSize(640,480);
	FaceDetect   *detect;
	MyTracker    *track;
	int	    key;

	// キャプチャの準備
	capture = cvCaptureFromCAM(0);
	if(!capture){
		printf("can't find catpture device");
		exit(-1);
	}
	cvSetCaptureProperty(capture,CV_CAP_PROP_FRAME_WIDTH ,size.width);
	cvSetCaptureProperty(capture,CV_CAP_PROP_FRAME_HEIGHT ,size.height);

	track= init_tracker(capture,size);
	detect = init_facedetect(size);
	
	// ウィンドウをつくる
	cvNamedWindow("result",1);
	cvNamedWindow("pan-tilt",1);

	//LEDの点滅を設定してみる (ON:OFF = 100ms:100ms)
	cvSetLED(capture,100,100);

	while(1){
		image = cvQueryFrame(capture);
		cvCopy(image,detect->frame,NULL);
		cvZero(track->graph);
		pantilt_locus_draw(track->graph,track->seq,&track->range);

		if(detect_and_draw(detect)>0){
			move_camera(capture,detect->result,track);
		}else{
			predict_move_camera(capture,track);
		}
		
		
		cvShowImage("pan-tilt",track->graph);
		cvShowImage( "result", detect->frame);

		if( (key =cvWaitKey(10)) == 'q')
			break;
		if (key == 'r')
			cvAbsolutePanTilt(capture,cvPanTilt(0,0),track->range);
			
			
	}
	free_mytracker(track);
	free_facedetect(detect);
	cvReleaseCapture(&capture);
	return 0;
}