#include "../wj_lidar/wj_lidar_encapsulation.h"
// #include <pcl/visualization/cloud_viewer.h>
#include <fstream>
#include <sys/types.h>
#include <sys/stat.h>
#include <stdint.h>

#include <stdio.h>
#include <string.h>
#include <unistd.h>

void save_cloud_txt(std::string txt, pcl::PointCloud<pcl::PointXYZ>::Ptr pCloud)
{
    std::ofstream os;
    os.open(txt, std::ios::out);
    for (int i = 0; i < pCloud->points.size(); i++)
    {
        pcl::PointXYZ point = pCloud->points[i];
        char buf[255];
        memset(buf, 0, 255);
        sprintf(buf, "%f %f %f\n", point.x, point.y, point.z);
        os.write(buf, strlen(buf));
    }
    os.close();
}

bool ReadProtoParam(std::string path, wj::wjLidarParams &params)
{
    int fd = open(path.c_str(), O_RDONLY);
    if (fd == -1)
        return false;
    FileInputStream *input = new FileInputStream(fd);
    bool success = google::protobuf::TextFormat::Parse(input, &params);
    // std::cout<<m_global_param.data_path()<<std::endl;
    delete input;
    close(fd);
    return success;
}

bool ReadProtoParam(std::string path, wj::wjManagerParams &params)
{
    int fd = open(path.c_str(), O_RDONLY);
    if (fd == -1)
        return false;
    FileInputStream *input = new FileInputStream(fd);
    bool success = google::protobuf::TextFormat::Parse(input, &params);
    // std::cout<<m_global_param.data_path()<<std::endl;
    delete input;
    close(fd);
    return success;
}

void lidar_test()
{
    Wj_lidar_encapsulation *lidar = new Wj_lidar_encapsulation();
    wj::wjLidarParams params;
    bool result = ReadProtoParam("../src/test/wj_lidar_settings.prototxt", params);
    LOG(INFO) << "读配置结果" << result;
    // params.mutable_net_config()->set_ip_address("192.168.0.2");
    // params.mutable_net_config()->set_port(2110);
    if (lidar != 0)
    {
        Error_manager code = lidar->initialize(params);
        LOG(INFO) << code.to_string();
        // usleep(1000 * 1);
        if (code == SUCCESS)
        {
            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
            for (int i = 0; i < 10; i++)
            {
                LOG(INFO) << "connect status: " << lidar->get_connection_status();
                code = lidar->get_cloud(cloud, std::chrono::steady_clock::now(), 3000);
                if (code == SUCCESS)
                {
                    char temp[100];
                    memset(temp, 0, 100);
                    sprintf(temp, "cloud_%d.txt", i);
                    if (cloud->size() > 0)
                    {
                        save_cloud_txt(temp, cloud);
                        LOG(INFO) << "cloud saved to " << std::string(temp);
                    }
                }
                else
                {
                    LOG(WARNING) << code.to_string();
                }
                usleep(1000 * 30);
                LOG(INFO) << "end of cycle";
            }
        }
    }
    delete lidar;
}

int main(int argc, char *argv[])
{
    lidar_test();
    usleep(1000 * 1000);

    // lidar_test();
    // usleep(1000 * 1000);

    // lidar_test();
    // usleep(1000 * 1000);

    LOG(INFO) << "end";
    getchar();
    return 0;
}