#include "Analysis.h"

//------------------------------------------------------------------------------
// コンストラクタ
Analysis::Analysis(void)
{
	// ポインタの初期化
	m_macbeth = NULL;
	m_camera  = NULL;
	m_show    = NULL;
	m_log     = NULL;
	m_gLog    = NULL;
	m_param   = NULL;
	m_bMat    = NULL;
	for (int i=0; i<NUM_PATCH; i++)	m_miniPatch[i] = NULL;
}

//------------------------------------------------------------------------------
// デストラクタ
Analysis::~Analysis(void)
{
	// メモリ解放
	if (m_macbeth != NULL) cvReleaseImage(&m_macbeth);
	if (m_camera  != NULL) cvReleaseImage(&m_macbeth);
	if (m_show    != NULL) cvReleaseImage(&m_show);
	if (m_log     != NULL) this->CloseLogFile(false);
	if (m_gLog    != NULL) this->CloseLogFile(true);
	if (m_param   != NULL) delete [] m_param;
	if (m_bMat    != NULL) cvReleaseMat(&m_bMat);
	for (int i=0; i<NUM_PATCH; i++) if (m_miniPatch[i]) delete [] m_miniPatch[i];

	// ウインドウを閉じる
	cvDestroyAllWindows();
}

//------------------------------------------------------------------------------
// 処理フロー
bool Analysis::Process()
{
	if (!this->Init()				) return false;

	for (int param=0; param < m_numParam; param ++)
	{
		for (int timeMDE=0; timeMDE < m_param[param].timeMDE; timeMDE++)
		{
			bool firstTime = true;
			for (int subject=1; subject <= m_param[param].subject; subject ++)
			{
				for (int count=1; count <= m_param[param].count; count ++)
				{
					if (!this->SetPath(m_param[param].dateStr, timeMDE, subject, count)) 
						return false;

					if (firstTime)
					{
						// 参照画像の処理
						SPRLOG "Ref processing: %s", m_refPath); WriteLog(true);
						if (!this->OpenLogFile(false, true)) return false;
						m_numFrames = 1;
						if (!this->GetPatchPosition(true)) return false;
						if (!this->GetAllPatchColor(true)) return false;
						if (!this->CloseLogFile()		 ) return false;
						firstTime = false;
					}

					// 舌画像の処理
					SPRLOG "Start processing: %s", m_filePath); WriteLog(true);
					if (!this->OpenLogFile()		) return false;
					if (!this->GetNumFrames()		) return false;
					if (!this->GetPatchPosition()	) return false;
					if (!this->GetAllPatchColor()	) return false;
					if (!this->CorrectionMat()		) return false;
					if (!this->CloseLogFile()		) return false;
				}
			}
		}
	}

	this->CloseLogFile(true);
	cvWaitKey(0);

	return true;
}

//------------------------------------------------------------------------------
// 初期化
bool Analysis::Init()
{
	// ミニマクベスの参照テンプレート画像を読み込み
	m_macbeth = cvLoadImage(MINIMACBETH);
	if (!m_macbeth) ERROR_RET("Can't open Macbeth image file.");

	// 全体ログを開く
	this->OpenLogFile(true);

	// 処理リストファイルを開く
	FILE *fp = NULL;
	fopen_s(&fp, PROCESS_LIST, "r");
	if (!fp) ERROR_RET("Can't open Process List file.");

	// 処理リストの件数取得
	char buffer[1024];
	int num = 0;
	while (!feof(fp))
	{
		fgets(buffer, sizeof(buffer), fp);
		if (strncmp(buffer, "//", 2))
		{
			Param p;
			sscanf_s(buffer, "%s %d %d %d", 
				p.dateStr, DATE_STRING, &p.timeMDE, &p.subject, &p.count);
			if (p.timeMDE > 0) num ++;
		}
	}
	if (num < 1) ERROR_RET("No parameter data is found in file.");
	m_numParam = num;
	SPRLOG "Parameter list: %d", m_numParam); WriteLog(true);

	// 全処理リストを読み込み
	m_param = new Param [m_numParam];
	int i = 0;
	fseek(fp, 0, SEEK_SET);
	while (!feof(fp))
	{
		fgets(buffer, sizeof(buffer), fp);
		if (strncmp(buffer, "//", 2))
		{
			sscanf_s(buffer, "%s %d %d %d", 
				m_param[i].dateStr, DATE_STRING, 
				&m_param[i].timeMDE, &m_param[i].subject, &m_param[i].count);
			if (m_param[i].timeMDE > 0) i ++;
		}
	}

	cvNamedWindow(DISP1);
	cvSetMouseCallback(DISP1, OnMouse1, this);
