#include "Sort.h"
#include <opencv2/opencv.hpp>
//#include "ImageSaveGPU.h"

#ifdef _MSC_VER
#include <io.h>
#include <direct.h>
#define _ACCESS _access
#define _MKDIR(a) _mkdir((a))
#else
#include <unistd.h>
#include <stdarg.h>
#include <sys/stat.h>
#define _ACCESS access
#define _MKDIR(a) mkdir((a),0755)
#endif
#include <time.h>

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()
{
    max_age = 10;
    min_hits = 3;
    frame_count = 0;
    WORK = true;
    //-----------------by zl---------------------//
    istraffic = false;	//by zl 不统计交通量
    trackcount = 0;
    //-----------------by zl---------------------//

	max_track_length = MAX_LENGTH;
}


int Sort::update(int width, int height, bool isUseDet, vector< vector<float> > &dets, VPT_ObjInfo *result, vector<int> &deleteObjectID)
{
	//cout << "track size: " << trackers.size() << endl;
    //get predicted locations from existing trackers.
    vector< vector<float> > trks;
    vector<float> pos;
	int ObjCount = 0;		//by zl 本帧图像中的有效前景个数
	vector<float> bbox;

    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();
    }

    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);
        //std::cout<<__FILE__<<" matched:"<<matched.size()<<"\n";
        //std::cout<<__FILE__<<" unmatched:"<<unmatched_dets.size()<<"\n";
        //update matched trackers with assigned detections
        for (int matched_number = 0; matched_number < matched.size(); matched_number++)
        {
            trackers[matched[matched_number][1]].update(dets[matched[matched_number][0]]);
            trackers[matched[matched_number][1]].score = dets[matched[matched_number][0]][4];
            trackers[matched[matched_number][1]].cls = dets[matched[matched_number][0]][5];
            //cout << dets[matched[matched_number][0]][4] << endl;
            //trackers[matched[matched_number][1]].update(trks[matched[matched_number][1]]);
        }


        //create and initialise new trackers for unmatched detections
        for (int unmatched_dets_number = 0; unmatched_dets_number < unmatched_dets.size(); unmatched_dets_number++)
        {
            KalmanBoxTracker tracker = KalmanBoxTracker(dets[unmatched_dets[unmatched_dets_number]], max_track_length);
            tracker.id = -1;
            /*tracker.id = trackcount;
              trackcount += 1;*/
            trackers.push_back(tracker);
            trackers[trackers.size() - 1].score = dets[unmatched_dets[unmatched_dets_number]][4];//by zl 20170525 解决第一次检测时置信度为0问题
           // cout << "trackers size: " << trackers.size() << endl;
        }

        for (int trackers_number = 0; trackers_number < trackers.size();)
        {
            //cout << trackers[trackers_number].id  << " " <<trackers[trackers_number].time_since_update << " " << trackers[trackers_number].hit_streak << " " << min_hits << " " << frame_count << endl;
            if (trackers[trackers_number].time_since_update > max_age)
            {
                if (trackers[trackers_number].id != -1)
                {
                    //cout << "----------------------------- delete id: " << trackers[trackers_number].id << endl;

                    deleteObjectID.push_back(trackers[trackers_number].id);
                }
				trackcount++;
                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 = trackcount++;
                result[ObjCount].id = trackers[trackers_number].id;
                bbox = trackers[trackers_number].get_state();
                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;	// 中心点
                if(trackers[trackers_number].age==2*FusionInterval)
                {
                    result[ObjCount].snap_flag = 1;	// 中心点
                }
                else
                {

                    result[ObjCount].snap_flag = 0;	// 中心点
                }

#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 = trackcount++;
            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 = bbox[5] - 1;// 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);	// 中心点


            result[ObjCount].snap_flag = 0;	// 中心点

            ObjCount++;
        }
    }

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

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

}

