//
// Created by zx on 2020/6/18.
//

#include <iostream>
#include "./error_code/error_code.h"
#include "LogFiles.h"
#include <glog/logging.h>

#include "./laser/laser_manager.h"
#include "./laser/Laser.h"
#include "./laser/LivoxLaser.h"

#include "./locate/locate_manager.h"

#include "./communication/communication_socket_base.h"

#include "./tool/thread_pool.h"
#include "./system/system_communication.h"
#include "./system/system_executor.h"

#include "./wanji_lidar/wanji_lidar_device.h"
#include "./tool/proto_tool.h"
#include "./wanji_lidar/wanji_manager.h"

#define LIVOX_NUMBER	     2

GOOGLE_GLOG_DLL_DECL void shut_down_logging(const char* data, int size)
{
	time_t tt;
	time( &tt );
	tt = tt + 8*3600;  // transform the time zone
	tm* t= gmtime( &tt );
	char buf[255]={0};
	sprintf(buf,"./%d%02d%02d-%02d%02d%02d-dump.txt",
			t->tm_year + 1900,
			t->tm_mon + 1,
			t->tm_mday,
			t->tm_hour,
			t->tm_min,
			t->tm_sec);

	FILE* tp_file=fopen(buf,"w");
	fprintf(tp_file,data,strlen(data));
	fclose(tp_file);

}



