#include "Analysis.h"
#include "ProcessList.h"

//-------------------------------------------------------------------------------
// コンストラクタ
CAnalysis::CAnalysis(void)
{
	m_RefProc  = new CReferenceProc;
	m_Casmatch = new CDetectCasmatch;
	m_Tracker  = new CTracking;
	m_NumProcFrames = 0;
}

//-------------------------------------------------------------------------------
// デストラクタ
CAnalysis::~CAnalysis(void)
{
	SAFE_DELETE(m_RefProc);
	SAFE_DELETE(m_Casmatch);
	SAFE_DELETE(m_Tracker);

	cvDestroyAllWindows();
}

//-------------------------------------------------------------------------------
// 全般的な初期化
bool CAnalysis::GlobalInit()
{
#ifndef DEBUG_TRACK_ONLY
	CALL(m_RefProc->Init());
	CALL(m_Casmatch->Init());
#endif

	return true;
}

//-------------------------------------------------------------------------------
// 全体処理
bool CAnalysis::GlobalProc()
{
	CALL(this->GlobalInit());

	CProcessList pl;
	CALL(pl.Init());

	FILE *fp;
	char filename[PATH_LEN];
	sprintf_s(filename, PATH_LEN, "output\\GlobalLog20110113.csv");
	fopen_s(&fp, filename, "w");
	if (!fp) ERROR_RET("Can't open Global log file.");
	fprintf(fp, "日付, 時間, 被験者, 回数, エラー, 処理枚数, 動き, "
		"L1(t0), a1(t0), b1(t0), L1(t10), a1(t10), b1(t10), "
		"L1(t20), a1(t20), b1(t20), L1(t30), a1(t30), b1(t30), "
		"L2(t0), a2(t0), b2(t0), L2(t10), a2(t10), b2(t10), "
		"L2(t20), a2(t20), b2(t20), L2(t30), a2(t30), b2(t30), "
		"L3(t0), a3(t0), b3(t0), L3(t10), a3(t10), b3(t10), "
		"L3(t20), a3(t20), b3(t20), L3(t30), a3(t30), b3(t30), "
		"L4(t0), a4(t0), b4(t0), L4(t10), a4(t10), b4(t10), "
		"L4(t20), a4(t20), b4(t20), L4(t30), a4(t30), b4(t30), \n");

	char workpath[PATH_LEN];
	bool noRefInfo = false;
	do
	{
#ifndef DEBUG_TRACK_ONLY
		if (pl.IsFirstTOD())
		{
			CALL(pl.GetRefDir(workpath));
			printf("\n***** RefProc: %s **********\n\n", workpath);

			if (m_RefProc->CalcMatrix(workpath, pl.CurrentRefFile())) noRefInfo = false;
			else noRefInfo = true;
		}
#endif
		if (noRefInfo) continue;

		if(!pl.GetFrameDir(workpath)) continue;
		printf("\n***** MeasurementProc: %s **********\n\n", workpath);

		int err = this->MeasurementProc(workpath, pl.CurrentImgFile());
		if (err == -1) continue;

		// ログに出力
		char buffer[256];
		pl.MakeParamStr(buffer, sizeof(buffer));
		fprintf(fp, "%s, %d, %d, %f, ", 
			buffer, err, m_NumProcFrames, m_Tracker->TotalMovement() / m_NumProcFrames);
		
		// ログに出力(領域1)
		for (int i = 0; i < ((m_NumProcFrames - 1) / m_FrameScale) + 1; i++)
		{
			fprintf(fp, "%.3f, %.3f, %.3f, ", 
				m_ROILabT1[i].val[0], m_ROILabT1[i].val[1], m_ROILabT1[i].val[2]);
		}
		for (int i = ((m_NumProcFrames - 1) / m_FrameScale) + 1; i < 4; i++)
		{
			fprintf(fp, " ,  ,  , ");
		}

		// ログに出力(領域2)
		for (int i = 0; i < ((m_NumProcFrames - 1) / m_FrameScale) + 1; i++)
		{
			fprintf(fp, "%.3f, %.3f, %.3f, ", 
				m_ROILabT2[i].val[0], m_ROILabT2[i].val[1], m_ROILabT2[i].val[2]);
		}
		for (int i = ((m_NumProcFrames - 1) / m_FrameScale) + 1; i < 4; i++)
		{
			fprintf(fp, " ,  ,  , ");
		}
		// ログに出力(領域3)
		for (int i = 0; i < ((m_NumProcFrames - 1) / m_FrameScale) + 1; i++)
		{
			fprintf(fp, "%.3f, %.3f, %.3f, ", 
				m_ROILabT3[i].val[0], m_ROILabT3[i].val[1], m_ROILabT3[i].val[2]);
		}
		for (int i = ((m_NumProcFrames - 1) / m_FrameScale) + 1; i < 4; i++)
		{
			fprintf(fp, " ,  ,  , ");
		}

		// ログに出力(領域4)
		for (int i = 0; i < ((m_NumProcFrames - 1) / m_FrameScale) + 1; i++)
		{
			fprintf(fp, "%.3f, %.3f, %.3f, ", 
				m_ROILabT4[i].val[0], m_ROILabT4[i].val[1], m_ROILabT4[i].val[2]);
		}
		for (int i = ((m_NumProcFrames - 1) / m_FrameScale) + 1; i < 4; i++)
		{
			fprintf(fp, " ,  ,  , ");
		}

		fprintf(fp, "\n");

	} while(pl.MoveNext());

	fclose(fp);

	printf("\nFinished.\n");
	if (GShowImage(NULL, 0) < 1)
	{
		printf("Press key on terminal.\n");
		getchar(); 
	}
	else
	{
		printf("Press key on window.\n");
		cvWaitKey(0);
	}

	return true;
}

