//
// Created by zx on 2019/12/30.
//

#include "locate_manager.h"
#include "../tool/proto_tool.h"
#include "locate_parameter.pb.h"

#include <pcl/filters//voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <glog/logging.h>

#include <string.h>


Locate_manager::Locate_manager()
{
	mp_point_sift = NULL;
	mp_cnn3d = NULL;

	m_locate_manager_status = LOCATE_MANAGER_UNKNOW;
	m_locate_manager_working_flag = false;
	mp_locate_manager_thread = NULL;

	mp_locate_manager_task = NULL;
	mp_locate_information = NULL;
	m_save_flag = false;
//	m_save_path = "";
}
Locate_manager::~Locate_manager()
{
	Locate_manager_uninit();
}


//初始化 定位 管理模块。如下三选一LOCATE_PARAMETER_PATH
Error_manager Locate_manager::Locate_manager_init()
{
	return Locate_manager_init_from_protobuf(LOCATE_PARAMETER_PATH);
}
//初始化 定位 管理模块。从文件读取
Error_manager Locate_manager::Locate_manager_init_from_protobuf(std::string prototxt_path)
{
	Measure::LocateParameter t_locate_parameters;
	if(!  proto_tool::read_proto_param(prototxt_path,t_locate_parameters) )
	{
		return Error_manager(LOCATER_MANAGER_READ_PROTOBUF_ERROR,MINOR_ERROR,"Locate_manager read_proto_param  failed");
	}

	return Locate_manager_init_from_protobuf(t_locate_parameters);
}
//初始化 定位 管理模块。从protobuf读取
Error_manager Locate_manager::Locate_manager_init_from_protobuf(Measure::LocateParameter& locate_parameters)
{
	LOG(INFO) << " ---Locate_manager_init_from_protobuf run--- "<< this;
	Error_manager t_error;
	m_locate_manager_working_flag = false;

	int point_size = locate_parameters.seg_parameter().point_size();
	int cls_num = locate_parameters.seg_parameter().cls_num();
	double freq = locate_parameters.seg_parameter().freq();
	std::string graph = locate_parameters.seg_parameter().graph();
	std::string cpkt = locate_parameters.seg_parameter().cpkt();
	Measure::Area3d area = locate_parameters.seg_parameter().area();
	Cloud_box t_cloud_box;
	t_cloud_box.x_min = area.x_min();
	t_cloud_box.x_max = area.x_max();
	t_cloud_box.y_min = area.y_min();
	t_cloud_box.y_max = area.y_max();
	t_cloud_box.z_min = area.z_min();
	t_cloud_box.z_max = area.z_max();
	mp_point_sift = new Point_sift_segmentation(point_size,cls_num,freq,t_cloud_box);

	std::string	graph_file =  graph;
	std::string	cpkt_file =  cpkt;
	std::cout << "graph_file" << graph_file << std::endl;
	std::cout << "cpkt_file" << cpkt_file << std::endl;
	t_error=mp_point_sift->init(graph_file,cpkt_file);
	if(t_error!=SUCCESS)
	{
		LOG(ERROR)<<t_error.to_string();
		return t_error;
	}
	LOG(INFO)<<"  pointSift init SUCCESS,  graph = "<< graph;


	int t_cnn3d_length = locate_parameters.net_3dcnn_parameter().length();
	int t_cnn3d_width = locate_parameters.net_3dcnn_parameter().width();
	int t_cnn3d_height = locate_parameters.net_3dcnn_parameter().height();
	int t_cnn3d_nclass = locate_parameters.net_3dcnn_parameter().nclass();
	int t_cnn3d_freq = locate_parameters.net_3dcnn_parameter().freq();
	mp_cnn3d = new Cnn3d_segmentation(t_cnn3d_length, t_cnn3d_width, t_cnn3d_height, t_cnn3d_freq, t_cnn3d_nclass);

	std::string	weights = locate_parameters.net_3dcnn_parameter().weights_file();
	t_error=mp_cnn3d->init(weights);
	if(t_error!=SUCCESS)
	{
		LOG(ERROR)<<t_error.to_string();
		return t_error;
	}
	LOG(INFO)<<" 3dcnn Init SUCCESS ";


	m_locate_manager_status = LOCATE_MANAGER_READY;
	//启动雷达管理模块的内部线程。默认wait。
	m_locate_manager_condition.reset(false, false, false);
	mp_locate_manager_thread = new std::thread(&Locate_manager::thread_work, this);

	return SUCCESS;
}
//反初始化 定位 管理模块。
Error_manager Locate_manager::Locate_manager_uninit()
{
	LOG(INFO) << " ---Locate_manager_uninit run--- "<< this;

	//关闭线程
	if (mp_locate_manager_thread)
	{
		m_locate_manager_condition.kill_all();
	}
	//回收线程的资源
	if (mp_locate_manager_thread)
	{
		mp_locate_manager_thread->join();
		delete mp_locate_manager_thread;
		mp_locate_manager_thread = NULL;
	}

	//回收内存
	if ( mp_point_sift !=NULL )
	{
		delete mp_point_sift;
		mp_point_sift = NULL;
	}
	if ( mp_cnn3d !=NULL )
	{
		delete mp_cnn3d;
		mp_cnn3d = NULL;
	}
	m_locate_manager_status = LOCATE_MANAGER_UNKNOW;
	m_locate_manager_working_flag = false;

	return Error_code::SUCCESS;
}

