/*
Robust Object Tracking via Locality Sensitive Histograms
Shengfeng He, Rynson W.H. Lau, Qingxiong Yang, Jiang Wang, Ming-Hsuan Yang
IEEE Transactions on Circuits and Systems for Video Technology (TCSVT)
*/

#ifndef HE_LSH_H
#define HE_LSH_H
#include "He_basic.h"

using namespace cv;
#define feature_bin_size 64
#define hist_bin_size 64
const int feature_QTZ = 256/feature_bin_size;
const int QTZ = 256/hist_bin_size;
#define down_scale 1
bool Spatial_mode = 0;
map<int, int> pallet;

template<typename T> inline T sqr(T x) { return x * x;}
template<class T> inline T vecSqrDist3(const Vec<T, 3> &v1, const Vec<T, 3> &v2) {return sqr(v1[0] - v2[0])+sqr(v1[1] - v2[1])+sqr(v1[2] - v2[2]);}


void LSH_recursive(double**out,Mat in,double**temp,double**temp_2w, double alphax,double alphay, int h,int w, int k
					, Point hist_lt, Point hist_rb)//temp_2w [2,w], alpha 0.6-0.95
{
	/*horizontal*/
	double**in_;
	double*temp_x=temp_2w[0];
	double *optr=out[k];
	for(int y=hist_lt.y;y<hist_rb.y;y++)
	{
		uchar* in_ptr = in.ptr<uchar>(y);
		double*tptr = temp[y];
		tptr[0]=((in_ptr[0])==k);
		double yc,yp=((in_ptr[0])==k);
		for(int x=hist_lt.x;x<hist_rb.x;x++)
		{
			tptr[x]=yc=(1-alphax)*((in_ptr[x])==k)+alphax*yp;
			yp=yc;
		}
		int w1=hist_rb.x-1;
		yp=((in_ptr[w1])==k);
		tptr[w1]=0.5*(tptr[w1]+yp);
		for(int x=hist_rb.x-2;x>=hist_lt.x;x--)
		{
			yc=(1-alphax)*((in_ptr[x])==k)+alphax*yp;
			tptr[x]=0.5*(tptr[x]+yc);
			yp=yc;
		}
	}

	/*vertical*/
	in_=temp;
	double*ycy,*ypy,*xcy;
	memcpy(optr,temp[0],sizeof(double)*w);
	for(int y=hist_lt.y+1;y<hist_rb.y;y++)
	{
		xcy=in_[y];
		ypy=optr + (y-1)*w;
		ycy=optr + y*w;
		for(int x=hist_lt.x;x<hist_rb.x;x++)
		{
			ycy[x]=(1-alphay)*xcy[x]+alphay*ypy[x];
		}
	}
	int h1=h-1;
	ycy=temp_2w[0];
	ypy=temp_2w[1];
	memcpy(ypy,in_[h1],sizeof(double)*w);
	for(int x=hist_lt.x;x<hist_rb.x;x++) optr[x]=0.5*(optr[x+h1*w]+ypy[x]);
	for(int y=hist_rb.y-2;y>=hist_lt.y;y--)
	{
		xcy=in_[y];//optr=out[y];
		for(int x=hist_lt.x;x<hist_rb.x;x++)
		{
			ycy[x]=(1-alphay)*xcy[x]+alphay*ypy[x];
			optr[x + y*w]=0.5*(optr[x+y*w]+ycy[x]);
		}
		memcpy(ypy,ycy,sizeof(double)*w);
	}
}

