//
// Created by zx on 2019/12/28.
//
#include <iostream>

#include "./laser/Laser.h"
#include "./laser/LivoxLaser.h"
#include "./error_code/error_code.h"
#include "LogFiles.h"
#include <string.h>
#include <fcntl.h>
#include <google/protobuf/io/zero_copy_stream_impl.h>
#include <google/protobuf/text_format.h>
//#include </usr/local/include/google/protobuf/io/zero_copy_stream_impl.h>
using google::protobuf::io::FileInputStream;
using google::protobuf::io::FileOutputStream;
using google::protobuf::io::ZeroCopyInputStream;
using google::protobuf::io::CodedInputStream;
using google::protobuf::io::ZeroCopyOutputStream;
using google::protobuf::io::CodedOutputStream;
using google::protobuf::Message;

#define O_RDONLY	     00
//读取protobuf 配置文件
//file:文件
//parameter:要读取的配置
bool read_proto_param(std::string file, ::google::protobuf::Message& parameter)
{
	int fd = open(file.c_str(), O_RDONLY);
	if (fd == -1) return false;
	FileInputStream* input = new FileInputStream(fd);
	bool success = google::protobuf::TextFormat::Parse(input, &parameter);
	delete input;
	close(fd);
	return success;
}



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


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

	Error_manager err;

	Laser_base *m_laser_vector;

//	Laser_proto::Laser_parameter_all laser_parameters;
/*	if (!read_proto_param("./setting/laser.prototxt", laser_parameters))
	{
		err.error_manager_reset(SYSTEM_READ_PARAMETER_ERROR, NORMAL, "read laser parameter failed");
		cout << err.to_string() << endl;
		return err.get_error_code();
	}
*/


	Laser_proto::laser_parameter laser_param;
	laser_param.set_frame_num(1000);
	laser_param.set_type("Livox");
	laser_param.set_sn("0TFDFG700601881");

	int i = 0;
	m_laser_vector = LaserRegistory::CreateLaser(laser_param.type(), i,
												 laser_param);
	if (!Start()) {
		Uninit();
		printf("Livox-SDK init fail!\n");
	}
	else
	{
		std::cout << "huli 201 Livox-SDK start" << std::endl;
	}

	if (m_laser_vector != NULL)
	{
		err = m_laser_vector->connect_laser();
		if ( err != Error_code::SUCCESS)
		{
			char description[255] = {0};
			sprintf(description, "Laser %d connect failed...", i);
			err.error_manager_reset(LASER_CONNECT_FAILED, NORMAL, description);

		}
		else
		{
		    std::cout << "huli 301 connect_laser Error_code::SUCCESS" << std::endl;
		}

		cout << "huli 311 " << err.to_string() << endl;

		pcl::PointCloud<pcl::PointXYZ>::Ptr scan_cloud(new pcl::PointCloud<pcl::PointXYZ>);
		std::mutex cloud_lock;

		Laser_task *laser_task = new Laser_task();
		//
		laser_task->task_init(TASK_CREATED, scan_cloud, &cloud_lock);
		laser_task->set_task_frame_maxnum(100);
		laser_task->set_task_save_path("123");

		//发送任务单给雷达
		std::this_thread::sleep_for(std::chrono::seconds(5));

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

		err = m_laser_vector->execute_task(laser_task);
		cout << "huli: 501 :" << err.to_string() << endl;
		cout << "huli: 501 :" << err.to_string() << endl;
	}
	else
	{
		cout << "huli: 601 :" << err.to_string() << endl;

	}


	std::this_thread::sleep_for(std::chrono::seconds(5));
	cout << "huli: 701 :" << err.to_string() << endl;

	m_laser_vector->disconnect_laser();
	cout << "huli: 801 :" << err.to_string() << endl;

	m_laser_vector->close_save_path();
	cout << "huli: 901 :" << err.to_string() << endl;

	std::this_thread::sleep_for(std::chrono::seconds(2));

	Uninit();

	cout << "huli 1234 main end" << endl;
}