#include "Tracking.h"

//-------------------------------------------------------------------------------
// コンストラクタ
CTracking::CTracking(void)
{
	m_Point[0] = NULL;
	m_Point[1] = NULL;
	m_PointInit = NULL;
	m_Gray = NULL;
	m_GrayPre = NULL;
	m_Pyramid = NULL;
	m_PyramidPre = NULL;
	m_Mask1 = NULL;
	m_Mask2 = NULL;
	m_Mask3 = NULL;
	m_Mask4 = NULL;
	m_Status = NULL;
}

//-------------------------------------------------------------------------------
// デストラクタ
CTracking::~CTracking(void)
{
	this->Release();
}

//-------------------------------------------------------------------------------
// メモリ解放
void CTracking::Release(void)
{
	SAFE_DELETEA(m_Point[0]);
	SAFE_DELETEA(m_Point[1]);
	SAFE_DELETEA(m_PointInit);
	SAFE_RELEASEIMG(m_Gray);
	SAFE_RELEASEIMG(m_GrayPre);
	SAFE_RELEASEIMG(m_Pyramid);
	SAFE_RELEASEIMG(m_PyramidPre);
	SAFE_RELEASEIMG(m_Mask1);
	SAFE_RELEASEIMG(m_Mask2);
	SAFE_RELEASEIMG(m_Mask3);
	SAFE_RELEASEIMG(m_Mask4);
	SAFE_DELETEA(m_Status)
}

//-------------------------------------------------------------------------------
// 初期化
bool CTracking::Init(const CvPoint *point, const int numPoint,
					 const IplImage *img)
{
	this->Release();

	m_NumPoint = numPoint;
	m_Criteria = cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03);
	m_PyrFlag  = 0;
	m_TotalMovement = 0;

	m_Point[0]   = new CvPoint2D32f [numPoint];
	m_Point[1]   = new CvPoint2D32f [numPoint];
	m_PointInit  = new CvPoint [numPoint];
	m_Gray       = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1);
	m_GrayPre    = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1);
	m_Pyramid    = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1);
	m_PyramidPre = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1);
	m_Mask1		 = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1);
	m_Mask2		 = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1);
	m_Mask3		 = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1);
	m_Mask4		 = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1);
	m_Status     = new char [numPoint];

	for (int i = 0; i < numPoint; i++)
	{
		m_PointInit[i] = point[i];
		m_Point[0][i] = cvPointTo32f(point[i]);
	}

	cvCvtColor(img, m_GrayPre, CV_BGR2GRAY);

	cvFindCornerSubPix(m_GrayPre, m_Point[0], numPoint, 
		cvSize(TRACK_WIN_SIZE, TRACK_WIN_SIZE), cvSize(-1, -1),
		m_Criteria);

	return true;
}

//-------------------------------------------------------------------------------
// フレーム処理（トラッキング）
bool CTracking::Frame(const IplImage *img)
{
	// グレースケール変換
	cvCvtColor(img, m_Gray, CV_BGR2GRAY);

	// オプティカルフロー検出
	cvCalcOpticalFlowPyrLK(m_GrayPre, m_Gray, m_PyramidPre, m_Pyramid,
		m_Point[0], m_Point[1], m_NumPoint, 
		cvSize(TRACK_WIN_SIZE, TRACK_WIN_SIZE),
		3, m_Status, 0, m_Criteria, m_PyrFlag);

	// 次回から前フレームのピラミッド計算を省略する
	m_PyrFlag |= CV_LKFLOW_PYR_A_READY;

	// バッファの交換
	IplImage *swap;
	CV_SWAP(m_Gray,    m_GrayPre,    swap);
	CV_SWAP(m_Pyramid, m_PyramidPre, swap);
	CvPoint2D32f *swapP;
	CV_SWAP(m_Point[0], m_Point[1], swapP);

	// 移動量算出
	for (int i = 0; i < m_NumPoint; i ++)
	{
		m_Movement = sqrt(
			pow((double)(m_Point[0][i].x - m_Point[1][i].x), 2.0) + 
			pow((double)(m_Point[0][i].y - m_Point[1][i].y), 2.0));
		m_TotalMovement += m_Movement;
	}

	// トラッキング成否
	for (int i = 0; i < m_NumPoint; i ++) if (!m_Status[i]) return false;
	this->ValidatePoints();
	if (m_Validate) this->CalcROIMask();

	return true;
}

//-------------------------------------------------------------------------------
// 追跡点を描画
bool CTracking::DrawPoints(IplImage *img)
{
	// 点の描画
	for (int i = 0; i < m_NumPoint; i++)
	{
		cvCircle(img, cvPointFrom32f(m_Point[0][i]), 5, 
			CV_RGB(0, 0, i*60), CV_FILLED);
		cvLine(img, cvPointFrom32f(m_Point[0][i]), 
			cvPointFrom32f(m_Point[0][(i+1) % m_NumPoint]), CV_RGB(0, 0, 255));
	}

	// 初期点の描画
	for (int i = 0; i < m_NumPoint; i++)
	{
		cvLine(img, m_PointInit[i], 
			m_PointInit[(i+1) % m_NumPoint], CV_RGB(0, 255, 0));
	}

	return true;
}

//-------------------------------------------------------------------------------
// ROIを描画
bool CTracking::DrawROI(IplImage *img)
{
	cvSet(img, CV_RGB(255, 0, 0), m_Mask1);
	cvSet(img, CV_RGB(0, 255, 0), m_Mask2);
	cvSet(img, CV_RGB(0, 0, 255), m_Mask3);
	cvSet(img, CV_RGB(244, 163, 46), m_Mask4);

	return true;
}

