#include "KalmanBoxTracker.h"


float IoU(vector<float> &bb_test, vector<float> &bb_gt)
{
	float xx1, yy1, xx2, yy2, w, h, wh, o;
	xx1 = max(bb_test[0], bb_gt[0]);
	yy1 = max(bb_test[1], bb_gt[1]);
	xx2 = min(bb_test[2], bb_gt[2]);
	yy2 = min(bb_test[3], bb_gt[3]);
	w = max(float(0), (xx2 - xx1));
	h = max(float(0), (yy2 - yy1));
	wh = w * h;
	o = wh / ((bb_test[2] - bb_test[0])*(bb_test[3] - bb_test[1])
		+ (bb_gt[2] - bb_gt[0])*(bb_gt[3] - bb_gt[1]) - wh);
	return o;
}

void convert_bbox_to_z(vector<float> &bbox, vector<float> &z)
{
	float w, h, x, y, s, r;
	w = bbox[2] - bbox[0];
	h = bbox[3] - bbox[1];
	x = bbox[0] + w / 2;
	y = bbox[1] + h / 2;
	s = w * h;   //scale is just area
	r = w / h;
	z.push_back(x);
	z.push_back(y);
	z.push_back(s);
	z.push_back(r);

}

void convert_x_to_bbox(Mat &x, vector<float> &bbox)
{
	float w, h;
	w = sqrt(x.at<float>(2) * x.at<float>(3));
	h = x.at<float>(2) / w;
	//bbox[x1,y1,x2,y2];
	bbox.push_back(x.at<float>(0) - w / 2);
	bbox.push_back(x.at<float>(1) - h / 2);
	bbox.push_back(x.at<float>(0) + w / 2);
	bbox.push_back(x.at<float>(1) + h / 2);
}



KalmanBoxTracker::KalmanBoxTracker(vector<float> &bbox)
{
	KF.init(7, 4, 0);                                    //初始化卡尔曼滤波器对象KF
	state = Mat::zeros(7, 1, CV_32F);
	processNoise = Mat::zeros(7, 1, CV_32F);
	measurement = Mat::zeros(4, 1, CV_32F);                 //定义测量值

	KF.transitionMatrix = (Mat_<float>(7, 7) << \
		1, 0, 0, 0, 1, 0, 0, \
		0, 1, 0, 0, 0, 1, 0, \
		0, 0, 1, 0, 0, 0, 1, \
		0, 0, 0, 1, 0, 0, 0, \
		0, 0, 0, 0, 1, 0, 0, \
		0, 0, 0, 0, 0, 1, 0, \
		0, 0, 0, 0, 0, 0, 1);  //状态转移矩阵A

	KF.measurementMatrix = (Mat_<float>(4, 7) << \
		1, 0, 0, 0, 0, 0, 0, \
		0, 1, 0, 0, 0, 0, 0, \
		0, 0, 1, 0, 0, 0, 0, \
		0, 0, 0, 1, 0, 0, 0);  //测量矩阵H

	KF.measurementNoiseCov = (Mat_<float>(4, 4) << \
		1, 0, 0, 0, \
		0, 1, 0, 0, \
		0, 0, 10, 0, \
		0, 0, 0, 10);  //测量噪声方差矩阵R

	KF.errorCovPost = (Mat_<float>(7, 7) << \
		10, 0, 0, 0, 0, 0, 0, \
		0, 10, 0, 0, 0, 0, 0, \
		0, 0, 10, 0, 0, 0, 0, \
		0, 0, 0, 10, 0, 0, 0, \
		0, 0, 0, 0, 10000, 0, 0, \
		0, 0, 0, 0, 0, 10000, 0, \
		0, 0, 0, 0, 0, 0, 10000);   //后验错误估计协方差矩阵P

	KF.processNoiseCov = (Mat_<float>(7, 7) << \
		1, 0, 0, 0, 0, 0, 0, \
		0, 1, 0, 0, 0, 0, 0, \
		0, 0, 1, 0, 0, 0, 0, \
		0, 0, 0, 1, 0, 0, 0, \
		0, 0, 0, 0, 0.01, 0, 0, \
		0, 0, 0, 0, 0, 0.01, 0, \
		0, 0, 0, 0, 0, 0, 0.0001);   //系统噪声方差矩阵Q
	
	vector<float> z;
	convert_bbox_to_z(bbox, z);
	KF.statePost = (Mat_<float>(7, 1) << z[0], z[1], z[2], z[3], 0, 0, 0);  //corrected state
	state = (Mat_<float>(7, 1) << z[0], z[1], z[2], z[3], 0, 0, 0);
	time_since_update = 0;
	cls = bbox[5];
	counts = 1;
	//history
	//hits = 0;
	hit_streak = 0;
	age = 0;
	istraffic = false;
}

void KalmanBoxTracker::update(vector<float> &bbox)
{
	time_since_update = 0;
	//history
	//hits += 1;
	hit_streak += 1;
	vector<float> z;
	convert_bbox_to_z(bbox, z);
	measurement = (Mat_<float>(4, 1) << z[0], z[1], z[2], z[3]);
	KF.correct(measurement);
	
}

void KalmanBoxTracker::updateTrackLine(int center_x, int center_y)
{
	trackLine.push_back(Point(center_x, center_y));
}

vector<float> KalmanBoxTracker::predict()
{
	if ((KF.statePost.at<float>(6) + KF.statePost.at<float>(2)) <= 0)
		KF.statePost.at<float>(6) *= 0.0;

	KF.predict();

	age += 1;
	if (time_since_update >= FusionInterval)
		hit_streak = 0;
	time_since_update += 1;
	vector<float> bbox;
	convert_x_to_bbox(KF.statePost, bbox);
	//history.push_back(bbox);

	return bbox;
}

vector<float> KalmanBoxTracker::get_state()
{
	vector<float> bbox;
	convert_x_to_bbox(KF.statePost, bbox);
	return bbox;
}