//-------------------------------------------------------------------------------
// 撮影処理
// 戻り値 エラー値 int 0=OK 1=特徴点位置不正 2=特徴点消失 3=手動エラー
//					   -1=致命的なエラー -2=画像なしエラー
//
int CAnalysis::MeasurementProc(const char *path, const char *file)
{
	// 画像枚数と手動抽出パラメータの取得
	int numFrames = GetNumFrames(path, file);
	if (numFrames < 1)
	{
		printf("No measurement file found.\n");
		return -1;
	}
	int frame0 = ReadParam(path);
	if (frame0 < 1) 
	{
		printf("Read param failed.\n");
		return -1;
	}

	// 処理画像枚数の決定
	int procFrames = (numFrames < (PROC_FRAMES_S + PROC_FRAMES_L) / 2 ?
					PROC_FRAMES_S : PROC_FRAMES_L);
	m_FrameScale = (procFrames == PROC_FRAMES_S ? 10 : 100);
#ifdef DEBUG_NUM_FRAMES
	procFrames = DEBUG_NUM_FRAMES;
#endif

	// 処理画像を開く
	char filename[PATH_LEN], filenam2[PATH_LEN];
	sprintf_s(filenam2, PATH_LEN, file, frame0);
	sprintf_s(filename, PATH_LEN, "%s\\%s", path, filenam2);

#ifndef DEBUG_TRACK_ONLY
	// キャスマッチの検出，パッチ色取得
	CALL(m_Casmatch->SetImage(filename));
	CALL(m_Casmatch->Detect());
#endif

	// トラッキングの初期化
	IplImage *img = cvLoadImage(filename);
	m_Tracker->Init(m_InitTrack, TRACK_POINTS, img);
//	m_Tracker->DrawPoints(img);
//	GShowImage(img, 1, "Init Tracker");
	SAFE_RELEASEIMG(img);

	// 結果出力ディレクトリの作成
	sprintf_s(filename, PATH_LEN, "%s\\%s", path, OUTPUT_DIR);
	if (!GFileExists(filename)) CreateDirectory(filename, NULL);

	// ログの準備
	FILE *fpLog;
	sprintf_s(filename, PATH_LEN, "%s\\result20101221.csv", path);
	fopen_s(&fpLog, filename, "w");
	if (!fpLog) ERROR_RET("Can't open local log file.");
	fprintf(fpLog, "フレーム, 動き, L*1, a*1, b*1, L*2, a*2, b*2, L*3, a*3, b*3, L*4, a*4, b*4\n");

	// 各フレームの処理
	int err = 0;
	m_NumProcFrames = 0;
	for (int frame = frame0; frame < frame0 + procFrames && err == 0; frame ++)
	{
		printf("\n* Frame %d\n", m_NumProcFrames);

		sprintf_s(filenam2, PATH_LEN, file, frame);
		sprintf_s(filename, PATH_LEN, "%s\\%s", path, filenam2);

		err = this->FrameProc(filename, path, m_NumProcFrames, fpLog);
		if (err > -1) m_NumProcFrames ++;
	}

#ifdef MANUAL_TRACKING_EVALUATION
	// 終了後に手動でトラッキング結果を評価する
	if (err == 0)
	{
		printf("Evaluate result pressing key [F]ailed or key else on the win:\n");
		if (cvWaitKey(0) == 'f') err = 3;
	}
#endif // MANUAL_TRACKING_EVALUATION

	fclose(fpLog);

	return err;
}

