#include <set>
#include "fusion.h"
#define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000)
char LCFusion::fusion(std::vector<Mat> &camInfo)
{
	Mat boxes = camInfo[1], dwha = camInfo[2];
	projectedPoints.clear();

	if (this->upScan.pointCloud.empty())
	{
		std::cout << "There is no lidar data!" << std::endl;
		return -2;
	}

	if (boxes.empty())
	{
		std::cout << "There is no detection box!" << std::endl;
		return -1;
	}
	else
	{
		draw_point();
		std::cout << "Fusion start!" << std::endl;
		// 大物体
		vector<int> clusterlable = clusters_bboxs(boxes);
		optimize_clusterlable(clusterlable, boxes);
		big_fusion(clusterlable, boxes, dwha);

		//小物体
		small_fusion(boxes, dwha);
	}
	return 1;
}

void LCFusion::big_fusion(vector<int> &clusterlable, Mat &bboxs, Mat &dwha)
{
	std::cout << "big_fusion" << std::endl;
	for (int i = 0; i < clusterlable.size(); i++)
	{
		int bbox_num = clusterlable[i];

		if (bbox_num != -1)
		{
			int bbox_cls = bboxs.at<float>(bbox_num, 5);
			if (class_index[bbox_cls] != 13 && class_index[bbox_cls] != 14)
			{
			//std::cout << "bboxcls:" << bbox_cls << std::endl;
				for (int j = 0; j < this->upScan.clustedCloud[i].size(); j++)
				{
					int laser_index = this->upScan.clustedCloud[i][j].at<float>(0, 3);
					this->upScan.intensities[laser_index] = class_intensity[bbox_cls + 1];
				}
			}
			else
			{
				if (upScan.clustedCloud[i].size() > 0 && upScan.clustedCloud[i].size() < 10)
				{
					cout << "******start draw circle******" << endl;
					draw_circle(i, bbox_num, bboxs, dwha);
				}
					
			}
		}
	}
}
void LCFusion::small_fusion(Mat &bboxs, Mat &dwha)
{
	std::cout << "small_fusion" << std::endl;
	for (int i = 0; i < bboxs.rows; i++)
	{
		int c = bboxs.at<float>(i, 5);
		std::cout << "cls:" << c << std::endl;
		if (class_index[c] >= 0 && class_index[c] <= 6)
		{

			Mat temp(2, 1, CV_32F);
			double d = dwha.at<float>(i, 0);
			double a = dwha.at<float>(i, 3);
			if (d == -1)
				continue;
			//std::cout << "d:" << d << " a:  " << a << std::endl;
			temp.at<float>(0, 0) = float((dwha.at<float>(i, 0) / 100.0) * cos(dwha.at<float>(i, 3)));
			temp.at<float>(1, 0) = float((dwha.at<float>(i, 0) / 100.0) * sin(dwha.at<float>(i, 3)));
			temp = c2lR * temp + c2lT;
			double y = temp.at<float>(0, 0);
			double x = temp.at<float>(1, 0);
			float dis = sqrt(x * x + y * y);

			float angle = 0.0;
			if (y >= 0)
				angle = (acos(x / dis) - this->upScan.angleMin) / this->upScan.angleInc;
			else
				angle = ((2 * M_PI - acos(x / dis)) - this->upScan.angleMin) / this->upScan.angleInc;

			double data_angle = atan((dwha.at<float>(i, 1)) / (200 * dis));
			data_angle /= this->upScan.angleInc;
			angle = (int)angle - (int)data_angle;
			int num_point = 2 * (int)data_angle + 1;

			int len = this->upScan.distance.size();
			for (int i = 0; i < num_point; i++)
			{
				if ((angle + i) < 0 || (angle + i) > (len - 1))
					continue;

				this->upScan.distance[angle + i] = dis;
				this->upScan.intensities[angle + i] = class_intensity[c + 1];
			}
		}
	}
}

void LCFusion::optimize_clusterlable(vector<int> &clusterlable, Mat &bboxs)
{
	for (int i = 0; i < bboxs.rows; i++)
	{
		vector<int> temp_cluster_num;
		Mat temp_bbox = bboxs.row(i).clone();
		for (int j = 0; j < clusterlable.size(); j++)
		{
			if (clusterlable[j] == i)
			{
				temp_cluster_num.push_back(j);
			}
		}
		if (temp_cluster_num.size() == 0)
			continue;
		choose_forceground(temp_cluster_num, temp_bbox, clusterlable);
	}
}