//对外的接口函数，负责接受并处理任务单，
//input：p_locate_task 定位任务单，基类的指针，指向子类的实例，（多态）
Error_manager Locate_manager::execute_task(Task_Base* p_locate_task)
{
	LOG(INFO) << " ---Locate_manager::execute_task run---  "<< this;
	Error_manager t_error;
	Error_manager t_result;

	//检查指针
	if (p_locate_task == NULL) {
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "Laser_manager::execute_task failed, POINTER_IS_NULL");
	}
	//检查任务类型，
	if (p_locate_task->get_task_type() != LOCATE_MANGER_TASK)
	{
		return Error_manager(Error_code::LOCATER_MANAGER_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
							 "Locate_manager::execute_task  get_task_type() != LOCATE_MANGER_TASK ");
	}

	//检查接收方的状态
	std::cout << "m_locate_manager_status"<<m_locate_manager_status << std::endl;
	t_error = check_status();
	if ( t_error != SUCCESS )
	{
		t_result.compare_and_cover_error(t_error);
	}
	else
	{
		//接受任务，并将任务的状态改为TASK_SIGNED已签收
		mp_locate_manager_task = (Locate_manager_task *) p_locate_task;
		mp_locate_manager_task->set_task_statu(TASK_SIGNED);

		//检查消息内容是否正确，
		//检查三维点云指针
		if (mp_locate_manager_task->get_task_point_cloud_map() == NULL)
		{
			t_error.error_manager_reset(Error_code::POINTER_IS_NULL,
										Error_level::MINOR_ERROR,
										"execute_task mp_task_point_cloud is null");
			t_result.compare_and_cover_error(t_error);
		}
		else if ( mp_locate_manager_task->get_task_cloud_lock() == NULL )
		{
			t_error.error_manager_reset(Error_code::POINTER_IS_NULL,
										Error_level::MINOR_ERROR,
										"execute_task mp_task_cloud_lock is null");
			t_result.compare_and_cover_error(t_error);
		}
		else
		{
			//校验map的大小
			if ( (mp_locate_manager_task->get_task_point_cloud_map()->size() ==1 && mp_locate_manager_task->get_cloud_aggregation_flag() == true)
				 || ( (mp_locate_manager_task->get_task_point_cloud_map()->size() == CNN3D_WHEEL_NUMBER
					   || mp_locate_manager_task->get_task_point_cloud_map()->size() == CNN3D_WHEEL_NUMBER-1 )
					  && mp_locate_manager_task->get_cloud_aggregation_flag() == false) )
			{
				//解析任务单,将任务单的数据保存在本地.
				mp_locate_information = mp_locate_manager_task->get_task_locate_information_ex();
				m_save_flag = mp_locate_manager_task->get_task_save_flag();
				m_save_path = mp_locate_manager_task->get_task_save_path();
				//启动定位管理模块，的核心工作线程
				m_locate_manager_status = LOCATE_MANAGER_SIFT;
				m_locate_manager_working_flag = true;
				m_locate_manager_condition.notify_all(true);
				//通知 thread_work 子线程启动。

				//将任务的状态改为 TASK_WORKING 处理中
				mp_locate_manager_task->set_task_statu(TASK_WORKING);
			}
			else
			{
				t_error.error_manager_reset(Error_code::LOCATER_MANAGER_CLOUD_MAP_ERROR,
											Error_level::MINOR_ERROR,
											"execute_task input map error");
				t_result.compare_and_cover_error(t_error);
			}


		}

		//return 之前要填充任务单里面的错误码.
		if (t_result != Error_code::SUCCESS)
		{
			//忽略轻微故障
			if ( t_result.get_error_level() >= Error_level::MINOR_ERROR)
			{
				//将任务的状态改为 TASK_ERROR 结束错误
				mp_locate_manager_task->set_task_statu(TASK_ERROR);
			}
			//返回错误码
			mp_locate_manager_task->set_task_error_manager(t_result);
		}
	}


	return t_result;

}