#ifdef DISP2
	cvNamedWindow(DISP2);
#endif
#ifdef DISP3
	cvNamedWindow(DISP3);
#endif

	return true;
}

//------------------------------------------------------------------------------
// 処理するフォルダのパスを決定
bool Analysis::SetPath(const char *dateStr, int timeMDE, int subject, int count)
{
	const char *timeString[3] = {"morning", "daytime", "evening"};

	sprintf_s(m_filePath, MAX_PATH_LEN, "%s\\%s\\%s%d-%d", 
		BASE_PATH, dateStr, timeString[timeMDE], subject, count);

	sprintf_s(m_refPath, MAX_PATH_LEN, "%s\\%s\\macbeth_%s", 
		BASE_PATH, dateStr, timeString[timeMDE]);

	return true;
}

//------------------------------------------------------------------------------
// ログファイルを開く
bool Analysis::OpenLogFile(bool gLog, bool ref)
{
	char filename[MAX_PATH_LEN];
	SYSTEMTIME st;
	GetLocalTime(&st);

	if (gLog)
	{
		sprintf_s(filename, MAX_PATH_LEN, "%s", GLOBAL_LOG);
		fopen_s(&m_gLog, filename, "w");
		if (!m_gLog) ERROR_RET("Can't create global log file.");
	}
	else
	{
		sprintf_s(filename, MAX_PATH_LEN, "%s\\%s", 
			(ref ? m_refPath : m_filePath), LOCAL_LOG);
		fopen_s(&m_log, filename, "w");
		if (!m_log) ERROR_RET("Can't create log file.");
	}

	SPRLOG "Log file, \"%s\"", filename); WriteLog(gLog);
	SPRLOG "Start, %02d-%02d-%02d %02d:%02d:%02d", 
		st.wYear, st.wMonth, st.wDay, st.wHour, st.wMinute, st.wSecond); WriteLog(gLog);

	return true;
}

//------------------------------------------------------------------------------
// ログファイルを閉じる
bool Analysis::CloseLogFile(bool gLog)
{
	SYSTEMTIME st;
	GetLocalTime(&st);
	SPRLOG ""); WriteLog(gLog);
	SPRLOG "End, %02d-%02d-%02d %02d:%02d:%02d", 
		st.wYear, st.wMonth, st.wDay, st.wHour, st.wMinute, st.wSecond); WriteLog(gLog);

	if (gLog)	{ fclose(m_gLog); m_gLog = NULL; }
	else		{ fclose(m_log ); m_log  = NULL; }

	return true;
}

//------------------------------------------------------------------------------
// フォルダ内のフレーム数を取得
bool Analysis::GetNumFrames()
{
	char filename[MAX_PATH_LEN];
	int frame = -1;
	WIN32_FIND_DATA ffd;
	do
	{
		sprintf_s(filename, MAX_PATH_LEN, "%s\\" CAMERA_FILE, m_filePath, ++ frame);
	} while (FindFirstFile(filename, &ffd) != INVALID_HANDLE_VALUE);

	if (frame < 1) ERROR_RET("No camera file found.");
	m_numFrames = frame;
#ifdef DEBUG_STOP_FRAME
	m_numFrames = DEBUG_STOP_FRAME
#endif

	SPRLOG "Frames, %d", m_numFrames); WriteLog();

	// 配列の確保
	for (int i=0; i<NUM_PATCH; i++)
	{
		if (m_miniPatch[i]) delete [] m_miniPatch[i];
		m_miniPatch[i] = new CvScalar [m_numFrames];
	}

	return true;
}