void LCFusion::choose_forceground(vector<int> &cluster_num, Mat &bbox, vector<int> &clusterlable)
{
	int box_class = bbox.at<float>(0, 5);
	if (class_index[box_class] >= 15 && class_index[box_class] <= 18 && cluster_num.size() > 3)
	{
		int total_point = 0;
		for (int i = 0; i < cluster_num.size(); i++)
		{
			total_point += (this->upScan.clustedCloud[cluster_num[i]].size());
		}
		for (int j = 0; j < cluster_num.size(); j++)
		{
			float num_cluster = this->upScan.clustedCloud[cluster_num[j]].size();
			if (num_cluster / total_point > 0.2)
			{
				clusterlable[cluster_num[j]] = -1;
			}
		}
	}
}

void LCFusion::draw_point()
{
	projectedPoints.clear();
	for (int i = 0; i < this->upScan.pointCloud.rows; i++)
	{
		Mat temp_pointcloud = this->upScan.pointCloud.row(i).clone();
		Mat uv = pointcloud_pixel(temp_pointcloud);
		int x = uv.at<float>(0, 0);
		int y = uv.at<float>(1, 0);
		if (x > 0 && x < 640 && y > 0 && y < 480)
		{
			Point2d pt_uv(x, y);
			projectedPoints.push_back(pt_uv);
		}
	}
}

void LCFusion::filter_laser(const FrameData &laserData, float angle_min, float angleInc)
{
	upScan.distance.clear();
	upScan.intensities.clear();
	float pointAngle;
	upScan.angleInc = angleInc;

	for (int i = 0; i < laserData.len; i++)
	{
		pointAngle = angle_min + i * upScan.angleInc;
		if (pointAngle > cameraAnglemin)
		{
			if (pointAngle > cameraAnglemax)
			{
				upScan.angleMax = pointAngle - upScan.angleInc;
				break;
			}
			upScan.distance.push_back(laserData.distance[i] / 1000.f);
			upScan.intensities.push_back(0);
			if (i == (laserData.len - 1))
				upScan.angleMax = pointAngle;
			// upScan.intensities.push_back(laserData.intensities[i]);
		}
	}
	upScan.angleMin = upScan.angleMax - (upScan.distance.size() - 1) * upScan.angleInc;
	this->scan_to_pointcloud();
	this->cluster();
}

void LCFusion::scan_to_pointcloud()
{
	u_int32_t len = upScan.distance.size();
	Mat pointcloud(len, 4, CV_32F);

	for (int i = 0; i < len; i++)
	{
		float dis = this->upScan.distance[i];
		float x_temp = cos(upScan.angleMin + i * upScan.angleInc) * dis;
		float y_temp = sin(upScan.angleMin + i * upScan.angleInc) * dis;
		pointcloud.at<float>(i, 0) = x_temp;
		pointcloud.at<float>(i, 1) = y_temp;
		pointcloud.at<float>(i, 2) = 0.0;
		pointcloud.at<float>(i, 3) = i;
	}
	this->upScan.pointCloud = pointcloud.clone();
}

uchar LCFusion::lidar2box(Mat uv, Mat boxes, uchar *boxflag)
{
	int x = uv.at<float>(0, 0);
	int y = uv.at<float>(1, 0);
	// 实验测试，过滤类别
	std::set<uchar> filterCls = {0, 7, 11, 14, 15, 21};
	for (uchar i = 0; i < boxes.rows; i++)
	{
		int clsIdx = boxes.at<float>(i, 5);
		if (filterCls.find(clsIdx) != filterCls.end())
			continue;

		float xmin = boxes.at<float>(i, 0);
		float xmax = boxes.at<float>(i, 2);
		float ymin = boxes.at<float>(i, 1);
		float ymax = boxes.at<float>(i, 3);

		// 实验测试，过滤过大的误检框
		float ratio = (xmax - xmin) * (ymax - ymin) / 308480.;
		if (ratio > 0.7)
		{
			boxflag[i] += 1;
			continue;
		}

		// 若雷达点处于某个目标框内就返回其对应强度值，未考虑目标框重叠情况
		if (x >= xmin && x <= xmax && y >= ymin && y <= ymax)
		{
			boxflag[i] += 1;
			return class_intensity[clsIdx + 1];
		}
	}
	return 0;
}

std::vector<int> LCFusion::clusters_bboxs(Mat &bboxs)
{
	std::vector<int> clusterlable;
	for (int i = 0; i < this->upScan.clustedCloud.size(); i++)
	{
		std::vector<Mat> temp_cluster = this->upScan.clustedCloud[i];
		int temp_clusters_bbox = cluster_bboxs(temp_cluster, bboxs);
		clusterlable.push_back(temp_clusters_bbox);
	}
	return clusterlable;
}

