#include "MeanShiftSegger.h"
#include "math.h"


int max(int x,int y)
{
	if(x > y)
		return x;

	return y;

}
int min(int x,int y)
{
	if(x > y)
		return y;

	return x;

}
/***********************************************************************
* 函數(shù)名稱：
*     MeanShiftSegger()
*
*說明：構(gòu)造函數(shù) 
***********************************************************************/
MeanShiftSegger::MeanShiftSegger():
imgWidth(0),imgHeight(0){
    for(int i = 0; i < HISTOGRAM_LENGTH; i ++)
		currHistogram[i] = 0;
    for(i = 0; i < HISTOGRAM_LENGTH; i ++)
		tempHistogram[i] = 0;
	//memset(currHistogram,0,sizeof(float) * HISTOGRAM_LENGTH);
	//memset(tempHistogram,0,sizeof(float) * HISTOGRAM_LENGTH);
	//SetZeroHistogram(currHistogram);
	//SetZeroHistogram(tempHistogram);
	return;

}
/***********************************************************************
* 函數(shù)名稱：
*     ~MeanShiftSegger()
* 說明：
*     析構(gòu)函數(shù)
***********************************************************************/
MeanShiftSegger::~MeanShiftSegger(){

}

/***********************************************************************
* 函數(shù)名稱：
*     InitMeanShiftTracker()
* 參數(shù)：
*     unsigned char * firstFrame 第一幀緩沖
*     int frameWidth-幀寬度
*     int frameHeight-幀高度
*     int targetPosX-目標(biāo)位置橫坐標(biāo)
*     int targetPosY-目標(biāo)位置縱坐標(biāo)
*     int targetWidth-目標(biāo)寬度
*     int targetHeight-目標(biāo)高度
* 說明：MeanShift過程跟蹤初始化程序
***********************************************************************/

void MeanShiftSegger::InitMeanShiftTracker(unsigned char*  firstFrame,int frameWidth,int frameHeight,int targetPosX,int targetPosY,int targetWidth,int targetHeight){
	this->imgWidth = frameWidth;
	this->imgHeight = frameHeight;

	this->currentX = targetPosX;
	this->currentY = targetPosY;

	this->trackWinHeight = targetHeight;
	this->trackWinWidth = targetWidth;
}


/***********************************************************************
* 函數(shù)名稱：
*     CalculateHistogramSp
* 參數(shù)：
*     image frame-當(dāng)前輸入幀
*     char kernel-核類型選擇（目前只支持Uniform核）
*     float * histogram-直方圖緩沖
* 說明：統(tǒng)計(jì)直方圖，建立區(qū)域模型（無參數(shù)概率密度估計(jì)）
***********************************************************************/
int MeanShiftSegger::CalculateHistogramSp(unsigned char*  frame,char kernel, float *histogram){
	int pxValue = 0;

	for(int i = 0 ; i < HISTOGRAM_LENGTH; i++)
		histogram[i] = 0;

	if(kernel == 'U')
	{

		for(int j = max(0,currentY - trackWinHeight / 2);j < min(currentY + trackWinHeight / 2,imgHeight - 1); j ++){
			for(int i = max(0,currentX - trackWinWidth / 2); i < min(currentX + trackWinWidth / 2,imgWidth - 1);i ++){
				pixel r = frame[j * imgWidth * 3 + i * 3] / 16;
				pixel g = frame[j * imgWidth * 3 + i * 3 + 1] / 16;
				pixel b = frame[j * imgWidth * 3 + i * 3 + 2] / 16;

				histogram[256 * (int)r + 16 * (int)g + (int)b] += 1;
				pxValue ++;

			}

		}

		for(int i = 0; i < HISTOGRAM_LENGTH; i ++)
			histogram[i] /= pxValue;
	}
	else
	{
		return 0;
	}
	return pxValue;
}

