#include "Sort.h"
#include <opencv2/opencv.hpp>
#if _MSC_VER
#include <io.h>
#include <direct.h>
#else
#include <unistd.h>
#include <sys/stat.h>
#include <sys/types.h>
#endif
#include <time.h>
using namespace cv;

const int color[11][3] = { { 255, 0, 0 }, { 255, 128, 255 }, { 255, 128, 0 }, { 255, 215, 0 }, { 154, 205, 50 }, { 0, 128, 0 }, \
{0, 128, 255}, { 186, 85, 211 }, { 91, 46, 0 }, { 0, 0, 0 }, { 255, 255, 255 } };


Sort::Sort(VPT_Line *m_line, int m_linecount, int m_detectType)
{
	max_age = 10;
	min_hits = 3;
	frame_count = 0;
	detectObjCount = 0;
	mTrafficStatistics = NULL;
	trackcount = 0;
	istraffic = false;	//by zl 流量统计
	linecount = m_linecount;

	//--------- 流量统计 ---------//
	if (linecount != 0)
	{
		linecount = m_linecount;
		detectType = m_detectType;
		trafficArray.resize(linecount);
		for (int i = 0; i < linecount; i++)
		{
			lineArray.push_back(m_line[i]);
			trafficArray[i].traffic = new int[detectType];
			for (int j = 0; j < detectType; j++)
			{
				trafficArray[i].traffic[j] = 0;
			}
		}

		mTrafficStatistics = new TrafficStatistics(LINE_LINE_INTERSECTION);
		mTrafficStatistics->SetLineArray(m_line, m_linecount);
		istraffic = true;;	//by zl 统计流量
	}
}

int Sort::updateLastFrame(vector<int> &deleteTrackers)
{
	for (int trackers_number = 0; trackers_number < trackers.size();)
	{
		if (trackers[trackers_number].id != -1)
		{
			//trackers[trackers_number].endFrame = frame_count-1;  //最后一帧了
			deleteTrackers.push_back(trackers[trackers_number].id);
			trackers_number++;     //此处先不删除轨迹信息，因为之后检测到人脸之后还需要保存轨迹信息，在保存完之后进行删除
		}
		else
			trackers.erase(trackers.begin() + trackers_number);
	}

	return deleteTrackers.size();
}