void IIF_LSH_recursive(double**out,Mat in,double**temp,double**temp_2w, double alphax,double alphay, int h,int w, int k
					   , Point hist_lt, Point hist_rb)//temp_2w [2,w], alpha 0.6-0.95
{
	/*horizontal*/
	double**in_;
	double*temp_x=temp_2w[0];
	double *optr=out[k];
	for(int y=hist_lt.y;y<hist_rb.y;y++)
	{
		uchar* in_ptr = in.ptr<uchar>(y);
		double*tptr = temp[y];
		tptr[0]=((in_ptr[0])==k);
		double yc,yp=((in_ptr[0])==k);
		for(int x=hist_lt.x;x<hist_rb.x;x++)
		{
			tptr[x]=yc=(1-alphax)*((in_ptr[x])==k)+alphax*yp;
			yp=yc;
		}
		int w1=hist_rb.x-1;
		yp=((in_ptr[w1])==k);
		tptr[w1]=0.5*(tptr[w1]+yp);
		for(int x=hist_rb.x-2;x>=hist_lt.x;x--)
		{
			yc=(1-alphax)*((in_ptr[x])==k)+alphax*yp;
			tptr[x]=0.5*(tptr[x]+yc);
			yp=yc;
		}
	}

	/*vertical*/
	in_=temp;
	double*ycy,*ypy,*xcy;
	memcpy(optr,temp[0],sizeof(double)*w);
	for(int y=hist_lt.y+1;y<hist_rb.y;y++)
	{
		xcy=in_[y];
		ypy=optr + (y-1)*w;
		ycy=optr + y*w;
		for(int x=hist_lt.x;x<hist_rb.x;x++)
		{
			ycy[x]=(1-alphay)*xcy[x]+alphay*ypy[x];
		}
	}
	int h1=h-1;
	ycy=temp_2w[0];
	ypy=temp_2w[1];
	memcpy(ypy,in_[h1],sizeof(double)*w);
	for(int x=hist_lt.x;x<hist_rb.x;x++) optr[x]=0.5*(optr[x+h1*w]+ypy[x]);
	for(int y=hist_rb.y-2;y>=hist_lt.y;y--)
	{
		xcy=in_[y];//optr=out[y];
		for(int x=hist_lt.x;x<hist_rb.x;x++)
		{
			ycy[x]=(1-alphay)*xcy[x]+alphay*ypy[x];
			optr[x + y*w]=0.5*(optr[x+y*w]+ycy[x]);
		}
		memcpy(ypy,ycy,sizeof(double)*w);
	}
}

inline void Compute_LSH(double**histogram, Mat in, double**histogram_temp,double**temp3w, double sigma,int h,int w, bool s_mode, Point hist_lt, Point hist_rb)
{
	Mat idx_img = in/QTZ;
	double t;
	{
		double alphay=exp(-sqrt(2.0)/(sigma*h));
		double alphax=exp(-sqrt(2.0)/(sigma*w));

		t = (double)cvGetTickCount();	
		for(int k = 0; k< hist_bin_size; k++)
		{
			LSH_recursive(histogram, idx_img, histogram_temp, temp3w, alphax,alphay, h, w, k, hist_lt, hist_rb);			
		}

		t = (double)cvGetTickCount() - t;
	}
}

inline void Compute_IIF_LSH(double**histogram, Mat in, double**histogram_temp,double**temp3w, double sigma,int h,int w, bool s_mode, Point hist_lt, Point hist_rb)
{
	Mat idx_img = in/feature_QTZ;
	double t;
	{
		double alphay=exp(-sqrt(2.0)/(sigma*h));
		double alphax=exp(-sqrt(2.0)/(sigma*w));

		t = (double)cvGetTickCount();	
		for(int k = 0; k< feature_bin_size; k++)
		{
			IIF_LSH_recursive(histogram, idx_img, histogram_temp, temp3w, alphax,alphay, h, w, k, hist_lt, hist_rb);			
		}

		t = (double)cvGetTickCount() - t;
	}
}

