#include "KalmanBoxTracker.h"
#include <cmath>

#include "../../common/logger.hpp"

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

// added by zsh 220718
float center_distance(vector<float> &bb_test, vector<float> &bb_gt, float maxLength)
{
	float w1 = bb_gt[2] - bb_gt[0];
	float h1 = bb_gt[3] - bb_gt[1];
    float bbox_cx = (bb_gt[0] + bb_gt[2]) / 2;
    float bbox_cy = (bb_gt[1] + bb_gt[3]) / 2;

    float w2 = bb_test[2] - bb_test[0];
	float h2 = bb_test[3] - bb_test[1];
	float cx = (bb_test[0] + bb_test[2]) / 2;
    float cy = (bb_test[1] + bb_test[3]) / 2;

	float scaleW = 100;
    float scaleH = 100;
    if (w2 != 0 && h2 != 0) {
        scaleW = w1 / w2;
        if (scaleW < 1) scaleW = 1. / scaleW;
        scaleH = h1 / h2;
        if (scaleH < 1) scaleH = 1. / scaleH;
    }
	
	float res = std::sqrt((cx - bbox_cx)*(cx - bbox_cx)+(cy - bbox_cy)*(cy - bbox_cy)) / maxLength *
                scaleW * scaleH;				
    //std::cout << res << std::endl;
    return res;
}

void convert_bbox_to_z(vector<float> &bbox, vector<float> &z)
{
	if(bbox.size() <= 0){
		LOG_ERROR("convert_bbox_to_z - bbox.size() 小于0");
		return;
	}
	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;
	if(fabs(h) < 1e-6){
		LOG_ERROR("convert_bbox_to_z - h小于0");
		return;
	}
	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(cv::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, int trackLength)
{
	KF.init(7, 4, 0);                                    //初始化卡尔曼滤波器对象KF
	state = cv::Mat::zeros(7, 1, CV_32F);
	processNoise = cv::Mat::zeros(7, 1, CV_32F);
	measurement = cv::Mat::zeros(4, 1, CV_32F);                 //定义测量值

	KF.transitionMatrix = (cv::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 = (cv::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 = (cv::Mat_<float>(4, 4) << \
		1, 0, 0, 0, \
		0, 1, 0, 0, \
		0, 0, 10, 0, \
		0, 0, 0, 10);  //测量噪声方差矩阵R

	KF.errorCovPost = (cv::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 = (cv::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 = (cv::Mat_<float>(7, 1) << z[0], z[1], z[2], z[3], 0, 0, 0);  //corrected state
	state = (cv::Mat_<float>(7, 1) << z[0], z[1], z[2], z[3], 0, 0, 0);
	time_since_update = 0;
	cls = bbox[5];
	//history
	//hits = 0;
	hit_streak = 0;
	age = 0;

	m_trackLength = trackLength;
	//history = new cycleQueue<vector<float>>(trackLength);
	history.set_param(trackLength);
}

KalmanBoxTracker::~KalmanBoxTracker()
{

	/*int trackerSize = history.size();

	for (int i = 0; i < trackerSize; i++)
	{
		history.get(i).clear();
		vector<float>().swap(history.get(i));

	}*/

	/*if (history != NULL)
	{
		delete history;
		history = NULL;
	}*/
}

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 = (cv::Mat_<float>(4, 1) << z[0], z[1], z[2], z[3]);
	KF.correct(measurement);
	frame_count++;
}

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

	TRACK_POINT tmp_point;
	tmp_point.x = bbox[0] + (bbox[2] - bbox[0]) / 2;
	tmp_point.y = bbox[1] + (bbox[3] - bbox[1]) / 2;
	history.push(tmp_point);

	return bbox;
}

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