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

#include <iostream>
#include "./error_code/error_code.h"
#include "./laser/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"

#define LIVOX_NUMBER	     2


void asd(int i)
{
	std::cout << " i = " << i << std::endl;
}

int main(int argc,char* argv[])
{
	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);
	std::cout << "System_executor = " << System_executor::get_instance_references().get_system_executor_status() << std::endl;

	System_communication::get_instance_references().communication_init();

	char ch ;
	std::cin >> ch ;

	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;

	Thread_pool pool(4);
	std::vector< std::future<int> > results;

//	for(int i = 0; i < 8; ++i) {
//		results.emplace_back(
//		pool.enqueue([i] {
//			std::cout << "hello " << i << std::endl;
//			std::this_thread::sleep_for(std::chrono::seconds(1));
//			std::cout << "world " << i << std::endl;
//			return i*i;
//		})
//		);
//	}

	pool.enqueue(&asd,8);

	for(auto && result: results)
		std::cout << result.get() << ' ';
	std::cout << std::endl;
	return 0;



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


	Error_manager t_error;
	Error_manager t_result ;



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

	std::cout << "1111111111111111111111" << std::endl;
	Laser_manager::get_instance_pointer()->laser_manager_init();
	Locate_manager::get_instance_pointer()->Locate_manager_init();

	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);


			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,true
			);
			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::LOCATE_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;

			}
			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;
}