int Quantize(const Mat& img3f, Mat &idx1i, Mat &_color3f, Mat &_colorNum, double ratio, bool update_pallet)
{
	static const int clrNums[3] = {12, 12, 12};
	static const float clrTmp[3] = {clrNums[0] - 0.0001f, clrNums[1] - 0.0001f, clrNums[2] - 0.0001f};
	static const int w[3] = {clrNums[1] * clrNums[2], clrNums[2], 1};

	CV_Assert(img3f.data != NULL);
	idx1i = Mat::zeros(img3f.size(), CV_32S);
	int rows = img3f.rows, cols = img3f.cols;
	if (img3f.isContinuous() && idx1i.isContinuous())
	{
		cols *= rows;
		rows = 1;
	}

	// Build color pallet
	
	for (int y = 0; y < rows; y++)
	{
		const float* imgData = img3f.ptr<float>(y);
		int* idx = idx1i.ptr<int>(y);
		for (int x = 0; x < cols; x++, imgData += 3)
		{
			idx[x] = (int)(imgData[0]*clrTmp[0])*w[0] + (int)(imgData[1]*clrTmp[1])*w[1] + (int)(imgData[2]*clrTmp[2]);
			if(update_pallet)
				pallet[idx[x]] ++;
		}
	}
	int maxNum = hist_bin_size;

	// Fine significant colors
	if(update_pallet){
		maxNum = 0;
		{
			int count = 0;
			vector<pair<int, int>> num; // (num, color) pairs in num
			num.reserve(pallet.size());
			for (map<int, int>::iterator it = pallet.begin(); it != pallet.end(); it++)
				num.push_back(pair<int, int>(it->second, it->first)); // (color, num) pairs in pallet
			sort(num.begin(), num.end(), std::greater<pair<int, int>>());

			maxNum = (int)num.size();
			int maxDropNum = cvRound(rows * cols * (1-ratio));
			for (int crnt = num[maxNum-1].first; crnt < maxDropNum && maxNum > 1; maxNum--)
				crnt += num[maxNum - 2].first;
			maxNum = min(hist_bin_size,maxNum);
			//if (maxNum < 10)
			//	maxNum = min((int)pallet.size(), 100);
			pallet.clear();
			for (int i = 0; i < maxNum; i++)
				pallet[num[i].second] = i; 
	
			vector<Vec3i> color3i(num.size());
			for (unsigned int i = 0; i < num.size(); i++)
			{
				color3i[i][0] = num[i].second / w[0];
				color3i[i][1] = num[i].second % w[0] / w[1];
				color3i[i][2] = num[i].second % w[1];
			}
	
			for (unsigned int i = maxNum; i < num.size(); i++)
			{
				int simIdx = 0, simVal = INT_MAX;
				for (int j = 0; j < maxNum; j++)
				{
					int d_ij = vecSqrDist3(color3i[i], color3i[j]);
					if (d_ij < simVal)
						simVal = d_ij, simIdx = j;
				}
				pallet[num[i].second] = pallet[num[simIdx].second];
			}
		}
	}

	_color3f = Mat::zeros(1, maxNum, CV_32FC3);
	_colorNum = Mat::zeros(_color3f.size(), CV_32S);

	Vec3f* color = (Vec3f*)(_color3f.data);
	int* colorNum = (int*)(_colorNum.data);
	for (int y = 0; y < rows; y++) 
	{
		const Vec3f* imgData = img3f.ptr<Vec3f>(y);
		int* idx = idx1i.ptr<int>(y);
		for (int x = 0; x < cols; x++)
		{
			idx[x] = pallet[idx[x]];
			color[idx[x]] += imgData[x];
			colorNum[idx[x]] ++;
		}
	}
	for (int i = 0; i < _color3f.cols; i++)
		color[i] /= colorNum[i];

	return _color3f.cols;
}


void Color_LSH_recursive(double**out,Mat in,double**temp,double**temp_2w, double alphax,double alphay, int h,int w, int k
						 , Point hist_lt, Point hist_rb)//temp_2w [2,w], alpha 0.6-0.95
{
	/*horizontal*/
	double**in_;
	double*temp_x=temp_2w[0];
	double *optr=out[k];
	for(int y=hist_lt.y;y<hist_rb.y;y++)
	{
		int* in_ptr = in.ptr<int>(y);
		double*tptr = temp[y];
		tptr[0]=((in_ptr[0])==k);
		double yc,yp=((in_ptr[0])==k);
		for(int x=hist_lt.x;x<hist_rb.x;x++)
		{
			tptr[x]=yc=(1-alphax)*((in_ptr[x])==k)+alphax*yp;
			yp=yc;
		}
		int w1=hist_rb.x-1;
		yp=((in_ptr[w1])==k);
		tptr[w1]=0.5*(tptr[w1]+yp);
		for(int x=hist_rb.x-2;x>=hist_lt.x;x--)
		{
			yc=(1-alphax)*((in_ptr[x])==k)+alphax*yp;
			tptr[x]=0.5*(tptr[x]+yc);
			yp=yc;
		}
	}

	/*vertical*/
	in_=temp;
	double*ycy,*ypy,*xcy;
	memcpy(optr,temp[0],sizeof(double)*w);
	for(int y=hist_lt.y+1;y<hist_rb.y;y++)
	{
		xcy=in_[y];
		ypy=optr + (y-1)*w;
		ycy=optr + y*w;
		for(int x=hist_lt.x;x<hist_rb.x;x++)
		{
			ycy[x]=(1-alphay)*xcy[x]+alphay*ypy[x];
		}
	}
	int h1=h-1;
	ycy=temp_2w[0];
	ypy=temp_2w[1];
	memcpy(ypy,in_[h1],sizeof(double)*w);
	for(int x=hist_lt.x;x<hist_rb.x;x++) optr[x]=0.5*(optr[x+h1*w]+ypy[x]);
	for(int y=hist_rb.y-2;y>=hist_lt.y;y--)
	{
		xcy=in_[y];//optr=out[y];
		for(int x=hist_lt.x;x<hist_rb.x;x++)
		{
			ycy[x]=(1-alphay)*xcy[x]+alphay*ypy[x];
			optr[x + y*w]=0.5*(optr[x+y*w]+ycy[x]);
		}
		memcpy(ypy,ycy,sizeof(double)*w);
	}
}