//-------------------------------------------------------------------------------
// フレーム処理
// 戻り値 エラー値 int 0=OK 1=特徴点位置不正 2=特徴点消失 
//					  -1=致命的なエラー -2=画像なしエラー
//
int CAnalysis::FrameProc(const char *inputfile, const char *path, 
						 const int frame, FILE *fpLog)
{
	int trackErr = 0;
	CHQTime timer;

	// ファイルを開く
	IplImage *imgCam = cvLoadImage(inputfile);
	if (!imgCam)
	{
		printf("Error: No camera file.\n");
		return -2;
	}

	// トラッキング
	if (!m_Tracker->Frame(imgCam))
	{
		printf("Track point missing.\n");
		trackErr = 2;
	}
	fprintf(fpLog, "%d, %f, ", frame, m_Tracker->Movement());
	if (!m_Tracker->IsVaildate())
	{
		printf("Track position invalidate.\n");
		trackErr = 1;
	}
	timer.LapTime("tracking");

#ifndef DEBUG_TRACK_ONLY
	// キャスマッチのパッチ色取得
	CvMat *crgbFC = NULL;
	CALL(m_Casmatch->SetImage(inputfile));
	CALL(m_Casmatch->CalcPatchColor9(&crgbFC));
#ifdef SHOW_RGB_VALUES
	GShowMat(crgbFC, "crgbFC", "%5.1f");
#endif // SHOW_RGB_VALUES

	// 線形化
	CvMat *lrgbFC = m_RefProc->GenLinearize(crgbFC);
#ifdef SHOW_RGB_VALUES
	GShowMat(lrgbFC, "lrgbFC", "%6.3f");
#endif // SHOW_RGB_VALUES

	// フレーム間色補正行列の算出
	CMRegressionRGB ccmFR(FRAME_REF_CONVERT_DIM);
#ifdef SHOW_REGRESSION_COEF
	printf("ccmFR  ");
#endif // SHOW_REGRESSION_COEF
	ccmFR.CalcCoef(lrgbFC, m_RefProc->lrgbRC());
	timer.LapTime("coef calculation");

	if (trackErr == 0)
	{
		// ROIの計算(マスク1)
		CvScalar roiCam1 = m_Tracker->ROIColor1(imgCam);
		CvScalar roiLin1 = m_RefProc->ScalarLinearize(roiCam1);
		CvScalar roiCor1 = ccmFR.ScalarConvert(roiLin1);
		CvScalar roiXYZ1 = m_RefProc->ScalarConvertXYZ(roiCor1);
		CvScalar roiLab1 = GXYZtoLab(roiXYZ1);
		printf("Lab1 = %f, %f, %f\n", 
			roiLab1.val[0], roiLab1.val[1], roiLab1.val[2]);
		fprintf(fpLog, "%f, %f, %f, ", 
			roiLab1.val[0], roiLab1.val[1], roiLab1.val[2]);

		if (frame % m_FrameScale == 0) 
			m_ROILabT1[frame / m_FrameScale] = roiLab1;

		// ROIの計算(マスク2)
		CvScalar roiCam2 = m_Tracker->ROIColor2(imgCam);
		CvScalar roiLin2 = m_RefProc->ScalarLinearize(roiCam2);
		CvScalar roiCor2 = ccmFR.ScalarConvert(roiLin2);
		CvScalar roiXYZ2 = m_RefProc->ScalarConvertXYZ(roiCor2);
		CvScalar roiLab2 = GXYZtoLab(roiXYZ2);
		printf("Lab2 = %f, %f, %f\n", 
			roiLab2.val[0], roiLab2.val[1], roiLab2.val[2]);
		fprintf(fpLog, "%f, %f, %f, ", 
			roiLab2.val[0], roiLab2.val[1], roiLab2.val[2]);

		if (frame % m_FrameScale == 0) 
			m_ROILabT2[frame / m_FrameScale] = roiLab2;

		// ROIの計算(マスク3)
		CvScalar roiCam3 = m_Tracker->ROIColor3(imgCam);
		CvScalar roiLin3 = m_RefProc->ScalarLinearize(roiCam3);
		CvScalar roiCor3 = ccmFR.ScalarConvert(roiLin3);
		CvScalar roiXYZ3 = m_RefProc->ScalarConvertXYZ(roiCor3);
		CvScalar roiLab3 = GXYZtoLab(roiXYZ3);
		printf("Lab3 = %f, %f, %f\n", 
			roiLab3.val[0], roiLab3.val[1], roiLab3.val[2]);
		fprintf(fpLog, "%f, %f, %f, ", 
			roiLab3.val[0], roiLab3.val[1], roiLab3.val[2]);

		if (frame % m_FrameScale == 0) 
			m_ROILabT3[frame / m_FrameScale] = roiLab3;

		// ROIの計算(マスク4)
		CvScalar roiCam4 = m_Tracker->ROIColor4(imgCam);
		CvScalar roiLin4 = m_RefProc->ScalarLinearize(roiCam4);
		CvScalar roiCor4 = ccmFR.ScalarConvert(roiLin4);
		CvScalar roiXYZ4 = m_RefProc->ScalarConvertXYZ(roiCor4);
		CvScalar roiLab4 = GXYZtoLab(roiXYZ4);
		printf("Lab4 = %f, %f, %f\n", 
			roiLab4.val[0], roiLab4.val[1], roiLab4.val[2]);
		fprintf(fpLog, "%f, %f, %f, ", 
			roiLab4.val[0], roiLab4.val[1], roiLab4.val[2]);

		if (frame % m_FrameScale == 0) 
			m_ROILabT4[frame / m_FrameScale] = roiLab4;
	}

#ifdef CONVERT_IMAGE
	// 画像の色変換
	IplImage *imgLin = m_RefProc->GenLinearize(imgCam);
	timer.LapTime("image linearize");

	IplImage *imgCor = ccmFR.GenConvert(imgLin);
	timer.LapTime("image correction");

	// XYZに変換
	IplImage *imgXYZ = m_RefProc->GenConvertXYZ(imgCor);
	timer.LapTime("image conversion to XYZ");

	// モニター用に変換
	IplImage *imgDisp = m_RefProc->GenConvertDisp(imgXYZ);
	timer.LapTime("image conversion to Monitor RGB");

	IplImage *imgGamma = GGenAddGamma(imgDisp);
	timer.LapTime("image conversion adding Gamma");
#endif // CONVERT_IMAGE

#endif // DEBUG_TRACK_ONLY

	// 解析結果の表示
#ifndef DEBUG_TRACK_ONLY

	IplImage *imgCV = m_Casmatch->GenPatchedImage();
	m_Tracker->DrawPoints(imgCV);
	if (trackErr == 0) m_Tracker->DrawROI(imgCV);
#ifdef SHOW_CV_IMAGE
	GShowImage(imgCV, 1, "Detection result");
#endif // SHOW_CV_IMAGE
#ifdef CONVERT_IMAGE
	GShowImage(imgGamma, 2, "Calibrated");
#endif // CONVERT_IMAGE

#ifdef SHOW_XYZ_IMAGE
	//IplImage *imgX = cvCreateImage(cvGetSize(imgXYZ), IPL_DEPTH_64F, 1);
	//IplImage *imgY = cvCreateImage(cvGetSize(imgXYZ), IPL_DEPTH_64F, 1);
	//IplImage *imgZ = cvCreateImage(cvGetSize(imgXYZ), IPL_DEPTH_64F, 1);
	//cvSplit(imgXYZ, imgX, imgY, imgZ, NULL);
	//GShowImage(imgX, 3, "X image");
	//GShowImage(imgY, 4, "Y image");
	//GShowImage(imgZ, 5, "Z image");
#endif // SHOW_XYZ_IMAGE
#else // DEBUG_TRACK_ONLY
	m_Tracker->DrawPoints(imgCam);
	if (trackErr < 1) GShowImage(imgCam, 1, "Detection result", 1);
	else		      GShowImage(imgCam, 1, "Tracking failed",  1);

#endif // DEBUG_TRACK_ONLY
	timer.LapTime("display");

	// 解析結果の保存
	char filename[PATH_LEN];
	sprintf_s(filename, PATH_LEN, OUTPUT_CV, path, frame);
	cvSaveImage(filename, imgCV);
	timer.LapTime("save image");

	// 計算に使用した行列の解放
	SAFE_RELEASEIMG(imgCam);
	SAFE_RELEASEIMG(imgCV);
#ifndef DEBUG_TRACK_ONLY
	SAFE_RELEASEMAT(crgbFC);
	SAFE_RELEASEMAT(lrgbFC);
#ifdef CONVERT_IMAGE
	SAFE_RELEASEIMG(imgLin);
	SAFE_RELEASEIMG(imgCor);
	SAFE_RELEASEIMG(imgXYZ);
	SAFE_RELEASEIMG(imgDisp);
	SAFE_RELEASEIMG(imgGamma);
#endif // CONVERT_IMAGE
#ifdef SHOW_XYZ_IMAGE
	SAFE_RELEASEIMG(imgX);
	SAFE_RELEASEIMG(imgY);
	SAFE_RELEASEIMG(imgZ);
#endif // SHOW_XYZ_IMAGE
#endif // DEBUG_TRACK_ONLY

	fprintf(fpLog, "\n");
	timer.CheckTime("FrameProc");

	return trackErr;
}

