#include "DetectCasmatch.h"

//-------------------------------------------------------------------------------
// コンストラクタ
CDetectCasmatch::CDetectCasmatch(void)
{
}

//-------------------------------------------------------------------------------
// デストラクタ
CDetectCasmatch::~CDetectCasmatch(void)
{
}

//-------------------------------------------------------------------------------
// 初期化
bool CDetectCasmatch::Init()
{
	// キャスマッチの参照テンプレート画像を読み込み
	CALL(this->SetTemplate(CASMATCH));

	m_NumPatch   = C_NUM_PATCH * NUM_CASMATCH;
	m_RotateStep = C_ROTATE_STEP;
	m_PatchSize  = C_PATCH_SIZE;

	SAFE_DELETEA(m_PatchPos);
	m_PatchPos = new CvPoint [m_NumPatch];

	return true;
}

//-------------------------------------------------------------------------------
// 検出処理
bool CDetectCasmatch::Detect()
{
	// ROIを使って右上と左下パッチを処理する
	CvRect roi;
	CvPoint pos;
	double angle;

	roi = cvRect(m_Input->width-SEARCH_AREA, 0,  SEARCH_AREA, SEARCH_AREA);
	cvSetImageROI(m_Input, roi);
	CALL(CDetect::Matching(&pos, &angle));
	CALL(CalcPatchPosition(pos, angle, 0));

	roi = cvRect(0, m_Input->height-SEARCH_AREA, SEARCH_AREA, SEARCH_AREA);
	cvSetImageROI(m_Input, roi);
	CALL(CDetect::Matching(&pos, &angle));
	CALL(CalcPatchPosition(pos, angle, 1));

	// ROI解除
	cvResetImageROI(m_Input);

	return true;
}

//-------------------------------------------------------------------------------
// 各パッチの位置と角度を計算する
bool CDetectCasmatch::CalcPatchPosition(CvPoint pos, double angle, int place)
{
	// 各パッチの中心座標を計算
	m_PatchPos[place * C_NUM_PATCH] = pos;
	for(int i=0; i < C_NUM_PATCH - 1; i++)
	{
		m_PatchPos[place * C_NUM_PATCH + i + 1].x = 
			cvRound(pos.x + BLACK_LEN * (i%2 ? cvSqrt(2.0) : 1.0) *
					cos((BLACK_ANGLE - angle + i*45.0)/180.0*CV_PI));
		m_PatchPos[place * C_NUM_PATCH + i + 1].y = 
			cvRound(pos.y + BLACK_LEN * (i%2 ? cvSqrt(2.0) : 1.0) * 
					sin((BLACK_ANGLE - angle + i*45.0)/180.0*CV_PI));
	}

	return true;
}

//-------------------------------------------------------------------------------
// 9パッチの平均色の計算
bool CDetectCasmatch::CalcPatchColor9(CvMat **rgb)
{
	SAFE_RELEASEMAT(*rgb);
	*rgb = cvCreateMat(C_NUM_PATCH, COLOR, CV_32FC1);

	CvMat *rgb18 = NULL;
	this->CalcPatchColor(&rgb18);

	// 平均を計算
	for(int i = 0; i < C_NUM_PATCH; i ++)
	{
		for (int c = 0; c < COLOR; c ++)
		{
			// 各キャスマッチから色を取得
			double sum = 0;
			int count = 0;
			for (int j = 0; j < NUM_CASMATCH; j++)
			{
				double val = GcvmGet(rgb18, i + C_NUM_PATCH * j, c);
				if (val >= 0) sum += val, count ++;
			}

			// 平均を算出（計測不可パッチを考慮）
			double mean;
			if (count < 1) mean = -1.0;
			else mean = sum / count;
			cvmSet(*rgb, i, c, mean);
		}
	}

	SAFE_RELEASEMAT(rgb18);

	return true;
}