//
// Created by zx on 2020/6/18.
//
#include <pcl/visualization/pcl_visualizer.h> 

#include <iostream>
#include "./error_code/error_code.h"
#include "./wanji_lidar/LogFiles.h"
#include <glog/logging.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"
#include "./velodyne_lidar/velodyne_manager.h"

#define LIVOX_NUMBER	     2
// #define POINT_DEBUG

#ifdef POINT_DEBUG
std::shared_ptr<pcl::visualization::PCLVisualizer> gp_visualizer;
#endif

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

}

void test_whole_process()
{
	System_executor::get_instance_references().execute_for_measure("key_for_test_only", 0, std::chrono::system_clock::now());
}

int main(int argc,char* argv[])
{
		std::cout << " measure：： " << " xxxxxxxxxxxxxxxxx " << std::endl;


	Error_manager t_error;
	Error_manager t_result ;
    Error_manager ec;

#ifdef POINT_DEBUG
	pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::visualization::PCLVisualizer viewer("Viewer");      //创建viewer对象
	gp_visualizer = std::shared_ptr<pcl::visualization::PCLVisualizer>(&viewer);
	gp_visualizer->addCoordinateSystem();
	gp_visualizer->setBackgroundColor(0, 0, 0);
	gp_visualizer->initCameraParameters();
	gp_visualizer->addPointCloud(t_cloud, "region");
#endif

	const char* logPath = "./log/";
	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;


//huli test
//    ec = System_communication_mq::get_instance_references().rabbitmq_init();
//    if(ec != SUCCESS)
//    {
//        LOG(ERROR) << "system communication mq init failed: " << ec.to_string();
//        return -1;
//    }
//
//    while (true)
//    {}
//
//    return 0;


	// std::cout << " huli test :::: " << " wanji_manager_init = " << 1 << std::endl;

	// 定义服务的终端id
	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;

//	Error_manager ec;

	// 初始化
	if(WJ_VELO == 0 || WJ_VELO == 2)
	{
		Wanji_manager::get_instance_references().wanji_manager_init(t_terminal_id);
		std::cout << "wanji_manager = " << Wanji_manager::get_instance_references().check_status().to_string() << std::endl;
		if (ec != SUCCESS)
		{
			LOG(ERROR) << "wanji_manager init failed: " << ec.to_string();
			return -1;
		}
	}
	if(WJ_VELO == 1 || WJ_VELO == 2)
	{
		ec = Velodyne_manager::get_instance_references().velodyne_manager_init(t_terminal_id);

		if (ec != SUCCESS)
		{
            std::cout << "veodyne_manager = " << ec.to_string() << std::endl;
            std::cout << "veodyne_manager = " << Velodyne_manager::get_instance_references().check_status().to_string() << std::endl;
			LOG(ERROR) << "velodyne_manager init failed: " << ec.to_string();
			return -1;
		}
	}

	ec = 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;
	if(ec != SUCCESS)
	{
		LOG(ERROR) << "system executor init failed: " << ec.to_string();
		return -1;
	}

	// usleep(1000 * 500);

	ec = System_communication::get_instance_references().communication_init();
	if(ec != SUCCESS)
	{
		LOG(ERROR) << "system communication init failed: " << ec.to_string();
		return -1;
	}
	System_communication::get_instance_references().set_encapsulate_cycle_time(110);


	ec = System_communication_mq::get_instance_references().rabbitmq_init();
	if(ec != SUCCESS)
	{
		LOG(ERROR) << "system communication mq init failed: " << ec.to_string();
		return -1;
	}
	System_communication_mq::get_instance_references().set_encapsulate_status_cycle_time(100);

	// prev_test_pred_task();
	// test_whole_process();
//    usleep(1000*5000);
    char ch='x' ;
	pcl::PointCloud<pcl::PointXYZ>::Ptr t_region_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
    while(ch != 'q') {
       
#ifdef POINT_DEBUG
		std::map<int, Ground_region *> t_ground_region_map = Velodyne_manager::get_instance_references().get_ground_region_map();
		for (auto iter = t_ground_region_map.begin(); iter != t_ground_region_map.end(); ++iter)
		{
			if (iter->first == DISP_TERM_ID)
			{
				t_region_cloud->clear();
				// 获取区域点云
				iter->second->get_region_cloud(t_region_cloud, Ground_region::Region_cloud_type::filtered);
				// gp_visualizer->showCloud(t_region_cloud);
				gp_visualizer->updatePointCloud(t_region_cloud, "region");
				// LOG(INFO) << "pcl cloud updated." <<t_region_cloud->size();
			}
		}
		gp_visualizer->spinOnce();
		usleep(100);
#else
		std::cout << "please input q to quit system." << std::endl;
		std::cin >> ch;
#endif
	}

	// 反初始化
	System_communication::get_instance_references().communication_uninit();
	System_communication_mq::get_instance_references().rabbitmq_uninit();

	if(WJ_VELO == 1 || WJ_VELO == 2)
		Velodyne_manager::get_instance_references().Velodyne_manager_uninit();
	System_executor::get_instance_references().system_executor_uninit();
	if(WJ_VELO == 0 || WJ_VELO == 2)
		Wanji_manager::get_instance_references().wanji_manager_uninit();

	return 0;
}