//检查状态,是否正常运行
Error_manager Locate_manager::check_status()
{
	if ( m_locate_manager_status == LOCATE_MANAGER_READY )
	{
		return Error_code::SUCCESS;
	}
	else if ( m_locate_manager_status == LOCATE_MANAGER_SIFT
			  || m_locate_manager_status == LOCATE_MANAGER_CAR
			  || m_locate_manager_status == LOCATE_MANAGER_WHEEL)
	{
		return Error_manager(Error_code::LOCATER_MANAGER_STATUS_BUSY, Error_level::NEGLIGIBLE_ERROR,
							 " Locate_manager::check_status error ");
	}
	else
	{
		return Error_manager(Error_code::LOCATER_MANAGER_STATUS_ERROR, Error_level::MINOR_ERROR,
							 " Locate_manager::check_status error ");
	}
	return Error_code::SUCCESS;
}
//结束任务单，里面会根据任务的故障等级修正雷达管理模块的状态和任务单的状态
Error_manager Locate_manager::end_task()
{
	LOG(INFO) << " ---Locate_manager::end_task run---"<< this;

	//关闭子线程
	m_locate_manager_working_flag=false;
	m_locate_manager_condition.notify_all(false);

	//释放缓存
	mp_locate_information = NULL;
	m_cloud_wheel_map.clear();
	m_cloud_car_map.clear();

	//在结束任务单时，将雷达任务状态改为 TASK_OVER 已结束
	if(mp_locate_manager_task !=NULL)
	{
		//判断任务单的错误等级，
		if ( mp_locate_manager_task->get_task_error_manager().get_error_level() < Error_level::MINOR_ERROR)
		{
			if ( m_locate_manager_status == LOCATE_MANAGER_SIFT
				 || m_locate_manager_status == LOCATE_MANAGER_CAR
					|| m_locate_manager_status == LOCATE_MANAGER_WHEEL)
			{
				//故障等级为 NORMAL NEGLIGIBLE_ERROR MINOR_ERROR，则恢复待机，可以接受下一次的任务单
				m_locate_manager_status = LOCATE_MANAGER_READY;
			}
			//else 状态不变

			//强制改为TASK_OVER，不管它当前在做什么。
			mp_locate_manager_task->set_task_statu(TASK_OVER);
		}
		else
		{
			//故障等级为 MAJOR_ERROR CRITICAL_ERROR，则雷达故障，不可以再接受任务单。
			m_locate_manager_status = LOCATE_MANAGER_FAULT;

			//强制改为 TASK_ERROR，不管它当前在做什么。
			mp_locate_manager_task->set_task_statu(TASK_ERROR);
		}
	}

	return Error_code::SUCCESS;
}
//取消任务单，由发送方提前取消任务单
Error_manager Locate_manager::cancel_task()
{
	end_task();
	//强制改为 TASK_DEAD，不管它当前在做什么。
	mp_locate_manager_task->set_task_statu(TASK_DEAD);
	return Error_code::SUCCESS;
}

//判断是否为待机，如果已经准备好，则可以执行任务。
bool Locate_manager::is_ready()
{
	return m_locate_manager_status == LOCATE_MANAGER_READY;;
}

Locate_manager::Locate_manager_status Locate_manager::get_locate_manager_status()
{
	return m_locate_manager_status;
}