void display_histogram(Mat color, Mat colornum, string win_name)
{
	int hist_height = 400; int hist_width = 1500; int font_height = 30; int hist_ref = 40;
	Mat display_hist(hist_height+font_height, hist_width + hist_ref, CV_8UC3, Scalar::all(255));
	char bin_num[255];
	strcpy(bin_num,"");
	char pixel_pos[255];
	strcpy(pixel_pos,"");
	char y_axis[255];
	strcpy(y_axis,"");
	sprintf(y_axis,"F");
	Point yaxis_pos = Point(1510, 200);
	line(display_hist, Point(1501,400),Point(1501,0), CV_RGB(255,128,12), 3);
	line(display_hist, Point(0,401),Point(1500,401), CV_RGB(255,128,12), 3);
	putText(display_hist,y_axis,yaxis_pos, FONT_HERSHEY_SIMPLEX, 1.5, CV_RGB(255,128,12), 2);

	//mouse_pos.x = 318;mouse_pos.y = 338;
	double max_num = 0;
	for(int i = 0;i < hist_bin_size; i++){
		if(colornum.at<int>(i)>max_num)
			max_num = colornum.at<int>(i);
	}
	for(int i = 0;i < hist_bin_size; i++){

		sprintf(bin_num,"%d",i);
		double histval = colornum.at<int>(i)/max_num;
		Point hist_lt = Point(i*hist_width/hist_bin_size, hist_height - histval*hist_height);
		Point hist_rb = Point(i*hist_width/hist_bin_size+hist_width/hist_bin_size, hist_height);

		Point font_lb = Point(i*hist_width/hist_bin_size + hist_width/hist_bin_size/4, hist_height+font_height/3*2);
		Point pixel_pos_lb = Point(10,20);
		
		rectangle(display_hist, hist_lt, hist_rb, CV_RGB(color.at<Vec3f>(i)[0]*255,color.at<Vec3f>(i)[1]*255,color.at<Vec3f>(i)[2]*255), CV_FILLED);

		//rectangle(display_hist, Point(hist_width,hist_height - (i+1)*hist_height/hist_bin_size), Point(hist_width+hist_ref,hist_height-i*hist_height/hist_bin_size), CV_RGB(i*QTZ,i*QTZ+64,i*QTZ+128), CV_FILLED);
		putText(display_hist,bin_num,font_lb, FONT_HERSHEY_SIMPLEX, min(max(QTZ*0.3/4,0.1),0.8), CV_RGB(255,128,12), 1.5);
		//putText(display_hist,pixel_pos,pixel_pos_lb, FONT_HERSHEY_SIMPLEX, 0.7, CV_RGB(255,128,12));
	}

	imwrite("Quantized_Histogram.jpg", display_hist);
	imshow(win_name,display_hist);
}