//------------------------------------------------------------------------------
// 画像を読み込む
bool Analysis::ReadImage(int frame, bool ref)
{
	// ファイル名の決定
	char filename[MAX_PATH_LEN];
	sprintf_s(filename, MAX_PATH_LEN, "%s\\" CAMERA_FILE, 
		(ref ? m_refPath : m_filePath), frame);

	// カメラ画像取り込み
	if (m_camera != NULL) cvReleaseImage(&m_camera);
	m_camera = cvLoadImage(filename);
	if (!m_camera) ERROR_RET("Can't open camera file.");

	// 表示用画像バッファ
	if (m_show != NULL) cvReleaseImage(&m_show);
	m_show = (IplImage*)cvClone(m_camera);

	return true;
}

//------------------------------------------------------------------------------
// パッチの位置を取得
bool Analysis::GetPatchPosition(bool ref)
{
	if (!m_macbeth) return false;

	this->ReadImage(0, ref);
	this->ShowImage(DISP3);

	// 相関分布画像バッファ作成
	IplImage *match = cvCreateImage(
		cvSize(SEARCH_AREA - m_macbeth->width + 1, 
		SEARCH_AREA - m_macbeth->height + 1),
		IPL_DEPTH_32F , 1);

	// マッチングの準備
	CvMat *rotateMat = cvCreateMat(2, 3, CV_32FC1);
	IplImage *rotateMacbeth = cvCreateImage(cvGetSize(m_macbeth), IPL_DEPTH_8U, 3);
	CvRect roi;
	double gAngle = 0;
	double minC, maxC, gMinC = -1.0;
	CvPoint minPos, maxPos, gMinPos;

	SPRLOG ""); WriteLog();
	SPRLOG "Mini Macbeth searching"); WriteLog();
	SPRLOG "Place, Angle, min value, x, y"); WriteLog();

	// 右上と左下のマクベスを順次処理
	for (int place = 0; place < NUM_MACBETH; place ++)
	{
		// マクベス周辺領域のROIを設定
		if (place==0) roi = cvRect(m_camera->width-SEARCH_AREA, 0,  SEARCH_AREA, SEARCH_AREA);
		else		  roi = cvRect(0, m_camera->height-SEARCH_AREA, SEARCH_AREA, SEARCH_AREA);
		cvSetImageROI(m_camera, roi);

		// 方向別テンプレートマッチング
		gMinC = -1.0;
		int count = 0;
		for(double angle = 0; angle < 360.0; angle += ROTATE_RESOLUTION)
		{
			cv2DRotationMatrix(cvPoint2D32f(m_macbeth->width/2.0, m_macbeth->height/2.0),
				angle, 1.0, rotateMat);
			cvWarpAffine(m_macbeth, rotateMacbeth, rotateMat, 
				CV_WARP_FILL_OUTLIERS, cvScalarAll(255.0));

			cvMatchTemplate(m_camera, rotateMacbeth, match, CV_TM_SQDIFF_NORMED);
			cvMinMaxLoc(match, &minC, &maxC, &minPos, &maxPos);
			if (minC < gMinC || gMinC < 0)
			{
				gAngle = angle;
				gMinPos.x = minPos.x + (m_macbeth->width  / 2) + roi.x;
				gMinPos.y = minPos.y + (m_macbeth->height / 2) + roi.y;
				gMinC = minC;
			}

			if ((++ count) % 10 == 0) cvWaitKey(1);
		}
		SPRLOG "%s, %3.0f, %f, %4d, %4d", (place==1 ? "UR" : "BL"),  
			gAngle, gMinC, gMinPos.x, gMinPos.y); WriteLog();

		// 各パッチの中心座標を計算
		m_patchPos[place * NUM_PATCH] = gMinPos;
		for(int i=0; i < NUM_PATCH - 1; i++)
		{
			m_patchPos[place * NUM_PATCH + i + 1].x = 
				cvRound(gMinPos.x + BLACK_LEN * (i%2 ? cvSqrt(2.0) : 1.0) *
						cos((BLACK_ANGLE - gAngle + i*45.0)/180.0*CV_PI));
			m_patchPos[place * NUM_PATCH + i + 1].y = 
				cvRound(gMinPos.y + BLACK_LEN * (i%2 ? cvSqrt(2.0) : 1.0) * 
						sin((BLACK_ANGLE - gAngle + i*45.0)/180.0*CV_PI));
		}

		// ROI解除
		cvResetImageROI(m_camera);
	}

	// 画像バッファの解放
	cvReleaseImage(&rotateMacbeth);
	cvReleaseImage(&match);
	cvReleaseMat(&rotateMat);

	return true;
}