int Sort::update(int width, int height, bool isUseDet, vector< vector<float> > &dets, CD_ObjInfo *result, vector<int> &deleteTrackers, VPT_TrafficResult* traffic /*= NULL*/, int detectType /*= 0*/)
{
	
	//get predicted locations from existing trackers.
	vector< vector<float> > trks;
	vector<float> pos;
	for (int i = 0; i < trackers.size(); i++)
	{
		pos = trackers[i].predict();
		pos.push_back(1);
		pos.push_back(trackers[i].cls);
		trks.push_back(pos);
		pos.clear();
	}

	int ObjCount = 0;		//by zl 本帧图像中的有效前景个数（真实计数）

	vector<float> bbox;

	if (isUseDet == true)
	{
		vector< vector<int> > matched;
		vector<int> unmatched_dets;
		vector<int> unmatched_trks;
		//dets, x1,y1,x2,y2,score,cls
		//trks, x1,y1,x2,y2,score,cls
		Sort::associate_detections_to_trackers(matched, unmatched_dets, unmatched_trks, dets, trks, 0.3);

		//update matched trackers with assigned detections
		for (int matched_number = 0; matched_number < matched.size(); matched_number++)
		{
			//cout << "检测+跟踪: " << dets[matched[matched_number][0]][6] << " " << trackers[matched[matched_number][1]].id << endl;
			trackers[matched[matched_number][1]].update(dets[matched[matched_number][0]]);
			trackers[matched[matched_number][1]].score = dets[matched[matched_number][0]][4];

			if (trackers[matched[matched_number][1]].counts == 0)
				trackers[matched[matched_number][1]].cls = dets[matched[matched_number][0]][5];
			else if (trackers[matched[matched_number][1]].cls == dets[matched[matched_number][0]][5])
				trackers[matched[matched_number][1]].counts++;
			else
				trackers[matched[matched_number][1]].counts--;

			if (trackers[matched[matched_number][1]].TrackerTimes != 0) trackers[matched[matched_number][1]].TrackerTimes = 0;
		}

		//尝试未匹配上的 继续跟踪两帧
		//for (int unmatched_trks_number = 0; unmatched_trks_number < unmatched_trks.size(); unmatched_trks_number++)
		//{
		//	if (trackers[unmatched_trks[unmatched_trks_number]].TrackerTimes == 4)
		//	{
		//		//cout << "跟踪的够多啦： " << trackers[unmatched_trks[unmatched_trks_number]].id << endl;
		//		continue;
		//	}

		//	//可能是一些错误的轨迹，就先不进行跟踪
		//	//条件一：可能是刚检测到的物体，没有检测到它3帧及以上，还不能算作轨迹
		//	//条件二：整个视频刚开始检测，这时候如果出现了错误轨迹也不要进行继续跟踪了，前三帧的断裂可以忽略，但是前三帧的错误检测继续跟踪的话，轨迹很奇怪
		//	if (trackers[unmatched_trks[unmatched_trks_number]].id == -1 || frame_count <= (min_hits*FusionInterval))
		//		continue;

		//	trackers[unmatched_trks[unmatched_trks_number]].update(trks[unmatched_trks[unmatched_trks_number]]);
		//	trackers[unmatched_trks[unmatched_trks_number]].TrackerTimes++;

		//	//cout << "跟踪: " << trackers[unmatched_trks[unmatched_trks_number]].id << "  " << trackers[unmatched_trks[unmatched_trks_number]].TrackerTimes << endl;
		//}

		//create and initialise new trackers for unmatched detections
		for (int unmatched_dets_number = 0; unmatched_dets_number < unmatched_dets.size(); unmatched_dets_number++)
		{
			//cout << "该检测到的点竟然没有跟踪上： " << dets[unmatched_dets[unmatched_dets_number]][6] << endl;
			KalmanBoxTracker tracker = KalmanBoxTracker(dets[unmatched_dets[unmatched_dets_number]]);
			tracker.id = -1;
			trackcount += 1;
			trackers.push_back(tracker);
			trackers[trackers.size() - 1].score = dets[unmatched_dets[unmatched_dets_number]][4];//by zl 20170525 解决第一次检测时置信度为0问题
		}


		//cout << "匹配情况: ①匹配上的：" << matched.size() << " ②未匹配上的预测点：" << unmatched_trks.size() << "  ③未匹配上的检测点：" << unmatched_dets.size() << endl;

		/*for (int trackers_number = 0; trackers_number < trackers.size(); trackers_number++)
		{
		if ((trackers[trackers_number].time_since_update < FusionInterval) && ((trackers[trackers_number].hit_streak >= min_hits) || (frame_count <= (min_hits*FusionInterval))))
		{
		totalObjCount++;
		}
		}

		if (totalObjCount > MAX_OBJ_COUNT)
		{
		printf("前景太多，已设置为最大前景数！\n");
		totalObjCount = MAX_OBJ_COUNT;
		}

		result = new VPT_ObjInfo[totalObjCount];*/

		for (int trackers_number = 0; trackers_number < trackers.size();)
		{
			if (trackers[trackers_number].time_since_update > max_age)
			{
				if (trackers[trackers_number].id != -1)
				{
					deleteTrackers.push_back(trackers[trackers_number].id);
				}

				trackers.erase(trackers.begin() + trackers_number);
				continue;
			}
			if ((trackers[trackers_number].time_since_update < FusionInterval) && ((trackers[trackers_number].hit_streak >= min_hits) || (frame_count <= (min_hits*FusionInterval))))
			{
				if (trackers[trackers_number].id == -1)
				{
					trackers[trackers_number].id = detectObjCount++;
					trackers[trackers_number].counts = 0;
			}

				//if (ObjCount >= totalObjCount) continue;   //前景数太多，已经超过了最大设置的前景数，不再返回结果

				bbox = trackers[trackers_number].get_state();

				/*if (trackers[trackers_number].position.size() > 1)
				{
					float cur_alpha = alpha[trackers[trackers_number].cls - 1];
					int index_end = trackers[trackers_number].position.size() - 1;
					int index = 0;

					bbox[index] = cur_alpha*bbox[index] + (1 - cur_alpha)*trackers[trackers_number].position[index_end][index];

					index = 1;
					bbox[index] = cur_alpha*bbox[index] + (1 - cur_alpha)*trackers[trackers_number].position[index_end][index];

					index = 2;
					bbox[index] = cur_alpha*bbox[index] + (1 - cur_alpha)*trackers[trackers_number].position[index_end][index];

					index = 3;
					bbox[index] = cur_alpha*bbox[index] + (1 - cur_alpha)*trackers[trackers_number].position[index_end][index];
				}
				trackers[trackers_number].position.push_back(bbox);*/


				result[ObjCount].id = trackers[trackers_number].id;
				result[ObjCount].left = bbox[0];	// bbout[i][0];
				result[ObjCount].top = bbox[1];	//bbout[i][1];
				result[ObjCount].right = bbox[2];	//bbout[i][2];
				result[ObjCount].bottom = bbox[3];	//bbout[i][3];
				result[ObjCount].confidence = trackers[trackers_number].score; // bbout[i][4];
				result[ObjCount].index = trackers[trackers_number].cls - 1; // bbout[i][5] - 1;
				RectboundCheck(width, height, &result[ObjCount]);
				result[ObjCount].center_x = result[ObjCount].left + (result[ObjCount].right - result[ObjCount].left) * 0.5;	// 中心点	add by 20170227
				result[ObjCount].center_y = result[ObjCount].top + (result[ObjCount].bottom - result[ObjCount].top) * 0.5;	// 中心点

				trackers[trackers_number].updateTrackLine(result[ObjCount].center_x, result[ObjCount].center_y);
#if _Debug
				printf("trackers_number = %d, trackers.size() = %d, update: index = %d,  id = %d, (%d, %d), (%d, %d)\n", trackers_number, trackers.size(), result[ObjCount].index, result[ObjCount].id, result[ObjCount].left, result[ObjCount].top, result[ObjCount].right, result[ObjCount].bottom);
#endif
				ObjCount++;


			}
			trackers_number++;//共多少条轨迹
		}
	}
	else
	{
		for (int trackers_number = 0; trackers_number < trackers.size(); trackers_number++)
		{
			if (trackers[trackers_number].id == -1)
				trackers[trackers_number].id = detectObjCount++;
			bbox = trackers[trackers_number].get_state();
			result[trackers_number].id = trackers[trackers_number].id;
			result[trackers_number].left = bbox[0];	// bbout[i][0];
			result[trackers_number].top = bbox[1];	//bbout[i][1];
			result[trackers_number].right = bbox[2];	//bbout[i][2];
			result[trackers_number].bottom = bbox[3];	//bbout[i][3];
			result[trackers_number].confidence = trackers[trackers_number].score; // bbout[i][4];
			result[trackers_number].index = trackers[trackers_number].cls - 1; // bbout[i][5] - 1;
			RectboundCheck(width, height, &result[trackers_number]);
			result[trackers_number].center_x = (int)(result[trackers_number].left + (result[trackers_number].right - result[trackers_number].left) * 0.5);	// 中心点	add by 20170227
			result[trackers_number].center_y = (int)(result[trackers_number].top + (result[trackers_number].bottom - result[trackers_number].top) * 0.5);	// 中心点
			ObjCount++;
		}
	}

	//cout << "确定是结果添加的点：" << ObjCount << endl;

	//---------------------------注释掉了这步操作 用了新的绘制轨迹的函数 需要绘制调用addTracker(Mat *img)方法 by lm---------------------------------------------/
	//addTracker(result, ObjCount);
	/*addTracker(result, ObjCount, NULL);*/


	if (traffic != NULL && istraffic == true)	//若需要返回交通流量 只要初始化时初始化了拌线 就会一直在统计，但只有在traffice不为NULL时才返回流量结果。其余状况下，统计但不返回结果
	{
		mTrafficStatistics->GetTrafficArray(result, ObjCount, trackers, trafficArray);
		for (int i = 0; i < trafficArray.size(); i++)
		{
			for (int j = 0; j < detectType; j++)
			{
				traffic[i].traffic[j] = trafficArray[i].traffic[j];
#if _Debug
				cout << i << ": " << trafficArray[i].traffic << endl;
#endif

			}
		}
	}

	frame_count += 1;	//帧数加一
	return ObjCount;
	
}

