/*
 * @Author: yct 18202736439@163.com
 * @Date: 2023-01-12 09:17:46
 * @LastEditors: yct 18202736439@163.com
 * @LastEditTime: 2023-01-12 15:12:49
 * @FilePath: /puai_wj_2021/tests/rslidar_test.cpp
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include <iostream>
#include <fstream>
#include <glog/logging.h>

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

}

// #include "../rslidar/rslidar_driver.h"
#include "../velodyne_lidar/rs_device_bridge.h"
void bridge_test()
{
    velodyne::velodyneLidarParams params;
    params.set_ip("");
    params.set_port(6699);
    params.set_model("HELIOS16P");
    params.set_calibrationfile("/home/youchen/extra_space/chutian/measure/puai_wj_2021/setting/VLP16db.yaml");

    RS_device_bridge t_device;
    auto ec = t_device.init(params);

	if(ec != SUCCESS)
	{
		std::cout<<"init failed. "<< ec.to_string()<<std::endl;
	}

    usleep(1000 * 1000);

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

	for (size_t j = 1; j <= 300; j++)
	{
		p_cloud->clear();
		Error_manager ec = t_device.get_new_cloud_and_wait(&p_mutex, p_cloud, std::chrono::steady_clock::now(), 1000);

		if (ec == SUCCESS)
		{
			if(j % 80 == 0)
			{
				std::ofstream out((std::string("./device_point") + std::to_string(j) + std::string(".txt")).c_str());
				if (out.is_open())
				{
					for (size_t i = 0; i < p_cloud->size(); i++)
					{
						char buf[255] = {0};
						sprintf(buf, "%.3f %.3f %.3f\n", p_cloud->points[i].x, p_cloud->points[i].y, p_cloud->points[i].z);
						out << buf;
					}
					std::cout << "cloud written" << std::endl;
					out.close();
				}
				else
				{
					std::cout << "cannot open file to write cloud." << std::endl;
				}
			}
		}
		else
		{
			std::cout << "get cloud failed: "<< ec.to_string() << std::endl;
		}
	}

	t_device.uninit();
}

int main()
{
    	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;

	// device_test();
	bridge_test();

    return 0;
}