/***********************************************************************
* 函數(shù)名稱：
*     MeanShiftTrackerProcess
* 參數(shù)：
*     unsigned char *frame-當(dāng)前幀緩沖
* 說明：跟蹤中的MeanShift過程
***********************************************************************/
void MeanShiftSegger::MeanShiftProcessSp(unsigned char * frame){
	float weights[HISTOGRAM_LENGTH];

	float newX = 0.0;
	float newY = 0.0;

	for (int i=0;i<HISTOGRAM_LENGTH;i++)
	{
		if (currHistogram[i] >0.0 )
			weights[i] = (float)tempHistogram[i]/(float)currHistogram[i];
		//目標(biāo)模板直方圖/當(dāng)前直方圖
		else
			weights[i] = 0;
	}

	float sumOfWeights = 0.0;

		for(int j = max(0,currentY - trackWinHeight / 2);j < min(currentY + trackWinHeight / 2,imgHeight - 1); j ++){
			for(int i = max(0,currentX - trackWinWidth / 2); i < min(currentX + trackWinWidth / 2,imgWidth - 1);i ++){
			pixel r = frame[j * imgWidth * 3 + i * 3] / 16;
			pixel g = frame[j * imgWidth * 3 + i * 3 + 1] / 16;
			pixel b = frame[j * imgWidth * 3 + i * 3 + 2] / 16;

			int ptr = (int)(256 * (int)r + 16 * (int)g + (int)b);

			newX += (weights[ptr] * (float)i);
			newY += (weights[ptr] * (float)j);

			sumOfWeights += weights[ptr];


		}

	}
	if(sumOfWeights != 0){
		currentX = int((newX/sumOfWeights) + 0.5);
		currentY = int((newY/sumOfWeights) + 0.5);
	}
	return;

}


/***********************************************************************
* 函數(shù)名稱
*     MeanShiftTrackProcess 
* 參數(shù)：
*     image frame-當(dāng)前輸入幀
*     int frameNumber-輸入幀號
* 說明：MeanShift跟蹤主調(diào)用過程
***********************************************************************/
void MeanShiftSegger::MeanShiftTrackProcess(unsigned char*  frame,int frameNumber)
{
	if(frameNumber == 0){
		//在指定位置建立目標(biāo)模型
		CalculateHistogramSp(frame,'U',this->tempHistogram);
	}
	else
	{
		int stopThreshold = 10;
		int iteratorCoumt = 0;
		while(iteratorCoumt < stopThreshold){

			//在后續(xù)幀中檢測目標(biāo)新位置
			CalculateHistogramSp(frame,'U',this->currHistogram);
			MeanShiftProcessSp(frame); //獲得新位置

			//更新直方圖
			iteratorCoumt++;
		}
		
		DrawTrackBox(frame);

	}
}


/***********************************************************************
* 函數(shù)名稱：
*     DrawTrackBox
* 參數(shù)：
*     unsigned char * frame-當(dāng)前幀緩沖
* 說明：在輸出中畫跟蹤窗口
***********************************************************************/
void MeanShiftSegger::DrawTrackBox(unsigned char*  frame)
{

	for(int i = currentX; i < min(imgWidth,currentX + trackWinWidth); i ++)
	{
		frame[currentY * imgWidth * 3 + i * 3 + 0] = 0;
		frame[currentY * imgWidth * 3 + i * 3 + 1] = 0;
		frame[currentY * imgWidth * 3 + i * 3 + 2] = 255;

		frame[min(imgHeight - 1,currentY + trackWinHeight) * imgWidth * 3 + i * 3 + 0] = 0;
		frame[min(imgHeight - 1,currentY + trackWinHeight) * imgWidth * 3 + i * 3 + 1] = 0;
		frame[min(imgHeight - 1,currentY + trackWinHeight) * imgWidth * 3 + i * 3 + 2] = 255;
	}

	for(int j = currentY; j < min(imgHeight - 1,currentY + trackWinHeight); j ++)
	{

		frame[j * imgWidth * 3 + currentX * 3 + 0] = 0;
		frame[j * imgWidth * 3 + currentX * 3 + 1] = 0;
		frame[j * imgWidth * 3 + currentX * 3 + 2] = 255;

		frame[j * imgWidth * 3 + min(imgWidth - 1,currentX + trackWinWidth) * 3 + 0] = 0;
		frame[j * imgWidth * 3 + min(imgWidth - 1,currentX + trackWinWidth) * 3 + 1] = 0;
		frame[j * imgWidth * 3 + min(imgWidth - 1,currentX + trackWinWidth) * 3 + 2] = 255;
	}


}

/***********************************************************************
* 函數(shù)名稱：
*     CalculateBhattacharyya
* 說明：計(jì)算Bhattachryya
***********************************************************************/
float MeanShiftSegger::CalculateBhattacharyya(float initHistogram [HISTOGRAM_LENGTH],float tempHistogram[HISTOGRAM_LENGTH]){
	float dis = 0;

	for(int i = 0;i < HISTOGRAM_LENGTH ;i ++)
		dis += float(sqrt(double(currHistogram[i] * tempHistogram[i])));

	return dis;
}