//mp_locate_manager_thread 线程执行函数，
//thread_work 内部线程负责locate定位分析整车的信息，并且回收汇总雷达的数据
void Locate_manager::thread_work()
{
	LOG(INFO) << " -------------------------mp_locate_manager_thread start "<< this;
	Error_manager t_error;
	Error_manager t_result;

	//定位管理的独立线程，
	while (m_locate_manager_condition.is_alive())
	{
		m_locate_manager_condition.wait();
		if ( m_locate_manager_condition.is_alive() )
		{
			std::this_thread::yield();
			//重新循环必须清除错误码.
			t_error.error_manager_clear_all();
			t_result.error_manager_clear_all();


			if ( mp_locate_manager_task == NULL )
			{
				m_locate_manager_status = LOCATE_MANAGER_FAULT;
				m_locate_manager_working_flag = false;
				m_locate_manager_condition.notify_all(false);
			}
			else if (mp_locate_manager_task->get_task_point_cloud_map() == NULL)
			{
				t_error.error_manager_reset(Error_code::POINTER_IS_NULL,
											Error_level::MINOR_ERROR,
											"thread_work mp_task_point_cloud is null");
				t_result.compare_and_cover_error(t_error);
				//因为故障,而提前结束任务.
				mp_locate_manager_task->compare_and_cover_task_error_manager(t_result);
				end_task();
			}
			else if ( mp_locate_manager_task->get_task_cloud_lock() == NULL )
			{
				t_error.error_manager_reset(Error_code::POINTER_IS_NULL,
											Error_level::MINOR_ERROR,
											"thread_work mp_task_cloud_lock is null");
				t_result.compare_and_cover_error(t_error);
				//因为故障,而提前结束任务.
				mp_locate_manager_task->compare_and_cover_task_error_manager(t_result);
				end_task();
			}
				//第一步
				//point_sift 点筛选分割 , 将输入点云分割为车身和轮胎,
				//注:地面会在box切割的时候,使用z轴 z_min 直接切除
			else if(m_locate_manager_status == LOCATE_MANAGER_SIFT)
			{
				m_cloud_wheel_map.clear();
				m_cloud_car_map.clear();

				//定位筛选,将输入点云map拆分为车轮和车身的map
				t_error = locate_manager_sift();
				t_result.compare_and_cover_error(t_error);

				//故障汇总, 只处理2级以上的故障
				if(t_result.get_error_level() >= MINOR_ERROR)
				{
					m_locate_manager_status = LOCATE_MANAGER_FAULT;
					mp_locate_manager_task->set_task_statu(TASK_ERROR);
					//因为故障,而提前结束任务.
					mp_locate_manager_task->compare_and_cover_task_error_manager(t_result);
					end_task();
				}
				else
				{
					//成功则 进入到下一个阶段,
					m_locate_manager_status = LOCATE_MANAGER_CAR;
				}
			}
				//第二步
				//_measure_height 计算汽车的长宽高
			else if(m_locate_manager_status == LOCATE_MANAGER_CAR)
			{
				//计算汽车长宽高,
				t_error = locate_manager_locate_car();
				t_result.compare_and_cover_error(t_error);

				//故障汇总, 只处理2级以上的故障
				if(t_result.get_error_level() >= MINOR_ERROR)
				{
					m_locate_manager_status = LOCATE_MANAGER_FAULT;
					mp_locate_manager_task->set_task_statu(TASK_ERROR);
					//因为故障,而提前结束任务.
					mp_locate_manager_task->compare_and_cover_task_error_manager(t_result);
					end_task();
				}
				else
				{
					//成功则 进入到下一个阶段,
					m_locate_manager_status = LOCATE_MANAGER_WHEEL;
				}
			}
				//第三步
				//locate_wheel 解析车轮信息
			else if(m_locate_manager_status == LOCATE_MANAGER_WHEEL)
			{
				//解析车轮信息,
				t_error = locate_manager_locate_wheel();
				t_result.compare_and_cover_error(t_error);

				//故障汇总, 只处理2级以上的故障
				if(t_result.get_error_level() >= MINOR_ERROR)
				{
					m_locate_manager_status = LOCATE_MANAGER_FAULT;
					mp_locate_manager_task->set_task_statu(TASK_ERROR);
					//因为故障,而提前结束任务.
					mp_locate_manager_task->compare_and_cover_task_error_manager(t_result);
					end_task();
				}
				else
				{
					m_locate_manager_working_flag = false;
					m_locate_manager_condition.set_pass_ever(false);
					//正常结束任务
					mp_locate_manager_task->compare_and_cover_task_error_manager(t_result);
					end_task();
				}
			}
		}
	}
	LOG(INFO) << " ---------------------------mp_locate_manager_thread end :"<<this;
	return;
}


