#include "MiniMacbeth.h"

MiniMacbeth::MiniMacbeth(void)
{
	m_macbeth = NULL;
	m_camera = NULL;
	m_show = NULL;
	m_fpOutput = NULL;
}

MiniMacbeth::~MiniMacbeth(void)
{
	if (m_macbeth != NULL) cvReleaseImage(&m_macbeth);
	if (m_camera != NULL) cvReleaseImage(&m_macbeth);
	if (m_show != NULL) cvReleaseImage(&m_show);
	if (m_fpOutput != NULL) fclose(m_fpOutput);

	cvDestroyAllWindows();
}

bool MiniMacbeth::Init()
{
	m_macbeth = cvLoadImage(MACBETH_PATH);
	if (!m_macbeth) return false;

	cvNamedWindow(DISP1);
//	cvNamedWindow(DISP2);

	return true;
}

bool MiniMacbeth::ReadImage(char *filename)
{
	// カメラ画像取り込み
	if (m_camera != NULL) cvReleaseImage(&m_camera);
	m_camera = cvLoadImage(filename);
	if (!m_camera) return false;

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

	return true;
}

bool MiniMacbeth::OpenOutputFile(char *filename)
{
	fopen_s(&m_fpOutput, filename, "w");
	if (!m_fpOutput) return false;

	const char *rgb = "RGB";
	const char *ms = "ms";
	for(int i=0; i<18*6; i++) fprintf(m_fpOutput, "p%d_%cm, ", (i%54)/3, rgb[i%3], ms[i/54]);
	fprintf(m_fpOutput, "\n");

	return true;
}

bool MiniMacbeth::Process()
{
	if (!m_camera || !m_macbeth) return false;

	// 探索結果用アレイ作成
	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;
	CvScalar ave[18], std[18];

	// 右上と左下のマクベスを順次処理
	for (int place=0; place<2; 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;
		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);

			cvMatchTemplate(m_camera, rotateMacbeth, match, CV_TM_SQDIFF);
			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;
			}
		}

		// 各パッチの平均値と標準偏差を計算
		printf("Best Angle:%.0f  min:%f  x,y=%d,%d\n", gAngle, gMinC, gMinPos.x, gMinPos.y);
		this->PatchProc(gMinPos.x, gMinPos.y, &ave[place*9], &std[place*9]);

		for(int i=0; i<8; i++)
		{
			int bx = cvRound(gMinPos.x + BLACK_LEN * (i%2 ? cvSqrt(2.0) : 1.0) *
				cos((BLACK_ANGLE - gAngle + i*45.0)/180.0*CV_PI));
			int by = cvRound(gMinPos.y + BLACK_LEN * (i%2 ? cvSqrt(2.0) : 1.0) * 
				sin((BLACK_ANGLE - gAngle + i*45.0)/180.0*CV_PI));
			this->PatchProc(bx, by, &ave[i+1+place*9], &std[i+1+place*9]);
		}
	}

	// 結果の表示
	for(int i=0; i<18; i++)
	{
		fprintf(m_fpOutput, "%.2f, %.2f, %.2f, %.2f, %.2f, %.2f, ", 
			ave[i].val[2], ave[i].val[1], ave[i].val[0], 
			std[i].val[2], std[i].val[1], std[i].val[0]);
	}
	fprintf(m_fpOutput, "\n");
	this->ShowImage();

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

	return true;
}

bool MiniMacbeth::PatchProc(int x, int y, CvScalar *ave, CvScalar *stdev)
{
	// 結果画像にパッチを描画
	cvRectangle(m_show, cvPoint(x-PATCH_SIZE, y-PATCH_SIZE), 
		cvPoint(x+PATCH_SIZE, y+PATCH_SIZE),
		cvScalar(255,0,0),CV_FILLED);

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

	// パッチの平均と標準偏差を計算
	cvAvgSdv(m_camera, ave, stdev);

	return true;
}

bool MiniMacbeth::ShowImage()
{
	IplImage *temp = cvCreateImage(cvSize(m_show->width/2, m_show->height/2), IPL_DEPTH_8U, 3);
	cvResize(m_show, temp, CV_INTER_LINEAR);
	cvShowImage(DISP1, temp);
	cvWaitKey(1);
	cvReleaseImage(&temp);
	return true;
}