//
////---------------------------by zl ---------------------------------------------/
//int Sort::addTracker(VPT_ObjInfo *result, int resultcount)
//{
//	for (int i = 0; i < resultcount; i++)
//	{
//		bool flag = false;
//		for (int j = 0; j < tracker.size(); j++)
//		{
//			if (result[i].id == tracker[j].id)		//若有匹配的 则更新
//			{
//				tracker[j].listinfo.push_back(result[i]);
//				tracker[j].lost = 0;
//				tracker[j].isupdate = true;
//				tracker[j].num++;	//每个ID的计数
//				result[i].num = tracker[j].num;
//			
//				if (tracker[j].istraffic == false && istraffic)
//				{
//					int listmax = tracker[j].listinfo.size() - 1;
//				}
//				flag = true;
//				break;
//			}
//		}
//		if (!flag)		//没有找到匹配项 新添加
//		{
//			mylist m_list;
//			m_list.listinfo.push_back(result[i]);
//			m_list.isupdate = true;
//			m_list.lost = 0;
//			m_list.id = result[i].id;
//			m_list.index = result[i].index;;
//			m_list.istraffic = false;
//			m_list.num = 0;				//20170306
//			result[i].num = 0;
//			m_list.startframe = frame_count;	
//#if _Debug
//			printf("addTracker_pushback :index = %d,  id = %d, (%d, %d), (%d, %d)\n", m_list.index, m_list.id, result[i].left, result[i].top, result[i].right, result[i].bottom);
//#endif
//			tracker.push_back(m_list);
//		}
//	}
//	//绘制轨迹部分
//
//	for (vector <mylist>::iterator iter = tracker.begin(); iter != tracker.end();)
//	{
//		if (iter->isupdate == false)			//未更新的不绘制轨迹	
//		{
//			iter->lost++;
//			if (iter->lost > LOSTMAXFRAMECCOUNT)		//若丢失太多 则删除该轨迹
//			{
//				iter->endframe = frame_count;	//结束帧
//				iter = tracker.erase(iter);//删除
//				continue;
//			}
//		}
//		else				//只对更新后的绘制轨迹
//		{
//			;
//		}
//		iter->isupdate = false;
//		iter++;
//	}
//
//	return 1;
//}


//---------------------------利用trackers中的history 绘制路径 -by lm ---------------------------------------------/
//固定长度的轨迹，采用循环队列，仅保存目前最前N length的轨迹，避免对于停留在画面中目标 导致的内存一直增长
int Sort::addTracker(cv::Mat *img)
{
	map<int, pair<int, int>> tracker;
	vector<float> bbox;


	int x_1, y_1, x_2, y_2;
	for (auto iter : trackers)
	{
		if (iter.time_since_update < FusionInterval)
		{
			int index = iter.history.getFront();
			int trackerSize = iter.history.size();

			for (int i = 0; i < trackerSize; i++)
			{
				if (i == 0)
				{
					x_1 = iter.history.get(index).x;
					y_1 = iter.history.get(index).y;
				}
				else
				{
					x_2 = iter.history.get(index).x;
					y_2 = iter.history.get(index).y;
					int colorIndex = iter.id % 11;
					cv::line(*img, cvPoint(x_1, y_1), cvPoint(x_2, y_2), cvScalar(color[colorIndex][0], color[colorIndex][1], color[colorIndex][2]), 1);

					//drawLineOnGPU()

					x_1 = x_2;
					y_1 = y_2;
				}

				//if (i == 0)
				//{
				//	x_1 = iter.history.get(index)[0] + (iter.history.get(index)[2] - iter.history.get(index)[0]) / 2;
				//	y_1 = iter.history.get(index)[1] + (iter.history.get(index)[3] - iter.history.get(index)[1]) / 2;
				//}
				//else
				//{
				//	x_2 = iter.history.get(index)[0] + (iter.history.get(index)[2] - iter.history.get(index)[0]) / 2;
				//	y_2 = iter.history.get(index)[1] + (iter.history.get(index)[3] - iter.history.get(index)[1]) / 2;
				//	int colorIndex = iter.id % 11;
				//	cv::line(*img, cvPoint(x_1, y_1), cvPoint(x_2, y_2), cvScalar(color[colorIndex][0], color[colorIndex][1], color[colorIndex][2]), 1);

				//	//drawLineOnGPU()

				//	x_1 = x_2;
				//	y_1 = y_2;
				//}

				index = (index + 1) % trackerSize;
			}

			//int index = iter.history->getFront();
			//int trackerSize = iter.history->size();

			//for (int i = 0; i < trackerSize; i++)
			//{
			//	if (i == 0)
			//	{
			//		x_1 = iter.history->get(index)[0] + (iter.history->get(index)[2] - iter.history->get(index)[0]) / 2;
			//		y_1 = iter.history->get(index)[1] + (iter.history->get(index)[3] - iter.history->get(index)[1]) / 2;
			//	}
			//	else
			//	{
			//		x_2 = iter.history->get(index)[0] + (iter.history->get(index)[2] - iter.history->get(index)[0]) / 2;
			//		y_2 = iter.history->get(index)[1] + (iter.history->get(index)[3] - iter.history->get(index)[1]) / 2;
			//		int colorIndex = iter.id % 11;
			//		cv::line(*img, cvPoint(x_1, y_1), cvPoint(x_2, y_2), cvScalar(color[colorIndex][0], color[colorIndex][1], color[colorIndex][2]), 1);

			//		//drawLineOnGPU()

			//		x_1 = x_2;
			//		y_1 = y_2;
			//	}

			//	index = (index + 1) % trackerSize;
			//}
		}

	}

	return 1;
}