//------------------------------------------------------------------------------
// パッチの色を取得
bool Analysis::GetPatchColor(int frame, bool ref)
{
	// パッチ画素値の平均と標準偏差を算出
	for(int i=0; i<NUM_PATCH * NUM_MACBETH; i++)
	{
		// 結果画像にパッチを描画
		cvRectangle(m_show, cvPoint(m_patchPos[i].x-PATCH_SIZE, m_patchPos[i].y-PATCH_SIZE), 
			cvPoint(m_patchPos[i].x+PATCH_SIZE, m_patchPos[i].y+PATCH_SIZE),
			cvScalar(255,0,0), CV_FILLED);

		// パッチROI設定
		cvSetImageROI(m_camera, cvRect(m_patchPos[i].x-PATCH_SIZE, m_patchPos[i].y-PATCH_SIZE, 
			PATCH_SIZE*2+1, PATCH_SIZE*2+1));

		// パッチの平均と標準偏差を計算
		if (ref) cvAvgSdv(m_camera, &m_meanRef[i], &m_stdevRef[i]);
		else     cvAvgSdv(m_camera, &m_mean[i],    &m_stdev[i]);

		// ROI解除
		cvResetImageROI(m_camera);
	}
	// 両マクベスの平均
	for (int i=0; i < NUM_PATCH; i ++)
	{
		if (ref)
		{
			m_miniPatchRef[i].val[0]  
				= (m_meanRef[i].val[0] + m_meanRef[i + NUM_PATCH].val[0]) / 2.0;
			m_miniPatchRef[i].val[1]  
				= (m_meanRef[i].val[1] + m_meanRef[i + NUM_PATCH].val[1]) / 2.0;
			m_miniPatchRef[i].val[2]  
				= (m_meanRef[i].val[2] + m_meanRef[i + NUM_PATCH].val[2]) / 2.0;
		}
		else
		{
			m_miniPatch[i][frame].val[0]  
				= (m_mean[i].val[0] + m_mean[i + NUM_PATCH].val[0]) / 2.0;
			m_miniPatch[i][frame].val[1]  
				= (m_mean[i].val[1] + m_mean[i + NUM_PATCH].val[1]) / 2.0;
			m_miniPatch[i][frame].val[2]  
				= (m_mean[i].val[2] + m_mean[i + NUM_PATCH].val[2]) / 2.0;
		}
	}

	// 結果の出力
	SPRLOG "%d, ", frame);
	for(int i = 0; i<NUM_PATCH * NUM_MACBETH; i++)
	{
		if (ref) SPRLOG "%s%.2f, %.2f, %.2f, ", m_logString, 
					m_meanRef[i].val[2], m_meanRef[i].val[1], m_meanRef[i].val[0]);
		else     SPRLOG "%s%.2f, %.2f, %.2f, ", m_logString, 
					m_mean[i].val[2], m_mean[i].val[1], m_mean[i].val[0]);
	}
	for(int i = 0; i<NUM_PATCH * NUM_MACBETH; i++)
	{
		if (ref) SPRLOG "%s%.2f, %.2f, %.2f, ", m_logString, 
					m_stdevRef[i].val[2], m_stdevRef[i].val[1], m_stdevRef[i].val[0]);
		else     SPRLOG "%s%.2f, %.2f, %.2f, ", m_logString, 
					m_stdev[i].val[2], m_stdev[i].val[1], m_stdev[i].val[0]);
	}
	this->WriteLog(false, true);

	// 結果画像の表示
	cvSetImageROI(m_show, 
		cvRect(m_camera->width-SEARCH_AREA, 0,  SEARCH_AREA, SEARCH_AREA));
	this->ShowImage();
	cvSetImageROI(m_show, 
		cvRect(0, m_camera->height-SEARCH_AREA,   SEARCH_AREA, SEARCH_AREA));
	this->ShowImage(DISP2);

	cvResetImageROI(m_show);

	cvWaitKey(1);

	return true;
}