void display_gray_histogram(Mat input_img, string win_name)
{
	int hist_height = 400; int hist_width = 1500; int font_height = 30; int hist_ref = 40;
	Mat display_hist(hist_height+font_height, hist_width + hist_ref, CV_8UC3, Scalar::all(255));
	char bin_num[255];
	strcpy(bin_num,"");
	char pixel_pos[255];
	strcpy(pixel_pos,"");
	char y_axis[255];
	strcpy(y_axis,"");
	sprintf(y_axis,"F");
	Point yaxis_pos = Point(1510, 200);
	line(display_hist, Point(1501,400),Point(1501,0), CV_RGB(255,128,12), 3);
	line(display_hist, Point(0,401),Point(1500,401), CV_RGB(255,128,12), 3);
	putText(display_hist,y_axis,yaxis_pos, FONT_HERSHEY_SIMPLEX, 1.5, CV_RGB(255,128,12), 2);

	int colornum[64];
	for(int i = 0;i < hist_bin_size; i++){
		colornum[i] = 0;
	}
	Mat qua_img;
	qua_img = input_img/QTZ;
	for(int i = 0;i < input_img.rows; i++)for(int j = 0;j < input_img.cols; j++){
		colornum[qua_img.at<uchar>(i,j)]++;
	}
	//mouse_pos.x = 318;mouse_pos.y = 338;
	double max_num = 0;
	for(int i = 0;i < hist_bin_size; i++){
		if(colornum[i]>max_num)
			max_num = colornum[i];
	}
	for(int i = 0;i < hist_bin_size; i++){

		sprintf(bin_num,"%d",i);
		double histval = colornum[i]/max_num;
		Point hist_lt = Point(i*hist_width/hist_bin_size, hist_height - histval*hist_height);
		Point hist_rb = Point(i*hist_width/hist_bin_size+hist_width/hist_bin_size, hist_height);

		Point font_lb = Point(i*hist_width/hist_bin_size + hist_width/hist_bin_size/4, hist_height+font_height/3*2);
		Point pixel_pos_lb = Point(10,20);

		rectangle(display_hist, hist_lt, hist_rb, CV_RGB(float(i/64.0)*255,float(i/64.0)*255,float(i/64.0)*255), CV_FILLED);

		//rectangle(display_hist, Point(hist_width,hist_height - (i+1)*hist_height/hist_bin_size), Point(hist_width+hist_ref,hist_height-i*hist_height/hist_bin_size), CV_RGB(i*QTZ,i*QTZ+64,i*QTZ+128), CV_FILLED);
		putText(display_hist,bin_num,font_lb, FONT_HERSHEY_SIMPLEX, min(max(QTZ*0.3/4,0.1),0.8), CV_RGB(255,128,12), 1.5);
		//putText(display_hist,pixel_pos,pixel_pos_lb, FONT_HERSHEY_SIMPLEX, 0.7, CV_RGB(255,128,12));
	}

	imwrite("Gray_Histogram.jpg", display_hist);
	imshow(win_name,display_hist);
}


inline void Compute_Color_LSH(double**histogram, Mat in, double**histogram_temp,double**temp3w, double sigma,int h,int w, bool s_mode, Point hist_lt, Point hist_rb, bool visualize, bool update_pallet)
{
	Mat idx1i, binColor3f, colorNums1i, qtz_img;
	qtz_img = in.clone();
	qtz_img.convertTo(qtz_img, CV_32FC3, 1.0/255);
	Quantize(qtz_img,idx1i,binColor3f,colorNums1i, 0.995, update_pallet);
	//display_histogram(binColor3f, colorNums1i, "Histogram");

	if(visualize){
		Vec3f* bincolor = (Vec3f*)(binColor3f.data);
		for (int i =0;i<h;i++)
		{
			float* ptr = qtz_img.ptr<float>(i);
			int* indptr = idx1i.ptr<int>(i);
			for (int j =0;j<w;j++)
			{
				ptr[3*j] = bincolor[indptr[j]][0];
				ptr[3*j+1] = bincolor[indptr[j]][1];
				ptr[3*j+2] = bincolor[indptr[j]][2];
			}
		}
		imshow("Quantized Color Image",qtz_img);
		//imwrite("Quantized_img.jpg", qtz_img*255);
	}

	double t;
	{
		double alphay=exp(-sqrt(2.0)/(sigma*h));
		double alphax=exp(-sqrt(2.0)/(sigma*w));

		t = (double)cvGetTickCount();	
		for(int k = 0; k< hist_bin_size; k++)
		{
			Color_LSH_recursive(histogram, idx1i, histogram_temp, temp3w, alphax,alphay, h, w, k, hist_lt, hist_rb);			
		}

		t = (double)cvGetTickCount() - t;
	}
}

#endif