//-------------------------------------------------------------------------------
// フレーム数の取得
int CAnalysis::GetNumFrames(const char *path, const char *file)
{
	char filename[PATH_LEN], filenam2[PATH_LEN];
	sprintf_s(filenam2, PATH_LEN, file, 0);
	sprintf_s(filename, PATH_LEN, "%s\\%s", path, filenam2);

	int frame = GFileExists(filename) ? -1 : 0;
	do
	{
		sprintf_s(filenam2, PATH_LEN, file, ++ frame);
		sprintf_s(filename, PATH_LEN, "%s\\%s", path, filenam2);
	} while (GFileExists(filename));

	printf("Found %d frames\n", frame);

	return frame;
}

//-------------------------------------------------------------------------------
// パラメータを読み込む
int CAnalysis::ReadParam(const char *path)
{
	// パラメータファイルを開く
	char filename[PATH_LEN];
	sprintf_s(filename, PATH_LEN, "%s\\" PARAM_FILE, path);

	// パラメータを読み込む
	FILE *fp = NULL;
	fopen_s(&fp, filename, "r");
	if (fp == NULL) ERROR_RET("Can't open parameter file.");

	const int BUFFER_LEN = 1024;
	char buffer[BUFFER_LEN] = {0};

	// 開始フレーム取得
	fgets(buffer, BUFFER_LEN, fp);
	int firstFrame = atoi(buffer);
#ifdef SHOW_FILELOAD
	printf("First frame %d\n", firstFrame);
#endif // SHOW_FILELOAD
	
	// 舌形状ポイントの取得
	for (int i = 0; i < TRACK_POINTS; i++)
	{
		fgets(buffer, BUFFER_LEN, fp);
		char *pt = buffer;
		for (; *pt != '\0' && *pt != ','; pt ++);
		if (*pt != ',') ERROR_RET("Parameter file error.");
		*pt = '\0';
		m_InitTrack[i].x = atoi(buffer) * 2;	// 手動座標は1/2されている
		m_InitTrack[i].y = atoi(pt + 1) * 2;
#ifdef SHOW_FILELOAD
		printf("Init. track point (%d, %d)\n", m_InitTrack[i].x, m_InitTrack[i].y);
#endif // SHOW_FILELOAD
	}

	return firstFrame;
}