//Program by Kiyota Shota @ Kagoshima Univ.

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

#ifdef _DEBUG
    //Debugモードの場合
    #pragma comment(lib,"cv210d.lib")
    #pragma comment(lib,"cxcore210d.lib")
    #pragma comment(lib,"cvaux210d.lib")
    #pragma comment(lib,"highgui210d.lib")
#else
    //Releaseモードの場合
    #pragma comment(lib,"cv210.lib")
    #pragma comment(lib,"cxcore210.lib")
    #pragma comment(lib,"cvaux210.lib")
    #pragma comment(lib,"highgui210.lib")
#endif

int main (int argc,char *argv[])
{
	if(argc <2 ){
		printf("usage: %s <avi file name> <thresh for track>\n", argv[0]);
		exit(0);
	}

	int thres_lk = 15;
	if(argc >2){
		thres_lk = atoi(argv[2]);
	}

	IplImage *src, *gray, *prevgray, *eig, *temp, *prev_pyramid, *curr_pyramid;
	CvPoint2D32f *corners1, *corners2;
	CvTermCriteria criteria =cvTermCriteria (CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 64, 0.01);

	int corner_count =300;
	char* status= (char *) cvAlloc (corner_count);


	char* g_status= (char *) cvAlloc (corner_count);
	FillMemory(g_status, 1, corner_count);
	CvPoint2D32f** log;
	log = new CvPoint2D32f*[1000];
	FILE *fp;


	CvCapture *capture = NULL;
	int key;

	// srcオペレータのパラメータ
	double low_threshold=50.0;
	double high_threshold=200.0;
	int aperture_size=3;

	//ファイル書き込み設定
	int isColor =1;
	int fps = 30;
	//int frameW = 320;
	//int frameH =240;
	int frameW = 720;
	int frameH = 480;
	src = cvCreateImage(cvSize(frameW,frameH),IPL_DEPTH_8U,1);
	prevgray = cvCreateImage (cvSize(frameW,frameH),IPL_DEPTH_8U,1);

	//指定したAVIファイルが見つからなかったら終了
	if(NULL==(capture = cvCaptureFromAVI(argv[1]))){
		fprintf(stderr,"指定のaviファイルは見つかりませんでした。");
		return -1;
	}

	gray = cvCreateImage(cvSize(frameW,frameH),IPL_DEPTH_8U,1);
	eig = cvCreateImage (cvSize(frameW,frameH),IPL_DEPTH_8U,1);
	temp = cvCreateImage (cvSize(frameW,frameH),IPL_DEPTH_8U,1);

	prev_pyramid = cvCreateImage (cvSize (src->width + 8, src->height / 3), IPL_DEPTH_8U, 1);
	curr_pyramid = cvCreateImage (cvSize (src->width + 8, src->height / 3), IPL_DEPTH_8U, 1);

	corners1 = (CvPoint2D32f *) cvAlloc (corner_count * sizeof (CvPoint2D32f));
	corners2 = (CvPoint2D32f *) cvAlloc (corner_count * sizeof (CvPoint2D32f));
	//gray->origin =1;
	eig->origin =1;
	temp->origin =1;
	prev_pyramid->origin =1;
	curr_pyramid->origin =1;


	//画像表示ウィンドウの準備
	cvNamedWindow("gray", CV_WINDOW_AUTOSIZE);
	cvNamedWindow("color",CV_WINDOW_AUTOSIZE);
	//画像表示ウィンドウの出現位置指定
	cvMoveWindow("gray", 50, 50);
	cvMoveWindow("color", 400, 50);

	bool once = true;
	float* error;
	long count = 0;

	for(;;count++){

		log[count] = new CvPoint2D32f[corner_count];

		//1.AVIファイルからフレーム画像を取り出す
		if(NULL==(src=cvQueryFrame(capture))){
			break;
		}
		if(NULL==(src=cvQueryFrame(capture))){
			break;
		}
		//fprintf(stderr, "%d, %d\n", src->width, src->height);
		//		src2 = cvCloneImage (src);
		//sprintf(filenameI, "img%03d.bmp", count);
		//cvSaveImage(filenameI, src);

		//2.グレースケールに変換
		cvCvtColor(src, gray, CV_BGR2GRAY);

		if(once){
			once = false;
			//prevgray = cvCloneImage (src);
			cvCopyImage(gray, prevgray);

			//3a.cvCornerHarrisを利用したコーナー検出
			cvGoodFeaturesToTrack (gray, eig, temp, corners1, &corner_count, 0.001, thres_lk, NULL, 3, 1, 0.01);

			cvFindCornerSubPix (gray, corners1, corner_count,
				cvSize (3, 3), cvSize (-1, -1), cvTermCriteria (CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03));
			error = new float[corner_count];
		}else{
			for (int i = 0; i < corner_count; i++){
				if(status[i]==0){
					corners1[i].x = -100;
					corners1[i].y = -100;
					g_status[i] = 0;
				}else{
					corners1[i]=corners2[i];
				}
			}
			//cvFindCornerSubPix (gray, corners1, corner_count,
			//                 cvSize (3, 3), cvSize (-1, -1), cvTermCriteria (CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03));
		}

		cvCalcOpticalFlowPyrLK (prevgray, gray, prev_pyramid, curr_pyramid,
			corners1, corners2, corner_count, cvSize (10, 10), 4, status, error, criteria, 0);

		for (int i = 0; i < corner_count; i++){
			if(corners2[i].x <0 || corners2[i].y < 0){
				g_status[i] = 0;
			}
			log[count][i] = corners2[i];
		}

		//4a.コーナーの描画
		for (int i = 0; i < corner_count; i++){
			cvCircle (src, cvPointFrom32f (corners2[i]), 3, CV_RGB (0, 0, 255), 2);
			printf("%d src:  %d x = %f , y=%f, st=%d, error=%f\n", count, i,corners2[i].x,corners2[i].y, (int)status[i], error[i]);
		}
		printf("---------------------------------------\n");
		cvShowImage ("color", src);

		//4b.コーナーの描画
		for (int i = 0; i < corner_count; i++){
			cvCircle (prevgray, cvPointFrom32f (corners1[i]), 3, CV_RGB (255, 0, 0), 2);
			//printf("sr3:  %d x = %f , y=%f\n",i,corners1[i].x,corners1[i].y);
		}
		cvShowImage ("gray", prevgray);

		cvCopyImage(gray, prevgray);

		//5.ESCキーを入力すると終了
		key = cvWaitKey (20);
		if(key==0x1b)
			break;
		if(key=='a'){
			Sleep(30000);
		}
	}

	int q=0;
	for(int i =0;i<corner_count;i++){
		if(status[i]==1){
			q++;
		}
	}
	char filename[128];
	for(int j =1;j<count;j++){
		sprintf(filename, "data%03d.dat", j-1);
		fp = fopen(filename,"w");
		fprintf(fp,"%d\n",q);
		for(int i =0;i<corner_count;i++){
			if(status[i]==1){
				fprintf(fp,"%f  %f\n", log[j][i].x, log[j][i].y);
			}
		}
		//		fprintf(fp,"---------------------------------------\n");
		//		fprintf(fp,"%d\n",q);
		fclose(fp);
	}

	//解放
	cvReleaseCapture(&capture);
	cvReleaseImage(&gray);
	cvReleaseImage(&prevgray);
	cvReleaseImage(&eig);
	cvReleaseImage(&temp);
	cvDestroyWindow("gray");
	cvDestroyWindow ("color");
	return 0;
}