#include "point_sift_segmentation.h"
#include <glog/logging.h>
#include <fstream>

void save_rgb_cloud_txt(std::string txt, pcl::PointCloud<pcl::PointXYZRGB>& pCloud)
{
	std::ofstream os;
	os.open(txt, std::ios::out);
	for (int i = 0; i < pCloud.points.size(); i++)
	{
		pcl::PointXYZRGB point = pCloud.points[i];
		char buf[255];
		memset(buf, 0, 255);
		sprintf(buf, "%f %f %f %d %d %d\n", point.x, point.y, point.z, point.r, point.g, point.b);
		os.write(buf, strlen(buf));
	}
	os.close();
}

Point_sift_segmentation::Point_sift_segmentation(int point_size, int cls, float freq, pcl::PointXYZ minp, pcl::PointXYZ maxp)
	:PointSifter(point_size, cls)
{
	m_point_num = point_size;
	m_cls_num = cls;
	m_freq = freq;
	m_minp = minp;
	m_maxp = maxp;
}

Error_manager Point_sift_segmentation::set_region(pcl::PointXYZ minp, pcl::PointXYZ maxp)
{
    m_minp = minp;
    m_maxp = maxp;
    if(m_maxp.x<=m_minp.x || m_maxp.y<=m_minp.y)
    {
        return Error_manager(LOCATER_SIFT_INPUT_BOX_PARAMETER_FAILED,NORMAL,
            "Point sift set region invalid :m_maxp.x<=m_minp.x || m_maxp.y<=m_minp.y ");
    }
    return SUCCESS;
}

Point_sift_segmentation::~Point_sift_segmentation()
{
}

Error_manager Point_sift_segmentation::init(std::string graph, std::string cpkt)
{
	if (!Load(graph, cpkt))
	{
		std::string error_string="pointSIFT Init ERROR:";
		error_string+=LastError();
		return Error_manager(LOCATER_SIFT_INIT_FAILED,NORMAL,error_string);
	}

    //创建空数据,第一次初始化后空跑
	float* cloud_data = (float*)malloc(m_point_num * 3 * sizeof(float));
	float* output = (float*)malloc(m_point_num * m_cls_num * sizeof(float));
	if (false == Predict(cloud_data, output))
	{
		free(cloud_data);
		free(output);
        std::string error_string="pointSIFT int first predict ERROR:";
        error_string+=LastError();
		return Error_manager(LOCATER_SIFT_PREDICT_FAILED,NORMAL,error_string);
	}
	else
	{
		free(cloud_data);
		free(output);

	}
	return SUCCESS;
}

#include <algorithm>
bool Point_sift_segmentation::Create_data(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, float* output)
{
	if (cloud->size() == 0)
		return false;
	////������άС����
	pcl::getMinMax3D(*cloud, m_minp, m_maxp);
	pcl::PointXYZ center;
	center.x = (m_minp.x + m_maxp.x) / 2.0;
	center.y = (m_minp.y + m_maxp.y) / 2.0;
	
	int l = int((m_maxp.x - m_minp.x) / m_freq) + 1;
	int w = int((m_maxp.y - m_minp.y) / m_freq) + 1;
	int h = int((m_maxp.z - m_minp.z) / m_freq) + 1;

	float* grid = (float*)malloc(l*w*h*sizeof(float));
	memset(grid, 0, l*w*h*sizeof(float));
	for (int i = 0; i < cloud->size(); ++i)
	{
		pcl::PointXYZ point = cloud->points[i];
		int idx = (point.x - m_minp.x) / m_freq;
		int idy = (point.y - m_minp.y) / m_freq;
		int idz = (point.z - m_minp.z) / m_freq;
		if (idx < 0 || idy < 0 || idz < 0)
			continue;
		*(grid + idx + idy*l + idz*l*w) = i+1;
	}
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_grid(new pcl::PointCloud<pcl::PointXYZ>);
	for (int i = 0; i < l*w*h; ++i)
	{
		if (*(grid + i) > 0)
		{
			int id = *(grid + i);
			if (id <= cloud->size())
			{
				pcl::PointXYZ point = cloud->points[id-1];
				cloud_grid->push_back(point);
			}
		}
	}
	free(grid);
	////ɸѡ�� m_point_num����
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_select(new pcl::PointCloud<pcl::PointXYZ>);
	if (cloud_grid->size()>m_point_num)
	{
		//����˳�� ȡǰm_point_num ����
		std::random_shuffle(cloud_grid->points.begin(), cloud_grid->points.end());	//����˳��
		cloud_select->points.assign(cloud_grid->points.begin(), cloud_grid->points.begin() + m_point_num);
	}
	else if (cloud_grid->size() < m_point_num)
	{
		int add = m_point_num - cloud_grid->size();
		if (add > cloud_grid->size())
			return false;
		std::random_shuffle(cloud_grid->points.begin(), cloud_grid->points.end());	//����˳��
		cloud_select->points.assign(cloud_grid->points.begin(), cloud_grid->points.begin() + add);
		cloud_select->points.insert(cloud_select->points.begin(), cloud_grid->points.begin(), cloud_grid->points.end());
	}
	else
	{
		cloud_select->points.assign(cloud_grid->points.begin(), cloud_grid->points.end());
	}

	if (cloud_select->points.size() != m_point_num)
	{
		LOG(ERROR) << "\tpointSIFT input select cloud is not " << m_point_num;
		return false;
	}

	for (int i = 0; i < m_point_num; ++i)
	{
		pcl::PointXYZ point = cloud_select->points[i];
		*(output + i * 3) = (point.x-center.x) / 1000.0;
		*(output + i * 3+1) = (point.y-center.y) / 1000.0;
		*(output + i * 3+2) = point.z / 1000.0;
	}
	return true;
}