int LCFusion::cluster_bboxs(vector<Mat> &one_cluster, Mat &bboxs)
{
	vector<int> temp_result;
	Mat begin = pointcloud_pixel(one_cluster[one_cluster.size() - 1]);
	Mat end = pointcloud_pixel(one_cluster[0]);
	for (int i = 0; i < bboxs.rows; i++)
	{
		Mat bbox = bboxs.row(i).clone();
		int cls = bbox.at<float>(0, 5);
		float ratio = ratio_in_1box(begin, end, bbox);
		if (ratio > 0.8 && this->class_index[cls] >= 7 && class_index[cls] <= 18)
			temp_result.push_back(i);
	}
	if (temp_result.size() < 1)
		return -1;
	else if (temp_result.size() < 2)
		return temp_result[0];
	else
	{
		int min = temp_result[0];
		int width_min = bboxs.at<float>(min, 2) - bboxs.at<float>(min, 0);
		for (int i = 0; i < temp_result.size(); i++)
		{
			int width_new = bboxs.at<float>(temp_result[i], 2) - bboxs.at<float>(temp_result[i], 0);
			if (width_new < width_min)
			{
				min = temp_result[i];
				width_min = width_new;
			}
		}
		return min;
	}
	return -1;
}

Mat LCFusion::pointcloud_pixel(Mat &pointcloud)
{
	Mat uv(3, 1, CV_32F), cutPointCloud(pointcloud.colRange(0, 3));
	Mat camPoint = rotate * cutPointCloud.t() + translation;
	// float ppp = camPoint.at<float>(2, 0);
	if (camPoint.at<float>(2, 0) <= 0)
	{
		uv = (Mat_<float>(3, 1) << -1, -1, -1);
		return uv;
	}

	float scaleX = camPoint.at<float>(0, 0) / camPoint.at<float>(2, 0);
	float scaleY = camPoint.at<float>(1, 0) / camPoint.at<float>(2, 0);
	float scaleD = scaleX * scaleX + scaleY * scaleY;
	float tempD = 1 + distCoeff.at<float>(0, 0) * scaleD + distCoeff.at<float>(0, 1) * scaleD * scaleD + distCoeff.at<float>(0, 4) * scaleD * scaleD * scaleD;

	camPoint.at<float>(0, 0) = scaleX * tempD + 2 * distCoeff.at<float>(0, 2) * scaleX * scaleY +
							   distCoeff.at<float>(0, 3) * (scaleD + 2 * scaleX * scaleX);

	camPoint.at<float>(1, 0) = scaleY * tempD + distCoeff.at<float>(0, 2) * (scaleD + 2 * scaleY * scaleY) +
							   2 * distCoeff.at<float>(0, 3) * scaleX * scaleY;

	camPoint.at<float>(2, 0) = 1.0;
	uv = cameraMat * camPoint;
	uv = p2r * uv + t;
	return uv;
}

float LCFusion::ratio_in_1box(Mat &begin, Mat &end, Mat &box)
{
	int x_begin = begin.at<float>(0, 0), x_end = end.at<float>(0, 0), y_begin = begin.at<float>(1, 0), y_end = end.at<float>(1, 0);
	int xmin = box.at<float>(0, 0), ymin = box.at<float>(0, 1), xmax = box.at<float>(0, 2), ymax = box.at<float>(0, 3);
	if (y_begin < ymin || y_begin > ymax)
		return 0;
	if (x_begin < xmin)
	{
		if (x_end < xmin)
			return 0;
		else if (x_end < xmax)
			return (float)(x_end - xmin) / (x_end - x_begin);
		else
			return (float)(xmax - xmin) / (x_end - x_begin);
	}
	else if (x_begin < xmax)
	{
		if (x_end < xmax)
			return 1;
		else
			return (float)(xmax - x_begin) / (x_end - x_begin);
	}
	else
		return 0;
}