//------------------------------------------------------------------------------
// パッチの色を取得
bool Analysis::GetAllPatchColor(bool ref)
{
	// ログにヘッダーを書き込む
	SPRLOG ""); WriteLog();
	SPRLOG "Patch Color"); WriteLog();
	const char *rgb = "RGB";
	const char *ms = "ms";
	SPRLOG "Frame, ");
	for(int i=0; i<NUM_PATCH*6; i++) 
		SPRLOG "%sp%d_%c%c, ", m_logString, (i%54)/3, rgb[i%3], ms[i/54]);
	WriteLog(false, true);

	// 全フレーム処理
	for(int frame = 0; frame < m_numFrames; frame ++)
	{
		// 画像読み込み
		this->ReadImage(frame, ref);

		if (!this->GetPatchColor(frame, ref)) return false;
	}

	return true;
}

//------------------------------------------------------------------------------
// 色補正係数を算出
bool Analysis::CorrectionMat()
{
	// ログにヘッダーを書き込む
	SPRLOG ""); WriteLog();
	SPRLOG "Color Correction"); WriteLog();

	CvMat *s = cvCreateMat(SSIZE, SSIZE, CV_64FC1);
	CvMat *y = cvCreateMat(SSIZE, COLOR, CV_64FC1);
	CvMat *si = cvCreateMat(SSIZE, SSIZE, CV_64FC1);
	CvMat *x = cvCreateMat(1, SSIZE, CV_64FC1);
	CvMat *c = cvCreateMat(1, COLOR, CV_64FC1);

	for (int frame = 0; frame < m_numFrames; frame ++)
	{
		// S行列を求める
		for (int row = 0; row < SSIZE; row ++)
		{
			for (int col = 0; col < SSIZE; col ++)
			{
				double val = 0;
				for (int patch = 0; patch < NUM_PATCH; patch ++)
					val += (GetMRVal(patch, frame, row) * GetMRVal(patch, frame, col));
				cvmSet(s, row, col, val);
			}
		}

		// Y行列を求める
		for (int row = 0; row < SSIZE; row ++)
		{
			for (int col = 0; col < COLOR; col ++)
			{
				double val = 0;
				for (int patch = 0; patch < NUM_PATCH; patch ++)
					val += (GetMRVal(patch, frame, row) * m_miniPatchRef[patch].val[col]);
				cvmSet(y, row, col, val);
			}
		}

		// Sの逆行列を求める
		cvInvert(s, si, CV_LU);

		// 係数行列を求める
		if (m_bMat    != NULL) cvReleaseMat(&m_bMat);
		m_bMat = cvCreateMat(SSIZE, COLOR, CV_64FC1);
		cvMatMul(si, y, m_bMat);

		SPRLOG "B Matrix of frame %d", frame); WriteLog();
		for(int col = 0; col < COLOR; col ++)
		{
			*m_logString = '\0';
			for (int row = 0; row < SSIZE; row ++)
			{
				SPRLOG "%s%.4f, ", m_logString, cvmGet(m_bMat, row, col));
			}
			WriteLog();
		}

		// 画像読み込み
		this->ReadImage(frame, false);

		// 色変換
		for (int i = 0; i < m_camera->width * m_camera->height * COLOR; i += COLOR)
		{
			double v0 = (double)((BYTE)m_camera->imageData[i  ]);
			double v1 = (double)((BYTE)m_camera->imageData[i+1]);
			double v2 = (double)((BYTE)m_camera->imageData[i+2]);
			for (int col = 0; col < SSIZE; col ++)
				cvmSet(x,0, col, GetMRVal2(v0, v1, v2, col));
			cvMatMul(x, m_bMat, c);
			m_camera->imageData[i  ] = ClipValue(cvmGet(c, 0, 0));
			m_camera->imageData[i+1] = ClipValue(cvmGet(c, 0, 1));
			m_camera->imageData[i+2] = ClipValue(cvmGet(c, 0, 2));

			// DEBUG
			if (((i/3) % m_camera->width == 400) && ((i/3) / m_camera->width == 500))
			{
				printf ("400,500 : %f %f %f -> %d %d %d\n",
					v0, v1, v2, (BYTE)m_camera->imageData[i  ], (BYTE)m_camera->imageData[i+1], 
					(BYTE)m_camera->imageData[i+2]);
			}
		}

		// 変換後のパッチ色計算
		this->GetPatchColor(frame);

		// 表示
		cvCopy(m_camera, m_show);
		this->ShowImage();
		cvWaitKey(100);
	}

	cvReleaseMat(&s);
	cvReleaseMat(&y);
	cvReleaseMat(&si);
	cvReleaseMat(&x);
	cvReleaseMat(&c);

	return true;
}