Error_manager Point_sift_segmentation::seg(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
    std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_seg, std::string save_dir)
{
    if(cloud->size()==0)
    {
        return Error_manager(LOCATER_SIFT_INPUT_CLOUD_EMPTY,NORMAL,"PointSift input cloud empty ");
    }
	float* data = (float*)malloc(m_point_num * 3 * sizeof(float));
	float* output = (float*)malloc(m_point_num*m_cls_num*sizeof(float));
	memset(data, 0, m_point_num * 3 * sizeof(float));
	memset(output, 0, m_point_num*m_cls_num * sizeof(float));
	LOG(INFO) << "cloud size:" << cloud->size()<<"  /  "<<m_point_num;
	if (!Create_data(cloud, data))
	{
		free(data);
		free(output);
		return Error_manager(LOCATER_SIFT_CREATE_INPUT_DATA_FAILED,NORMAL,"pointSIFT Create input data ERROR");
	}

	if (!Predict(data, output))
	{
		free(data);
		free(output);
		return Error_manager(LOCATER_SIFT_PREDICT_FAILED,NORMAL,"pointSIFT predict ERROR");
	}
	////������
	RecoveryCloud(output, data, cloud_seg);
	if (!FilterObs(cloud_seg,save_dir))
	{
		free(data);
		free(output);
		return Error_manager(LOCATER_SIFT_FILTE_OBS_FAILED,NORMAL,"FilterObs failed");
	}
	free(output);
	free(data);
	//确保有车辆点云及轮胎存在,车辆类别 2,轮胎类别0
	int segmentation_class_size=cloud_seg.size();
	if(segmentation_class_size<3)
    {
	    return Error_manager(LOCATER_SIFT_PREDICT_NO_CAR_POINT,NORMAL,"PointSift detect no car point");
    }
	///保存中间文件
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr seg(new pcl::PointCloud<pcl::PointXYZRGB>);
	for (int i = 0; i < segmentation_class_size; ++i)
	{
		seg->operator +=(*cloud_seg[i]);
	}

	static int count = 0;
	count = (count + 1) % m_cls_num;
	char buf[64] = { 0 };
	sprintf(buf, "%s/SIFT_%d.txt", save_dir.c_str(), count);
	save_rgb_cloud_txt(buf, *seg);

	return SUCCESS;
}

bool Point_sift_segmentation::RecoveryCloud(float* output, float* cloud, std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_seg)
{
	pcl::PointXYZ center;
	center.x = (m_minp.x + m_maxp.x) / 2.0;
	center.y = (m_minp.y + m_maxp.y) / 2.0;

	for (int k = 0; k < m_cls_num; ++k)
	{
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb(new pcl::PointCloud<pcl::PointXYZRGB>);
		cloud_seg.push_back(cloud_rgb);
	}
	for (int i = 0; i < m_point_num; ++i)
	{
		pcl::PointXYZ point;
		point.x = *(cloud + i * 3)*1000.+center.x;
		point.y = *(cloud + i * 3 + 1)*1000.+center.y;
		point.z = *(cloud + i * 3 + 2)*1000.;
		if (point.x > m_maxp.x || point.x<m_minp.x
			|| point.y>m_maxp.y || point.y<m_minp.y
			|| point.z>m_maxp.z || point.z < m_minp.z)
		{
			continue;
		}
		float* prob = output + i*m_cls_num;
		int cls = 0;
		float max = prob[0];
		for (int j = 1; j < m_cls_num; j++)
		{
			if (prob[j] > max)
			{
				max = prob[j];
				cls = j;
			}
		}
		int r = 255, g = 255, b = 255;
		if (cls == 1)
		{
			r = 0;
			b = 0;
		}
		if (cls == 2)
		{
			b = 0;
			g = 0;
		}
		if (cls < m_cls_num)
		{
			pcl::PointXYZRGB point_rgb;
			point_rgb.x = point.x;
			point_rgb.y = point.y;
			point_rgb.z = point.z;
			point_rgb.r = r;
			point_rgb.g = g;
			point_rgb.b = b;
			cloud_seg[cls]->push_back(point_rgb);
		}
	}
		
	return true;
}