//-------------------------------------------------------------------------------
// 追跡点を描画
void CTracking::ValidatePoints()
{
	int conX[8][2] = {{0,2},{0,3},{0,4},{1,2},{1,3},{1,4},{2,3},{2,4}};
	int conY[8][2] = {{0,1},{0,2},{0,3},{1,2},{3,2},{4,1},{4,1},{4,3}};

	m_Validate = true;
	for (int i = 0; i < 8; i++)
	{
		if (m_Point[0][conX[i][0]].x >= m_Point[0][conX[i][1]].x) m_Validate = false;
		if (m_Point[0][conY[i][0]].y >= m_Point[0][conY[i][1]].y) m_Validate = false;
	}
}

//-------------------------------------------------------------------------------
// ROIマスク画像を作る
void CTracking::CalcROIMask()
{
	// ROIマスク画像1
	// 0____________ 4
	//  |           |
	//  |           |
	//  |           |
	// 1|●       ●|3
	//  舌　　　   /
	//   舌_______/
	//        2

	int points1[2][3] = {{0, 2, 1}, {4, 2, 3}};

	cvSet(m_Mask1, cvScalar(0));
	for (int i = 0; i < 2; i ++)
	{
		CvPoint2D32f roi;
		float cx1 = (m_Point[0][points1[i][0]].x + m_Point[0][points1[i][1]].x) / 2;
		float cy1 = (m_Point[0][points1[i][0]].y + m_Point[0][points1[i][1]].y) / 2;
		roi.x = (m_Point[0][points1[i][2]].x + cx1) / 2;
		roi.y = (m_Point[0][points1[i][2]].y + cy1) / 2;
		cvDrawCircle(m_Mask1, cvPointFrom32f(roi), CALC_ROI_RADIUS, cvScalar(1), CV_FILLED);
	}
	
	// ROIマスク画像2
	// 0____________ 4
	//  |   ● ●   |
	//  |           |
	//  |           |
	// 1|           |3
	//  舌　　　   /
	//   舌_______/
	//        2

	int points2[2][4] = {{0, 3, 0, 4}, {4, 1, 4, 0}};

	cvSet(m_Mask2, cvScalar(0));
	for (int i = 0; i < 2; i ++)
	{
		CvPoint2D32f roi;
		float cx1 = m_Point[0][points2[i][0]].x + (m_Point[0][points2[i][1]].x - m_Point[0][points2[i][0]].x) / 4;
		float cy1 = m_Point[0][points2[i][0]].y + (m_Point[0][points2[i][1]].y - m_Point[0][points2[i][0]].y) / 4;
		float cx2 = (m_Point[0][points2[i][2]].x + m_Point[0][points2[i][3]].x) / 2 ;
		float cy2 = (m_Point[0][points2[i][2]].y + m_Point[0][points2[i][3]].y) / 2;
		roi.x = (cx1 + cx2) / 2;
		roi.y = (cy1 + cy2) / 2;
		cvDrawCircle(m_Mask2, cvPointFrom32f(roi), CALC_ROI_RADIUS, cvScalar(1), CV_FILLED);
	}

	// ROIマスク画像3
	// 0____________ 4
	//  |           |
	//  |           |
	//  |           |
	// 1|   ● ●   |3
	//  舌　　　   /
	//   舌_______/
	//        2

	int points3[2][3] = {{0, 3, 2}, {4, 1, 2}};

	cvSet(m_Mask3, cvScalar(0));
	for (int i = 0; i < 2; i ++)
	{
		CvPoint2D32f roi;
		float cx1 = m_Point[0][points3[i][0]].x + (m_Point[0][points3[i][1]].x - m_Point[0][points3[i][0]].x) / 4;
		float cy1 = m_Point[0][points3[i][0]].y + (m_Point[0][points3[i][1]].y - m_Point[0][points3[i][0]].y) / 4;
		roi.x = (m_Point[0][points3[i][2]].x + cx1) / 2;
		roi.y = (m_Point[0][points3[i][2]].y + cy1) / 2;
		cvDrawCircle(m_Mask3, cvPointFrom32f(roi), CALC_ROI_RADIUS, cvScalar(1), CV_FILLED);
	}

	// ROIマスク画像3
	// 0____________ 4
	//  |           |
	//  |           |
	//  |           |
	// 1|           |3
	//  舌　　　   /
	//   舌_●_●_/
	//        2

	int points4[2][2] = {{0, 2}, {4, 2}};

	cvSet(m_Mask4, cvScalar(0));
	for (int i = 0; i < 2; i ++)
	{
		CvPoint2D32f roi;
		roi.x = m_Point[0][points4[i][0]].x + (m_Point[0][points4[i][1]].x - m_Point[0][points4[i][0]].x) * 7 / 8;
		roi.y = m_Point[0][points4[i][0]].y + (m_Point[0][points4[i][1]].y - m_Point[0][points4[i][0]].y) * 7 / 8;
		cvDrawCircle(m_Mask4, cvPointFrom32f(roi), CALC_ROI_RADIUS, cvScalar(1), CV_FILLED);
	}


}

//-------------------------------------------------------------------------------
// ROIの平均色を計算
CvScalar CTracking::ROIColor1(IplImage *img)
{
	return cvAvg(img, m_Mask1);
}

CvScalar CTracking::ROIColor2(IplImage *img)
{
	return cvAvg(img, m_Mask2);
}

CvScalar CTracking::ROIColor3(IplImage *img)
{
	return cvAvg(img, m_Mask3);
}

CvScalar CTracking::ROIColor4(IplImage *img)
{
	return cvAvg(img, m_Mask4);
}