//------------------------------------------------------------------------------
// 重回帰推定行列の要素値を返す
double Analysis::GetMRVal(int patch, int frame, int index)
{
	return GetMRVal2(m_miniPatch[patch][frame].val[0], 
		m_miniPatch[patch][frame].val[1], m_miniPatch[patch][frame].val[2], index);

	return 0;
}

//------------------------------------------------------------------------------
// 重回帰推定行列の要素値を返す
double Analysis::GetMRVal2(double v0, double v1, double v2, int index)
{
	switch (index)
	{
	case  0: return 1.0;
	case  1: return v0;
	case  2: return v1;
	case  3: return v2;
	case  4: return v0 * v1;
	case  5: return v1 * v2;
	case  6: return v2 * v0;
	case  7: return v0 * v0;
	case  8: return v1 * v1;
	case  9: return v2 * v2;
	case 10: return v0 * v1 * v2;
	}
	return 0;
}

//------------------------------------------------------------------------------
// 算出値のクリッピング
char Analysis::ClipValue(double val)
{
	// 四捨五入で丸める
	int intVal = cvRound(val);

	// クリッピング
	if (intVal > 255) intVal = 255;
	if (intVal < 0  ) intVal = 0;

	return (char)intVal;
}

//------------------------------------------------------------------------------
// 解析画像を表示
bool Analysis::ShowImage(char *window)
{
	// 表示のスケーリング
	int width, height;
	if (m_show->roi) width = m_show->roi->width, height = m_show->roi->height;
	else			 width = m_show->width,      height = m_show->height;
	int showHeight = height * SHOW_WIDTH / width;
	if (!strcmp(window, DISP1)) m_showScale[0] = (float)SHOW_WIDTH / width;

	// 表示
	IplImage *temp = cvCreateImage(cvSize(SHOW_WIDTH, showHeight), IPL_DEPTH_8U, 3);
	cvResize(m_show, temp, CV_INTER_LINEAR);
	cvShowImage(window, temp);

	// 後処理
	cvWaitKey(1);
	cvReleaseImage(&temp);
	return true;
}

//------------------------------------------------------------------------------
// ログを出力
bool Analysis::WriteLog(bool gLog, bool fileOnly)
{
	fprintf(gLog ? m_gLog : m_log, "%s\n", m_logString);
	fflush(gLog ? m_gLog : m_log);

	if (fileOnly) return true;

#ifdef _CONSOLE
	if (gLog && strlen(m_logString) > 0) fprintf(stdout, "***** ");
	fprintf(stdout, "%s\n", m_logString);
	fflush(stdout);
#else
	if (gLog) OutputDebugString("***** ");
	OutputDebugString(m_logString);
#endif

	return true;
}

//------------------------------------------------------------------------------
// マウスクリックコールバック
void Analysis::OnMouse(int disp, int ev, int x, int y, int flags)
{
	if (ev == CV_EVENT_LBUTTONDOWN)
	{
		printf("Disp:%d x,y=%d,%d\n", 
			disp, (int)(x/m_showScale[disp-1]), (int)(y/m_showScale[disp-1]));
	}
}

//------------------------------------------------------------------------------
// マウスクリックコールバック（グローバル）
void OnMouse1(int ev, int x, int y, int flags, void* param)
{
	((Analysis*)param)->OnMouse(1, ev, x, y, flags);
}