//定位筛选,将输入点云map拆分为车轮和车身的map.(数据可以直接从locate_manager内部获取)
Error_manager Locate_manager::locate_manager_sift()
{
	if ( mp_locate_manager_task == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							" Locate_manager::locate_manager_sift() error ");
	}
	else if (mp_locate_manager_task->get_task_point_cloud_map() == NULL)
	{
		return Error_manager(Error_code::POINTER_IS_NULL,
									Error_level::MINOR_ERROR,
									"locate_manager_sift mp_task_point_cloud is null");
	}
	else if ( mp_locate_manager_task->get_task_cloud_lock() == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL,
									Error_level::MINOR_ERROR,
									"locate_manager_sift mp_task_cloud_lock is null");
	}

	Error_manager t_error;
	Error_manager t_result;

	//获取任务单的输入点云
	std::unique_lock<std::mutex> lck (*(mp_locate_manager_task->get_task_cloud_lock()));
	std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr> *tp_task_point_cloud_map = mp_locate_manager_task->get_task_point_cloud_map();

	//清除map
	m_cloud_wheel_map.clear();
	m_cloud_car_map.clear();

	//遍历任务单的输入点云map
	for (const auto &map_iter : *tp_task_point_cloud_map)
	{
		if (map_iter.second.get() == NULL)
		{
			return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
								 " Locate_manager::locate_manager_sift() pcl::PointCloud<pcl::PointXYZ>::Ptr is NULL ");
		}
		//提前为拆分之后的点云分配内存.
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr tp_cloud_wheel(new pcl::PointCloud<pcl::PointXYZRGB>);
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr tp_cloud_car(new pcl::PointCloud<pcl::PointXYZRGB>);
		//利用PointSift从场景中分割车辆点云
		t_error = locate_sift(map_iter.second, tp_cloud_wheel, tp_cloud_car, m_save_flag, m_save_path);
		if (t_error != Error_code::SUCCESS)
		{
			char buf[256] = {0};
			sprintf(buf, " map.id = %d, locate_sift error", map_iter.first );
			t_error.add_error_description(buf, strlen(buf) );
			t_result.compare_and_cover_error(t_error);
			//注:这里不直接return,而是要把map全部执行完成
		}
		else
		{
			//将结果存入本地的中间缓存,此时map点云是有内存的,后续要记得回收
			m_cloud_wheel_map[map_iter.first] = tp_cloud_wheel;
			m_cloud_car_map[map_iter.first] = tp_cloud_car;
		}
	}
	return t_result;
}

//定位筛选,对具体的点云进行操作,分离出车轮和车身.
//input::cloud输入点云
//output::cloud_wheel输出车轮点云
//output::cloud_car输出车身点云
//work_dir:中间文件保存路径
Error_manager Locate_manager::locate_sift(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in,
                                   pcl::PointCloud<pcl::PointXYZRGB>::Ptr& p_cloud_wheel,pcl::PointCloud<pcl::PointXYZRGB>::Ptr& p_cloud_car,
										   bool save_flag, std::string work_dir)
{
    if(p_cloud_in.get()==0)
    {
        return Error_manager(LOCATER_SIFT_INPUT_CLOUD_UNINIT,MINOR_ERROR,"sift input cloud uninit");
    }
    if(p_cloud_in->size()==0)
    {
        return Error_manager(LOCATER_SIFT_INPUT_CLOUD_EMPTY,MINOR_ERROR,"locate_sift input cloud empty");
    }
    if(mp_point_sift==NULL)
    {
        return Error_manager(LOCATER_POINTSIFT_UNINIT,MINOR_ERROR,"Point Sift unInit");
    }
	Error_manager t_error;

    //分割车辆点云
    std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> segmentation_clouds;
	t_error=mp_point_sift->segmentation(p_cloud_in, segmentation_clouds, save_flag, work_dir);
    if(t_error!=SUCCESS)
    {
        return t_error;
    }
    //第0类即是轮胎点云,第1类为车身点云
    pcl::copyPointCloud(*segmentation_clouds[0], *p_cloud_wheel);
    pcl::copyPointCloud(*segmentation_clouds[1], *p_cloud_car);

    return SUCCESS;
}