//---------------------------????trackers?械?history ????路?? -by lm ---------------------------------------------/
int Sort::addTracker(Mat *img)
{
	map<int, pair<int, int>> tracker;
	vector<float> bbox;
	
	int x_1 = 0, y_1 = 0, x_2 = 0, y_2 = 0;

	for (auto iter : trackers)
	{
		for (int i = 0; i < iter.trackLine.size(); i++)
		{
			if (i == 0)
			{
				x_1 = iter.trackLine[i].x;
				y_1 = iter.trackLine[i].y;
			}
			else
			{
				x_2 = iter.trackLine[i].x;
				y_2 = iter.trackLine[i].y;
				int colorIndex = iter.id % 11;
				line(*img, cvPoint(x_1, y_1), cvPoint(x_2, y_2), cvScalar(color[colorIndex][0], color[colorIndex][1], color[colorIndex][2]), 3);
				x_1 = x_2;
				y_1 = y_2;
			}
		}
	}
/*
	for (auto iter : trackers)
	{
		for (int i = 0; i < iter.history.size(); i++)
		{		
			if (i == 0)
			{
				x_1 = iter.history[i][0] + (iter.history[i][2] - iter.history[i][0]) / 2;
				y_1 = iter.history[i][1] + (iter.history[i][3] - iter.history[i][1]) / 2;
			}
			else
			{
				x_2 = iter.history[i][0] + (iter.history[i][2] - iter.history[i][0]) / 2;
				y_2 = iter.history[i][1] + (iter.history[i][3] - iter.history[i][1]) / 2;
				int colorIndex = iter.id % 11;
				line(*img, cvPoint(x_1, y_1), cvPoint(x_2, y_2), cvScalar(color[colorIndex][0], color[colorIndex][1], color[colorIndex][2]), 3);
				x_1 = x_2;
				y_1 = y_2;
			}
		}
	}*/

	return 1;
}