void LCFusion::cluster()
{
	this->upScan.clustedCloud.clear();
	int i = 0;

	for (; i < this->upScan.distance.size();)
	{
		std::vector<Mat> temp;
		int j = i;
		temp.push_back(this->upScan.pointCloud.row(i));
		for (; j < this->upScan.distance.size() - 1; j++)
		{

			// 距离的6%作为分类的阈值
			if (abs(this->upScan.distance[j] - this->upScan.distance[j + 1]) < this->upScan.distance[j] * 0.07 )
			{
				temp.push_back(this->upScan.pointCloud.row(j + 1));
			}
			else
			{
				i = j + 1;
				break;
			}
		}
		this->upScan.clustedCloud.push_back(temp);
		if (j == this->upScan.distance.size() - 1)
			break;
		if (i == upScan.distance.size() - 1)
		{
			temp.push_back(upScan.pointCloud.row(i));
			this->upScan.clustedCloud.push_back(temp);
			break;
		}
	}

	this->clusterIdx.clear();
	int ii = 0, totoalNum = 0;
	for (auto c : this->upScan.clustedCloud)
	{
		for (int jj = 0; jj < c.size(); jj++)
		{
			this->clusterIdx.push_back(ii);
		}
		totoalNum += c.size();
		ii++;
	}
	//std::cout << "cluster num: " << this->upScan.clustedCloud.size() << " ii:" << ii << " total:" << totoalNum << " oriNum: " << this->upScan.distance.size() << std::endl;
}
void LCFusion::draw_circle(int cluster_num, int bbox_num, Mat &bboxs,  Mat &dwha)
{
	vector<float> center_point = find_circle_center(cluster_num);
	float alfa = center_point[1] * upScan.angleInc + upScan.angleMin, r = dwha.at<float>(bbox_num, 1)/200.f, 
			d = center_point[0],  half_data_num = atan(r/d)/(upScan.angleInc);
	int num_laser_circle = half_data_num * 2 + 1, cls = bboxs.at<float>(bbox_num, 5);
	float laser_index = center_point[1] - half_data_num;
	//cout << "alfa: " << alfa << " r: " << r << " d: " << d << "  num_laser_circle: " << num_laser_circle << " start_index: " << laser_index << endl;
	for (int i = 0; i < num_laser_circle; i++)
	{
		float theta = upScan.angleMin + laser_index * upScan.angleInc;
		if (laser_index < 0 || laser_index > upScan.distance.size())
		{
			laser_index++;
			continue;
		}
		float data = pow(r, 2) - pow(d, 2) * pow(sin(theta - alfa), 2);
		if (data >= 0)
		{
			//cout << "draw_one_point" << endl;
			upScan.distance[laser_index] = circle(d, alfa ,theta, data);
			upScan.intensities[laser_index] = class_intensity[cls + 1];
		}
		laser_index++;
	}
}
vector<float> LCFusion::find_circle_center(int cluster_num)
{
	vector<float> center_point(2);
	int index = 0;
	float total_dis = 0;
	int num = upScan.clustedCloud[cluster_num].size();
	int no_zero_dis = 0;
	for (int i = 0 ; i < num; i++)
	{
		index = upScan.clustedCloud[cluster_num][i].at<float>(0, 3);
		if (upScan.distance[index] != 0)
			no_zero_dis++;
		total_dis += upScan.distance[index];
	}
	center_point[0] = total_dis/ no_zero_dis;
	index -= (num/2);
	float center_laser_index = index;
	center_point[1] = center_laser_index;
	return center_point;
}
float LCFusion::circle(float d, float alfa ,float theta, float data) 	// d 中心点距离 r 圆半径 a 中心点角度 theta 雷达角度 
{
 	float ro = d * cos(theta - alfa) - sqrt(data);
 	return ro > 0 ? ro:0;
}
void LCFusion::set_laser_data(sensor_msgs::LaserScan &scan, FrameData &laserData)
{
	float angle_min = ANGLE_TO_RADIAN(laserData.angle_min);
	float angle_max = ANGLE_TO_RADIAN(laserData.angle_max);
	uint32_t len = laserData.len;
	float angle_inc = (angle_max - angle_min) / (len - 1.);
	scan.header.frame_id = "down_laser_link";
	scan.range_min = 0.15;
	scan.range_max = 8.0;
	scan.angle_min = angle_min;
	scan.angle_max = angle_max;
	scan.angle_increment = angle_inc;
	scan.ranges.resize(len);
	scan.intensities.resize(len);
	scan.time_increment = 1. / 2400.;
	for (int i = 0; i < len; i++)
	{
		scan.ranges[i] = laserData.distance[i] / 1000.f;
		scan.intensities[i] = 0;
	}
}
void LCFusion::set_laser_data(sensor_msgs::LaserScan &scan)
{
	scan.header.frame_id = "up_laser_link";
	scan.range_min = 0.15;
	scan.range_max = 8.0;
	scan.angle_increment = upScan.angleInc;
	scan.ranges.resize(upScan.distance.size());
	scan.intensities.resize(upScan.distance.size());
	scan.angle_min = upScan.angleMin;
	scan.angle_max = upScan.angleMax;
	for (int i = 0; i < upScan.distance.size(); i++)
	{
		scan.ranges[i] = upScan.distance[i];
		scan.intensities[i] = upScan.intensities[i];
	}
	scan.time_increment = 1. / 2400.;
}