//计算汽车高度,
Error_manager Locate_manager::locate_manager_locate_car()
{
	if ( mp_locate_manager_task == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 " Locate_manager::locate_manager_locate_car() error ");
	}
	else if (mp_locate_manager_task->get_task_point_cloud_map() == NULL)
	{
		return Error_manager(Error_code::POINTER_IS_NULL,
							 Error_level::MINOR_ERROR,
							 "locate_manager_locate_car mp_task_point_cloud is null");
	}
	else if ( mp_locate_manager_task->get_task_cloud_lock() == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL,
							 Error_level::MINOR_ERROR,
							 "locate_manager_locate_car mp_task_cloud_lock is null");
	}
	Error_manager t_error;

	pcl::PointXYZRGB t_point3d_min, t_point3d_max;

	std::unique_lock<std::mutex> lck (*(mp_locate_manager_task->get_task_cloud_lock()));

	for (auto iter = m_cloud_car_map.begin(); iter != m_cloud_car_map.end(); ++iter)
	{
		pcl::PointXYZRGB min, max;
		t_error = locate_car(iter->second, min, max);
		if ( t_error != Error_code::SUCCESS )
		{
			return t_error;
		}

		//比较选出整个map点云的边界
		if ( t_point3d_min.x <= min.x )
		{
			t_point3d_min.x = min.x;
		}
		if ( t_point3d_min.y <= min.y )
		{
			t_point3d_min.y = min.y;
		}
		if ( t_point3d_min.z <= min.z )
		{
			t_point3d_min.z = min.z;
		}
		if ( t_point3d_max.x >= max.x )
		{
			t_point3d_max.x = max.x;
		}
		if ( t_point3d_max.y >= max.y )
		{
			t_point3d_max.y = max.y;
		}
		if ( t_point3d_max.z >= max.z )
		{
			t_point3d_max.z = max.z;
		}

	}
	mp_locate_information->locate_length = t_point3d_max.y - t_point3d_min.y;
	mp_locate_information->locate_width = t_point3d_max.x - t_point3d_min.x;;
	mp_locate_information->locate_height = t_point3d_max.z;
	//注意了:在雷达扫描时, 就已经将地面标定位为 z = 0
	return Error_code::SUCCESS;
}

//根据汽车点云计算汽车的边界
Error_manager Locate_manager::locate_car(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_car, pcl::PointXYZRGB& min, pcl::PointXYZRGB& max)
{
    if(cloud_car.get()==0)
    {
        return Error_manager(LOCATER_MEASURE_HEIGHT_CLOUD_UNINIT,MINOR_ERROR,"measure height input cloud uninit");
    }
    if(cloud_car->size()==0)
    {
        return Error_manager(LOCATER_MEASURE_HEIGHT_CLOUD_EMPTY,MINOR_ERROR,"measure height input cloud is empty");
    }

    //获取点云的边界
    pcl::getMinMax3D(*cloud_car,min,max);

    return SUCCESS;
}




//计算汽车的车轮信息
Error_manager Locate_manager::locate_manager_locate_wheel()
{
	if ( mp_locate_manager_task == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 " Locate_manager::locate_manager_locate_car() error ");
	}
	else if (mp_locate_manager_task->get_task_point_cloud_map() == NULL)
	{
		return Error_manager(Error_code::POINTER_IS_NULL,
							 Error_level::MINOR_ERROR,
							 "locate_manager_locate_car mp_task_point_cloud is null");
	}
	else if ( mp_locate_manager_task->get_task_cloud_lock() == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL,
							 Error_level::MINOR_ERROR,
							 "locate_manager_locate_car mp_task_cloud_lock is null");
	}
	if(mp_cnn3d==NULL)
	{
		return Error_manager(LOCATER_3DCNN_UNINIT,MINOR_ERROR,"locate_wheel 3dcnn is not init");
	}

	Error_manager t_error;
	std::unique_lock<std::mutex> lck (*(mp_locate_manager_task->get_task_cloud_lock()));
	//解析车轮
	t_error=mp_cnn3d->analytic_wheel(m_cloud_wheel_map,mp_locate_manager_task->get_cloud_aggregation_flag(),
									 mp_locate_manager_task->get_task_locate_information_ex(),
									 m_save_flag,m_save_path);
	if ( t_error != Error_code::SUCCESS )
	{
		return t_error;
	}

	return Error_code::SUCCESS;
}

//保存点云成txt到文件
void Locate_manager::save_cloud_txt(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,std::string save_file)
{
    FILE* pfile=fopen(save_file.c_str(),"w");
    for(int i=0;i<cloud->size();++i)
    {
        fprintf(pfile,"%.3f %.3f %.3f\n",cloud->points[i].x,cloud->points[i].y,cloud->points[i].z);
    }
    fclose(pfile);
}