void Sort::Release()
{
	//tracker.clear();
	//vector <mylist>().swap(tracker);
	trackers.clear();
	vector<KalmanBoxTracker>().swap(trackers);

	for (vector<VPT_TrafficResult>::iterator it = trafficArray.begin(); it != trafficArray.end(); it++)
		if (NULL != it->traffic)
		{
			delete it->traffic;
			it->traffic = NULL;
		}

	vector<VPT_TrafficResult>().swap(trafficArray);
	
}
bool Sort::GetState()
{
	return WORK;
}

void Sort::Pause()
{
	WORK = false;
}

void Sort::ReSet()
{
	WORK = true;
	//Release();
	//max_age = 1;
	//min_hits = 3;
	//frame_count = 0;

	////-----------------by zl---------------------//
	//istraffic = false;	//by zl ??统?平?通量
	//trackcount = 0;
	////-----------------by zl---------------------//
}

void RectboundCheck(int Width, int Height, CD_ObjInfo * result)	//??止????越?? by zl
{
	if (result->left < 0)
		result->left = 0;
	if (result->left >= Width-1)
		result->left = Width-2;

	if (result->top < 0)
		result->top = 0;
	if (result->top >= Height-1)
		result->top = Height-2;

	if (result->right <= result->left)
		result->right = result->left + 1;
	if (result->right >= Width)
		result->right = Width-1;

	if (result->bottom < result->top)
		result->bottom = result->top + 1;
	if (result->bottom >= Height)
		result->bottom = Height-1;


}
//------------------------------------????????----------------------------------------//
void Sort::associate_detections_to_trackers(vector< vector<int> > &matched, vector<int> &unmatched_dets, vector<int> &unmatched_trks, vector< vector<float> > &dets, vector< vector<float> > &trks, float iou_threshold)
{
	if (0 == trks.size())
	{
		for (int x = 0; x < dets.size(); x++)
		{
			unmatched_dets.push_back(x);
		}
	}
	else if (0 == dets.size())
	{
		for (int x = 0; x < trks.size(); x++)
		{
			unmatched_trks.push_back(x);
		}
	}
	else
	{
		Mat IoUMat(dets.size(), trks.size(), CV_32FC1);
		for (int i = 0; i < dets.size(); i++)
		for (int j = 0; j < trks.size(); j++)
		{
			//cls????
			if ( 1/*dets[i][5] == trks[j][5]*/)
			{
				IoUMat.at<float>(i, j) = IoU(dets[i], trks[j]);
			}
			else
			{
				IoUMat.at<float>(i, j) = 0;
			}

		}

		//???????惴?
		vector<int> assignment;
		munkres(IoUMat, assignment);

		vector<int>::iterator iter;
		for (int trackers_indices = 0; trackers_indices < trks.size(); trackers_indices++)
		{
			iter = find(assignment.begin(), assignment.end(), trackers_indices);
			if (iter == assignment.end())
			{
				//assignment?胁?????trackers_indices值
				unmatched_trks.push_back(trackers_indices);
			}
		}

		vector<int> matched_row_col;
		for (int detections_indices = 0; detections_indices < assignment.size(); detections_indices++)
		{
			if (assignment[detections_indices] == -1)
			{
				unmatched_dets.push_back(detections_indices);
			}
			else if (IoUMat.at<float>(detections_indices, assignment[detections_indices]) > iou_threshold)
			{
				matched_row_col.push_back(detections_indices);
				matched_row_col.push_back(assignment[detections_indices]);
				matched.push_back(matched_row_col);
				matched_row_col.clear();
			}
			else
			{
				unmatched_dets.push_back(detections_indices);
				unmatched_trks.push_back(assignment[detections_indices]);
			}
		}
	}
}
//?卸?两?????欠??嘟?


///------------alg 2------------