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

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

//#define LIVOX_NUMBER         2
// #define POINT_DEBUG

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

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;

    ZX::InitGlog("MeasureNode", ETC_PATH"/etc/MeasureNodeLog/");

    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

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

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

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

    LOG(INFO) << "begain init rabbitmq.";
    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);

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