int main(int argc,char* argv[])
{


	Error_manager t_error;
	Error_manager t_result ;


	const char* logPath = "./";
	google::InitGoogleLogging("LidarMeasurement");
	google::SetStderrLogging(google::INFO);
	google::SetLogDestination(0, logPath);
	google::SetLogFilenameExtension("zxlog");
	google::InstallFailureSignalHandler();
	google::InstallFailureWriter(&shut_down_logging);
	FLAGS_colorlogtostderr = true;        // Set log color
	FLAGS_logbufsecs = 0;                // Set log output speed(s)
	FLAGS_max_log_size = 1024;            // Set max log file size(GB)
	FLAGS_stop_logging_if_full_disk = true;



/*
	Wanji_manager::get_instance_references().wanji_manager_init();
	std::cout << " huli test :::: " << " wanji_manager_init = " << 1 << std::endl;

	while (1  )
	{
		char ch1 ;
		std::cin >> ch1 ;
		if (ch1 == 'b'  )
		{
			break;
		}

		auto start   = std::chrono::system_clock::now();


		Wanji_manager_task task;
		task.task_init(TASK_CREATED, "", NULL,
					   std::chrono::milliseconds(1000),
					   0,
					   std::chrono::system_clock::now());
		t_error = Wanji_manager::get_instance_references().execute_task(&task);
		std::cout << " huli test 123123:::: " << " t_error = " << t_error << std::endl;


		//判断任务是否结束, 阻塞 等待, 直到任务完成或者超时
		while ( task.is_task_end() == false)
		{
			if ( task.is_over_time() )
			{
				//超时处理。取消任务。
				Wanji_manager::get_instance_references().cancel_task(&task);
				task.set_task_statu(TASK_DEAD);
				t_error.error_manager_reset(Error_code::WANJI_LIDAR_DEVICE_TASK_OVER_TIME, Error_level::MINOR_ERROR,
											" locate_manager_task is_over_time ");
				task.set_task_error_manager(t_error);
			}
			else
			{
				//继续等待
				std::this_thread::sleep_for(std::chrono::microseconds(1));
				std::this_thread::yield();
			}

		}
		//提取任务单 的错误码
		t_error = task.get_task_error_manager();
		std::cout << "huli  error 456456:::::::"  << t_error << std::endl;
		std::cout << " huli test :::: " << " task.get_task_statu() = " << task.get_task_statu() << std::endl;


		auto end   = std::chrono::system_clock::now();
		auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
		std::cout <<  "花费了"
			 << double(duration.count()*1000) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den   << "毫秒" << std::endl;
		std::cout << " huli test :::: " << " std::chrono::microseconds::period::num = " << std::chrono::microseconds::period::num << std::endl;
		std::cout << " huli test :::: " << " std::chrono::microseconds::period::den = " << std::chrono::microseconds::period::den << std::endl;
		std::cout << " huli test :::: " << " duration.count() = " << duration.count() << std::endl;


	}
//	Wanji_manager::get_instance_references().wanji_manager_uninit();
	std::cout << " huli test :::: " << " uninit = " << 999999 << std::endl;
	return 0;

*/












/*


	int t_terminal_id = 0;
	if ( argc == 2 )
	{
		std::cout << " huli test :::: " << " argv[1] = " << argv[1] << std::endl;
		t_terminal_id = atoi(argv[1]);
	}
	std::cout << " huli test :::: " << " t_terminal_id = " << t_terminal_id << std::endl;


	Laser_manager::get_instance_references().laser_manager_init();
	std::cout << "Laser_manager = " << Laser_manager::get_instance_references().get_laser_manager_status() << std::endl;
	Locate_manager::get_instance_references().Locate_manager_init();
	std::cout << "Locate_manager = " << Locate_manager::get_instance_references().get_locate_manager_status() << std::endl;
	System_executor::get_instance_references().system_executor_init(4, t_terminal_id);
	std::cout << "System_executor = " << System_executor::get_instance_references().get_system_executor_status() << std::endl;

	System_communication::get_instance_references().communication_init();

	char ch;
	while ( 1 )
	{
		std::cin >> ch ;

		if ( ch == 'b' )
		{
			break;
		}


		//第一步, 创建点云缓存, 和 指定雷达,目前就一个
		std::map<int,pcl::PointCloud<pcl::PointXYZ>::Ptr>		point_cloud_map;
		std::mutex cloud_lock;

		pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud(new pcl::PointCloud<pcl::PointXYZ>);

		std::ifstream is;
		is.open("/home/huli/huli/Terminal_project/hl_measure/Terminal_process_CT/test_data/car_cloud/20200907-084334_src - Cloud.txt", std::ios::out);
		char t_buf[256];
		while ( !is.eof() )
		{
			is.getline(t_buf, 256);
			pcl::PointXYZ t_point;
			sscanf(t_buf, "%f %f %f", &t_point.x, &t_point.y, &t_point.z);
			t_point.x = t_point.x / 1000.0;
			t_point.y = t_point.y / 1000.0;
			t_point.z = t_point.z / 1000.0;

			p_cloud->push_back(t_point);
		}
		std::cout << " huli test :::: " << " p_cloud->size() = " << p_cloud->size() << std::endl;
		point_cloud_map[0] = p_cloud;





		Locate_manager_task locate_manager_task ;
		Common_data::Car_measure_information t_car_information_by_livox;

		//第五步, 创建定位模块的任务单, 并发送到 Locate_manager
		locate_manager_task.task_init(TASK_CREATED,"",NULL,std::chrono::milliseconds(15000),
									  true,"/home/huli/huli/Terminal_project/hl_measure/Terminal_process_CT/data",
									  &cloud_lock,&point_cloud_map, true	);
		t_error = Task_command_manager::get_instance_references().execute_task(&locate_manager_task);

		//第六步, 等待任务单完成
		if ( t_error != Error_code::SUCCESS )
		{
			LOG(INFO) << " Locate_manager  execute_task error ";
		}
		else
		{
			//判断任务是否结束, 阻塞 等待, 直到任务完成或者超时
			while ( locate_manager_task.is_task_end() == false)
			{
				if ( locate_manager_task.is_over_time() )
				{
					//超时处理。取消任务。
					Locate_manager::get_instance_pointer()->cancel_task();
					locate_manager_task.set_task_statu(TASK_DEAD);
					t_error.error_manager_reset(Error_code::LOCATER_MANAGER_TASK_OVER_TIME, Error_level::MINOR_ERROR,
												" locate_manager_task is_over_time ");
					locate_manager_task.set_task_error_manager(t_error);
				}
				else
				{
					//继续等待
					std::this_thread::sleep_for(std::chrono::microseconds(1));
					std::this_thread::yield();
				}

			}
			//提取任务单 的错误码
			t_error = locate_manager_task.get_task_error_manager();
			if ( t_error == Error_code::SUCCESS )
			{
				t_car_information_by_livox.center_x = locate_manager_task.get_task_locate_information_ex()->locate_x;
				t_car_information_by_livox.center_y = locate_manager_task.get_task_locate_information_ex()->locate_y;
				t_car_information_by_livox.car_angle = locate_manager_task.get_task_locate_information_ex()->locate_angle;
				t_car_information_by_livox.car_length = locate_manager_task.get_task_locate_information_ex()->locate_length;
				t_car_information_by_livox.car_width = locate_manager_task.get_task_locate_information_ex()->locate_width;
				t_car_information_by_livox.car_height = locate_manager_task.get_task_locate_information_ex()->locate_height;
				t_car_information_by_livox.wheel_base = locate_manager_task.get_task_locate_information_ex()->locate_wheel_base;
				t_car_information_by_livox.wheel_width = locate_manager_task.get_task_locate_information_ex()->locate_wheel_width;
				t_car_information_by_livox.front_theta = 0;
				t_car_information_by_livox.correctness = true;

				std::cout << " huli test :::: " << " t_car_information_by_livox.center_x = " << t_car_information_by_livox.center_x << std::endl;
				std::cout << " huli test :::: " << " t_car_information_by_livox.center_y = " << t_car_information_by_livox.center_y << std::endl;
				std::cout << " huli test :::: " << " t_car_information_by_livox.car_angle = " << t_car_information_by_livox.car_angle << std::endl;
				std::cout << " huli test :::: " << " t_car_information_by_livox.car_length = " << t_car_information_by_livox.car_length << std::endl;
				std::cout << " huli test :::: " << " t_car_information_by_livox.car_width = " << t_car_information_by_livox.car_width << std::endl;
				std::cout << " huli test :::: " << " t_car_information_by_livox.car_height = " << t_car_information_by_livox.car_height << std::endl;
				std::cout << " huli test :::: " << " t_car_information_by_livox.wheel_base = " << t_car_information_by_livox.wheel_base << std::endl;
				std::cout << " huli test :::: " << " t_car_information_by_livox.wheel_width = " << t_car_information_by_livox.wheel_width << std::endl;
				std::cout << " huli test :::: " << " t_car_information_by_livox.front_theta = " << t_car_information_by_livox.front_theta << std::endl;
				std::cout << " huli test :::: " << " t_car_information_by_livox.correctness = " << t_car_information_by_livox.correctness << std::endl;
			}
			else
			{
				LOG(INFO) << " locate_manager_task error :::::::"  << locate_manager_task.get_task_error_manager().to_string() ;
			}
		}

	}

	System_communication::get_instance_references().communication_uninit();
	System_executor::get_instance_references().system_executor_uninit();
	Locate_manager::get_instance_references().Locate_manager_uninit();
	Laser_manager::get_instance_references().laser_manager_uninit();

	return 0;


	LOG(INFO) << " --- main start --- " ;

*/


	Error_manager t_code;

	std::cout << "huli 101 main start!" << std::endl;

	std::cout << "1111111111111111111111" << std::endl;
	t_code = Laser_manager::get_instance_pointer()->laser_manager_init();
	std::cout << " huli test :::: " << " t_code = " << t_code << std::endl;
//	t_code = Locate_manager::get_instance_pointer()->Locate_manager_init();
//	std::cout << " huli test :::: " << " t_code = " << t_code << std::endl;

	Point_sift_segmentation*                mp_point_sift;
	int point_size = 8192;
	int cls_num = 3;
	double freq = 20.0;
	std::string graph = "../model_param/seg_model_404500.ckpt.meta";
	std::string cpkt = "../model_param/seg_model_404500.ckpt";
	pcl::PointXYZ minp(-10000,-10000,-10000);
	pcl::PointXYZ maxp(10000,10000,10000);



	while(1)
	{




		std::cout << "huli 401 connect_laser Error_code::SUCCESS" << std::endl;

		std::cout << " press to start" << std::endl;
		char ch ;
//		= getchar();

		std::this_thread::sleep_for(std::chrono::seconds(2));
		std::cin >> ch ;

		if ( ch == 'b' )
		{
			std::cout << "  end scan ------------" << std::endl;
			break;
		}
		std::cout << "  start scan ------------" << std::endl;

		int n = 1;
		while(n)
		{
			n--;

			std::map<int,pcl::PointCloud<pcl::PointXYZ>::Ptr>		point_cloud_map;
			std::mutex cloud_lock;
			std::vector<int> select_laser_id_vector;
			select_laser_id_vector.push_back(0);
			select_laser_id_vector.push_back(1);
//			select_laser_id_vector.push_back(2);
//			select_laser_id_vector.push_back(3);


			time_t nowTime;
			nowTime = time(NULL);
			struct tm* sysTime = localtime(&nowTime);
			char t_save_path[256] = { 0 };
			sprintf(t_save_path, "../data/%04d%02d%02d_%02d%02d%02d",
					sysTime->tm_year + 1900, sysTime->tm_mon + 1, sysTime->tm_mday, sysTime->tm_hour, sysTime->tm_min, sysTime->tm_sec);

			Laser_manager_task * laser_manager_task = new Laser_manager_task;
			laser_manager_task->task_init(TASK_CREATED,"",NULL,std::chrono::milliseconds(15000),
										  true,t_save_path,&cloud_lock,&point_cloud_map,false,
										  1000,false,select_laser_id_vector	);



			t_error = Task_command_manager::get_instance_references().execute_task(laser_manager_task);
//			tp_laser_manager->execute_task(laser_manager_task);

			if ( t_error != Error_code::SUCCESS )
			{
				std::cout << "huli Laser_manager:::::" << t_error.to_string() << std::endl;
			}
			else
			{
				while ( laser_manager_task->is_task_end() == false)
				{
					if ( laser_manager_task->is_over_time() )
					{
						//超时处理。取消任务。
						Laser_manager::get_instance_pointer()->cancel_task();
						laser_manager_task->set_task_statu(TASK_DEAD);
						t_error.error_manager_reset(Error_code::LASER_MANAGER_TASK_OVER_TIME, Error_level::MINOR_ERROR,
													" laser_manager_task is_over_time ");
						laser_manager_task->set_task_error_manager(t_error);
					}
					else
					{
						std::this_thread::yield();
					}

				}
				std::cout << "huli task_error::::"  << laser_manager_task->get_task_error_manager().to_string() << std::endl;

			}
			delete(laser_manager_task);


			std::cout << "huli laser result---------------------" << t_error.to_string() << std::endl;
			std::cout << "huli zczxczxczx---------------------" << t_error.to_string() << std::endl;
			std::cout << "huli zczxczxczx---------------------" << t_error.to_string() << std::endl;
			std::cout << "huli zczxczxczx---------------------" << t_error.to_string() << std::endl;



/*
			pcl::PointCloud<pcl::PointXYZ>::Ptr scan_cloud ;
			scan_cloud =  point_cloud_map[0];

			//locate
			pcl::getMinMax3D(*scan_cloud,minp,maxp);
			std::cout << "huli 112" << std::endl;
			mp_point_sift = new Point_sift_segmentation(point_size,cls_num,freq,minp,maxp);
			std::cout << t_error.to_string() << std::endl;

			t_error = mp_point_sift->init(graph,cpkt);
			std::cout << t_error.to_string() << std::endl;

			std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> segmentation_clouds;
			t_error = mp_point_sift->segmentation(scan_cloud, segmentation_clouds, true, "../data/locate_data");
			std::cout << t_error.to_string() << std::endl;
			*/

/*
			Locate_manager_task * locate_manager_task = new Locate_manager_task;
			locate_manager_task->task_init(TASK_CREATED,"",NULL,std::chrono::milliseconds(15000),
										   true,t_save_path,&cloud_lock,&point_cloud_map, false	);
			t_error = Task_command_manager::get_instance_references().execute_task(locate_manager_task);

			if ( t_error != Error_code::SUCCESS )
			{
				std::cout << "huli Locate_manager:::::" << t_error.to_string() << std::endl;
			}
			else
			{
				while ( locate_manager_task->is_task_end() == false)
				{
					if ( locate_manager_task->is_over_time() )
					{
						//超时处理。取消任务。
						Locate_manager::get_instance_pointer()->cancel_task();
						locate_manager_task->set_task_statu(TASK_DEAD);
						t_error.error_manager_reset(Error_code::LOCATER_MANAGER_TASK_OVER_TIME, Error_level::MINOR_ERROR,
													" locate_manager_task is_over_time ");
						locate_manager_task->set_task_error_manager(t_error);
					}
					else
					{
						std::this_thread::yield();
					}

				}

				std::cout << "huli task_error:::::"  << locate_manager_task->get_task_error_manager().to_string() << std::endl;


				std::cout << "locate_x = " << locate_manager_task->get_task_locate_information_ex()->locate_x << std::endl;
				std::cout << "locate_y = " << locate_manager_task->get_task_locate_information_ex()->locate_y << std::endl;
				std::cout << "locate_length = " << locate_manager_task->get_task_locate_information_ex()->locate_length << std::endl;
				std::cout << "locate_width = " << locate_manager_task->get_task_locate_information_ex()->locate_width << std::endl;
				std::cout << "locate_height = " << locate_manager_task->get_task_locate_information_ex()->locate_height << std::endl;
				std::cout << "locate_wheel_base = " << locate_manager_task->get_task_locate_information_ex()->locate_wheel_base << std::endl;
				std::cout << "locate_wheel_width = " << locate_manager_task->get_task_locate_information_ex()->locate_wheel_width << std::endl;
				std::cout << "locate_angle = " << locate_manager_task->get_task_locate_information_ex()->locate_angle << std::endl;
				std::cout << "locate_correct = " << locate_manager_task->get_task_locate_information_ex()->locate_correct << std::endl;


			}
			delete(locate_manager_task);
			std::cout << "huli locate result:::::" << t_error.to_string() << std::endl;
*/
		}

		cout << "huli: 601 :" << t_error.to_string() << endl;
		std::this_thread::sleep_for(std::chrono::seconds(2));

	}

	Laser_manager::get_instance_pointer()->laser_manager_uninit();
	Locate_manager::get_instance_pointer()->Locate_manager_uninit();

	cout << "huli 1234 main end" << endl;
	std::this_thread::sleep_for(std::chrono::seconds(3));


	return 0;
}