//
// Created by zx on 2019/12/6.
//
#include "FenceController.h"

bool FenceController::ReadProtoParam(std::string path)
{
    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, &m_global_param);
    delete input;
    close(fd);
    return success;
}

FenceController::FenceController(std::string path):
m_initialized(0)
{
    p_merged_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
    m_initialized = ReadProtoParam(path);
    const int lidar_size = m_global_param.lidar_params_size();
    const int region_size = m_global_param.regions_size();
    p_lidars.resize(lidar_size);
    p_region_workers.resize(region_size);
    p_plc_data = PlcData::GetInstance("192.168.0.1");
    // init lidar instances
    for (int i = 0; i < m_global_param.lidar_params_size(); ++i) {
        p_lidars[i] = new Lidar(m_global_param.lidar_params(i), m_handle);
    }
    // init region_detector instances
    for (int j = 0; j < m_global_param.regions_size(); ++j) {
        p_region_workers[j] = new Region_worker(j, m_global_param.regions(j));
    }
    m_cond_exit.Notify(false);
    m_update_thread = new std::thread(cloudMergeUpdate, this);
//    while(m_initialized){
//        cloudMergeUpdate(this);
//        usleep(1000*50);
//    }
}

FenceController::~FenceController()
{
    m_cond_exit.Notify(true);
    if(m_update_thread){
        if(m_update_thread->joinable()){
            m_update_thread->join();
        }
        delete m_update_thread;
        m_update_thread = 0;
    }
    // delete lidar instances
    for (int i = 0; i < m_global_param.lidar_params_size(); ++i) {
        delete p_lidars[i];
    }
    // delete region_detector instances
    for (int j = 0; j < m_global_param.regions_size(); ++j) {
        delete p_region_workers[j];
    }
    PlcData::Release();
}
void FenceController::GetCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out){
    m_cloud_mutex.lock();
    cloud_out->clear();
    cloud_out->operator+=(*p_merged_cloud);
    m_cloud_mutex.unlock();
}

void FenceController::cloudMergeUpdate(FenceController* fc)
{
    if(fc==0) return;
    while(!fc->m_cond_exit.WaitFor(1)) {
        fc->m_cloud_mutex.lock();
        fc->p_merged_cloud->clear();
        for (int i = 0; i < fc->p_lidars.size(); ++i) {
            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
            if (fc->p_lidars[i]->GetCloud(cloud)) {
                fc->p_merged_cloud->operator+=(*cloud);
            }
        }
        for (int i = 0; i < fc->p_region_workers.size(); ++i) {
            fc->p_region_workers[i]->update_cloud(fc->p_merged_cloud);
        }
        fc->m_cloud_mutex.unlock();
    }
}