#include "Detect.h"

//-------------------------------------------------------------------------------
// コンストラクタ
CDetect::CDetect(void)
{
	m_Input    = NULL;
	m_Template = NULL;
	m_PatchPos = NULL;
}

//-------------------------------------------------------------------------------
// デストラクタ
CDetect::~CDetect(void)
{
	SAFE_RELEASEIMG(m_Input);
	SAFE_RELEASEIMG(m_Template);
	SAFE_DELETEA(m_PatchPos);
}

//-------------------------------------------------------------------------------
// 処理画像をセット
bool CDetect::SetImage(const char *filename)
{
	SAFE_RELEASEIMG(m_Input);
	m_Input = cvLoadImage(filename);
	printf("Reading : %s\n", filename);
	if (!m_Input) ERROR_RET("Can't open input file for detection.");

	return true;
}

//-------------------------------------------------------------------------------
// テンプレート画像をセット
bool CDetect::SetTemplate(const char *filename)
{
	SAFE_RELEASEIMG(m_Template);
	m_Template = cvLoadImage(filename);
	if (!m_Template) ERROR_RET("Can't open template file.");

	return true;
}

//-------------------------------------------------------------------------------
// 検出
bool CDetect::Matching(CvPoint *matchPos, double *matchAngle)
{
	// 画像サイズ(ROIに対応）
	int width, height;
	if (m_Input->roi) width = m_Input->roi->width, height = m_Input->roi->height;
	else			  width = m_Input->width,      height = m_Input->height;

	// 方向別テンプレートマッチング
	double gMinC = -1.0;
	double startTime = omp_get_wtime();

	// 角度のループを並列処理する
#ifdef _OPENMP
#pragma omp parallel for schedule(dynamic)
#endif
	for(int i = 0; i < (int)(360.0 / m_RotateStep); i++)
	{
		double angle = m_RotateStep * i;

		// マッチングの準備
		CvMat *rotateMat = cvCreateMat(2, 3, CV_32FC1);
		IplImage *rotateTemplate = cvCreateImage(cvGetSize(m_Template), 
			IPL_DEPTH_8U, COLOR);
		IplImage *match = cvCreateImage(
			cvSize(width - m_Template->width + 1, height - m_Template->height + 1),
			IPL_DEPTH_32F , 1);

		// テンプレートの回転
		cv2DRotationMatrix(cvPoint2D32f(m_Template->width/2.0, 
			m_Template->height/2.0), angle, 1.0, rotateMat);
		cvWarpAffine(m_Template, rotateTemplate, rotateMat, 
			CV_WARP_FILL_OUTLIERS, cvScalarAll(255.0));

		// マッチング
		cvMatchTemplate(m_Input, rotateTemplate, match, CV_TM_SQDIFF_NORMED);
		CvPoint minPos, maxPos;
		double minC, maxC;
		cvMinMaxLoc(match, &minC, &maxC, &minPos, &maxPos);

		// 最大相関の検出（共有変数に書き込むのでクリティカルにする）
		#pragma omp critical(c1)
		{
			if (minC < gMinC || gMinC < 0)
			{
				*matchAngle = angle;
				gMinC = minC;
				matchPos->x = minPos.x + (m_Template->width  / 2);
				matchPos->y = minPos.y + (m_Template->height / 2);
				if (m_Input->roi)
				{
					matchPos->x += m_Input->roi->xOffset;
					matchPos->y += m_Input->roi->yOffset;
				}
			}
		}

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

	double elapsedTime = (omp_get_wtime() - startTime) * 1000.0;
	printf("Matching Angle %3.0f, Coef %.4f, Position(%4d, %4d), Time %.0fms\n", 
		*matchAngle, gMinC, matchPos->x, matchPos->y, elapsedTime);

	return true;
}

//-------------------------------------------------------------------------------
// パッチの平均色の計算
bool CDetect::CalcPatchColor(CvMat **rgb)
{
	SAFE_RELEASEMAT(*rgb);
	*rgb = cvCreateMat(m_NumPatch, COLOR, CV_32FC1);

	for (int i = 0; i < m_NumPatch; i++)
	{
		int x1 = m_PatchPos[i].x - m_PatchSize;
		int y1 = m_PatchPos[i].y - m_PatchSize;
		int x2 = m_PatchPos[i].x + m_PatchSize;
		int y2 = m_PatchPos[i].y + m_PatchSize;
		int psize = m_PatchSize * 2 + 1;
		CvScalar mean, stdev;

		if (   (x1 < 0 || x1 >= m_Input->width || y1 < 0 || y1 > m_Input->height)
			&& (x2 < 0 || x2 >= m_Input->width || y2 < 0 || y2 > m_Input->height))
		{
			// パッチが画像の
			mean.val[0] = mean.val[1] = mean.val[2] = -1.0;
		}
		else
		{
			// パッチROI設定
			cvSetImageROI(m_Input, cvRect(x1, y1, psize, psize));

			// パッチの平均と標準偏差を計算
			cvAvgSdv(m_Input, &mean, &stdev);
		}

		// 行列に代入
		for(int c=0; c<COLOR; c++) cvmSet(*rgb, i, c, mean.val[c]);
	}
	cvResetImageROI(m_Input);

	return true;
}

//-------------------------------------------------------------------------------
// パッチROIの描画
bool CDetect::DrawROI(IplImage *img)
{
	for (int i = 0; i < m_NumPatch; i++)
	{
		int x1 = m_PatchPos[i].x - m_PatchSize;
		int y1 = m_PatchPos[i].y - m_PatchSize;
		int x2 = m_PatchPos[i].x + m_PatchSize;
		int y2 = m_PatchPos[i].y + m_PatchSize;

		// 結果画像にパッチを描画
		cvRectangle(img, cvPoint(x1, y1), cvPoint(x2, y2), 
			cvScalar(255,0,0), CV_FILLED);
	}

	return true;
}

//-------------------------------------------------------------------------------
// パッチ画像の生成
IplImage* CDetect::GenPatchedImage()
{
	IplImage *patched = (IplImage*)cvClone(m_Input);

	this->DrawROI(patched);

	return patched;
}