#include"TrackingTarget.h"


KalmanTracking::KalmanTracking(Target& Object_First_positionint, int dynamParams = 8, int measureParams = 8, int controlParams = 0) :
	KF(dynamParams, measureParams, controlParams), LossObject_index(0), id(Object_First_positionint.label)
{
	A = (Mat_<float>(8, 8) << 1, 0, 0, 0, 1, 0, 0, 0,
		0, 1, 0, 0, 0, 1, 0, 0,
		0, 0, 1, 0, 0, 0, 1, 0,
		0, 0, 0, 1, 0, 0, 0, 1,
		0, 0, 0, 0, 1, 0, 0, 0,
		0, 0, 0, 0, 0, 1, 0, 0,
		0, 0, 0, 0, 0, 0, 1, 0,
		0, 0, 0, 0, 0, 0, 0, 1);
	A_ = (Mat_<float>(8, 8) << 1, 0, 0, 0, 1, 0, 0, 0,
		0, 1, 0, 0, 0, 1, 0, 0,
		0, 0, 1, 0, 0, 0, 0, 0,
		0, 0, 0, 1, 0, 0, 0, 0,
		0, 0, 0, 0, 1, 0, 0, 0,
		0, 0, 0, 0, 0, 1, 0, 0,
		0, 0, 0, 0, 0, 0, 1, 0,
		0, 0, 0, 0, 0, 0, 0, 1);
	Z = Mat(measureParams, 1, CV_32F);

	cv::setIdentity(KF.measurementMatrix);							// H
	cv::setIdentity(KF.processNoiseCov, cv::Scalar::all(0.1));		// Q
	cv::setIdentity(KF.measurementNoiseCov, cv::Scalar::all(10)); // R
	cv::setIdentity(KF.errorCovPost, cv::Scalar::all(1));			// P post

	cv::Rect2f rect2f = cv::Rect2f(Object_First_positionint.box);
	KF.statePost = (Mat_<float>(measureParams, 1) << rect2f.x, rect2f.y, rect2f.width, rect2f.height, 0, 0, 0, 0);
}

KalmanTracking::~KalmanTracking() {}

void KalmanTracking::Track_Iteration(std::vector<Target>& all_Objects,std::pair<Target,bool>& real_target)
{
	float max_iou = 0.0f;
	Target best_target;
	bool target_found = false;
	cv::Rect predicted_rect(
		static_cast<int>(KF.statePost.at<float>(0) + 0.5), static_cast<int>(KF.statePost.at<float>(1) + 0.5),
		static_cast<int>(KF.statePost.at<float>(2) + 0.5), static_cast<int>(KF.statePost.at<float>(3) + 0.5));

	for (const Target& target : all_Objects) {
		float iou = calculateIOU(predicted_rect, target.box);
		if (iou > max_iou) {
			best_target = target;
			max_iou = iou;
			target_found = true;
		}
	}

	if (target_found) {
		real_target.first = best_target;
		real_target.second = true;

		float dx = best_target.box.x - KF.statePost.at<float>(0);
		float dy = best_target.box.y - KF.statePost.at<float>(1);
		float dw = best_target.box.width - KF.statePost.at<float>(2);
		float dh = best_target.box.height - KF.statePost.at<float>(3);
		Z = (Mat_<float>(8, 1) << (float)best_target.box.x, (float)best_target.box.y, (float)best_target.box.width, (float)best_target.box.height, dx, dy, dw, dh);

		KF.transitionMatrix = A;
		KF.statePre=KF.predict();
		KF.statePost=KF.correct(Z);

		LossObject_index = 0;
	}
	else {
		real_target.first = Target();
		real_target.second = false;

		KF.transitionMatrix = A_;
		KF.statePost = KF.predict();
		LossObject_index++;
	}
}

Target_Tracker::Target_Tracker(Target& target_first_position) :
	tracker(std::make_unique<KalmanTracking>(target_first_position)),tracked_normal(true){}

void Target_Tracker::is_tracked_normal() {
	if (tracker->LossObject_index > LOSSOBJETCT_NUM) {
			tracked_normal = false;		
	}
}
void Target_Tracker::SingleTraget_Tarcking(std::vector<Target>& all_targets, std::pair<Target,bool>& real_target, bool& is_tarck_normal)
{
	is_tracked_normal();
	if (tracked_normal) {
		tracker->Track_Iteration(all_targets,real_target);
		is_tarck_normal = true;
	}
	else {
		is_tarck_normal = false;
	}
}

cv::Rect Target_Tracker::get_PredictTarget_box()
{
	cv::Rect2f rect2f(
		static_cast<int>(tracker->KF.statePost.at<float>(0) + 0.5), static_cast<int>(tracker->KF.statePost.at<float>(1) + 0.5),
		static_cast<int>(tracker->KF.statePost.at<float>(2) + 0.5), static_cast<int>(tracker->KF.statePost.at<float>(3) + 0.5));
	return cv::Rect(rect2f);
}