bool Point_sift_segmentation::FilterObs(std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_seg,std::string save_dir)
{
	/*if (cloud_seg.size() != m_cls_num)
	{
		LOG(ERROR) << "\t cloud_seg.size() != m_cls_num";
		return false;
	}*/
	
	const int obs_id = m_cls_num - 1;
	const int target_id = 1;
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr obs_cloud = cloud_seg[obs_id];

	if(cloud_seg.size()>0) {
        std::string sift_in = save_dir + "/c0.txt";
        save_rgb_cloud_txt(sift_in, *cloud_seg[0]);
    }
    if(cloud_seg.size()>1) {
        std::string sift_in = save_dir + "/c1.txt";
        save_rgb_cloud_txt(sift_in, *cloud_seg[1]);
    }

    if(cloud_seg.size()>2) {
        std::string sift_in = save_dir + "/c2.txt";
        save_rgb_cloud_txt(sift_in, *cloud_seg[2]);
    }

    return true;


	/*if (obs_cloud->size() > 100)
	{
		////ŷʽ����
		pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree_upground(new pcl::search::KdTree<pcl::PointXYZRGB>);
		tree_upground->setInputCloud(obs_cloud);

		std::vector<pcl::PointIndices> cluster_indices_upground;
		pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
		ec.setClusterTolerance(100); // 10cm
		ec.setMinClusterSize(30);
		ec.setMaxClusterSize(10000);
		ec.setSearchMethod(tree_upground);
		ec.setInputCloud(obs_cloud);
		ec.extract(cluster_indices_upground);

		std::vector<pcl::PointCloud<pcl::PointXYZRGB>> clusters_obs;
		for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices_upground.begin(); it != cluster_indices_upground.end(); ++it)
		{
			pcl::PointCloud<pcl::PointXYZRGB> cloud_cluster;
			for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
				cloud_cluster.push_back(obs_cloud->points[*pit]); //*
			cloud_cluster.width = cloud_cluster.size();
			cloud_cluster.height = 1;
			cloud_cluster.is_dense = true;
			clusters_obs.push_back(cloud_cluster);
		}
		/////  �� obs ���������
		std::vector<std::vector<cv::Point2f> > contours;
		for (int k = 0; k < clusters_obs.size(); ++k)
		{
			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_projected_XY(new pcl::PointCloud<pcl::PointXYZRGB>);
			pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
			coefficients->values.resize(4);
			coefficients->values[0] = 0.;
			coefficients->values[1] = 0.;
			coefficients->values[2] = 1.0;
			coefficients->values[3] = 0.;

			// Create the filtering object
			pcl::ProjectInliers<pcl::PointXYZRGB> proj;
			proj.setModelType(pcl::SACMODEL_PLANE);
			proj.setInputCloud(clusters_obs[k].makeShared());
			proj.setModelCoefficients(coefficients);
			proj.filter(*cloud_projected_XY);

			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_convexhull(new pcl::PointCloud<pcl::PointXYZRGB>);
			pcl::ConvexHull<pcl::PointXYZRGB> cconvexhull;
			cconvexhull.setInputCloud(cloud_projected_XY);
			cconvexhull.setDimension(2);
			cconvexhull.reconstruct(*cloud_convexhull);

			if (cloud_convexhull->size() > 3)
			{
				std::vector<cv::Point2f> contour;
				for (int j = 0; j < cloud_convexhull->size(); ++j)
				{
					pcl::PointXYZRGB point = cloud_convexhull->points[j];
					cv::Point2f  pt(point.x, point.y);
					contour.push_back(pt);
				}
				contours.push_back(contour);
			}
		}
		//// ��Ŀ������
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_target(new pcl::PointCloud<pcl::PointXYZRGB>);
		for (int i = 0; i < cloud_seg[target_id]->size(); ++i)
		{
			pcl::PointXYZRGB point = cloud_seg[target_id]->points[i];
			cv::Point2f  pt(point.x, point.y);
			bool valid = true;
			for (int n = 0; n < contours.size(); ++n)
			{
				if (cv::pointPolygonTest(contours[n], pt, true)>-300.)
				{
					valid = false;
					break;
				}
			}
			if (valid)
			{
				cloud_target->push_back(point);
			}
		}
		cloud_seg[target_id] = cloud_target;
		char buf[255] = { 0 };
		static int count = 0;
		sprintf(buf, "%s/target_%d.txt", save_dir.c_str(), count++%m_cls_num);
		save_rgb_cloud_txt(buf, *cloud_target);
	}
	return true;*/
}