//不固定长度的轨迹版本 采用vector，轨迹一直保留，对于停留在画面中的物体，会有内存一直增长的隐患
//int Sort::addTracker(cv::Mat *img)
//{
//    map<int, pair<int, int>> tracker;
//    vector<float> bbox;
//
//
//    int x_1, y_1, x_2, y_2;
//    for (auto iter : trackers)
//    {
//        if (iter.time_since_update < FusionInterval)
//        {
//            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;
//                    cv::line(*img, cvPoint(x_1, y_1), cvPoint(x_2, y_2), cvScalar(color[colorIndex][0], color[colorIndex][1], color[colorIndex][2]), 1);
//
//                    //drawLineOnGPU()
//
//                    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);
}
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---------------------//
}

//---------------------------by zl ---------------------------------------------/
bool line_rect_intersection(cv::Point start_p, cv::Point end_p, int left, int top, int right, int bottom)
{
    int a = start_p.y - end_p.y;
    int b = end_p.x - start_p.x;
    int c = start_p.x* end_p.y - end_p.x* start_p.y;

    ////思路:先看线段所在直线是否与矩形相交，如果不相交则必为 “Ｆ”，
    ////如果相交，则看线段的两个点是否在矩形的同一边（即两点的 x（y)  坐标都比矩形的小 x(y) 坐标小，或者大）,
    ////若在同一边则为“Ｆ”，否则就是相交的情况。
    if ((a* left + b*top + c >= 0 && a* right + b* bottom + c <= 0) ||
            (a* left + b*top + c <= 0 && a* right + b* bottom + c >= 0) ||
            (a* left + b*bottom + c >= 0 && a* right + b* top + c <= 0) ||
            (a* left + b*bottom + c >= 0 && a* right + b* top + c <= 0))
    {
        if (left > right)
        {
            swap(left, right);
        }
        if (top < bottom)
        {
            swap(top, bottom);
        }
        if ((start_p.x < left && end_p.x < left) ||
                (start_p.x > right && end_p.x < left) ||
                (start_p.y > top && end_p.y > top) ||
                (start_p.y < bottom && end_p.y < bottom))  ///判断线段是否在矩形一侧
        {
            return false;
        }
        else
        {
            return true;
        }
    }
    else
    {
        return false;
    }
}
void RectboundCheck(int Width, int Height, VPT_ObjInfo * result)	//防止坐标越界 by zl
{
    if (result->left < 0)
        result->left = 0;
    if (result->left >= Width)
        result->left = Width;

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

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

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


}
//------------------------------------其他函数----------------------------------------//
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
    {
        cv::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)
                    //if (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------------  
//叉积  
double mult(cv::Point a, cv::Point b, cv::Point c)
{
    return (a.x - c.x)*(b.y - c.y) - (b.x - c.x)*(a.y - c.y);
}

//aa, bb为一条线段两端点 cc, dd为另一条线段的两端点 相交返回true, 不相交返回false  
bool intersect(cv::Point aa, cv::Point bb, cv::Point cc, cv::Point dd)
{
    if (max(aa.x, bb.x)<min(cc.x, dd.x))
    {
        return false;
    }
    if (max(aa.y, bb.y)<min(cc.y, dd.y))
    {
        return false;
    }
    if (max(cc.x, dd.x)<min(aa.x, bb.x))
    {
        return false;
    }
    if (max(cc.y, dd.y)<min(aa.y, bb.y))
    {
        return false;
    }
    if (mult(cc, bb, aa)*mult(bb, dd, aa)<0)
    {
        return false;
    }
    if (mult(aa, dd, cc)*mult(dd, bb, cc)<0)
    {
        return false;
    }
    return true;
}
///------------alg 2------------