//
// Created by zx on 2019/12/9.
//
#include "region_worker.h"
#include "PlcData.h"

Region_worker::Region_worker(int id, EleFence::Region region){
    m_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
    m_detector = new Region_detector(id, region);
    m_cond_exit.Notify(false);
    m_update_thread = new std::thread(detect_loop, this);
    m_update_thread->detach();
}

Region_worker::~Region_worker()
{
    m_cond_exit.Notify(true);
    if(m_update_thread){
        delete m_update_thread;
    }
    if(m_detector){
        delete m_detector;
    }
}

void Region_worker::update_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in)
{
    m_mutex.lock();
    m_cloud->clear();
    for (int i = 0; i < cloud_in->size(); ++i) {
        m_cloud->push_back(cloud_in->points[i]);
    }
    b_cloud_updated = true;
    m_mutex.unlock();
}

void Region_worker::detect_loop(Region_worker *worker)
{
    if(worker == 0) return;
    if(worker->m_detector == 0) return;
    while(!worker->m_cond_exit.WaitFor(1)){
        short *p_data;
        p_data = (short *)malloc(sizeof(short));
        memset(p_data, 0, sizeof(short));
        worker->m_mutex.lock();
        if(worker->b_cloud_updated){
            bool result = worker->m_detector->detect(worker->m_cloud);
            worker->b_cloud_updated = false;
            *p_data = result? (short)1: (short)2;
        }
        PlcData* p = PlcData::GetInstance();
        if(p) {
            p->UpdateData(p_data, worker->m_detector->m_region_id);
        }
        free(p_data);
        worker->m_mutex.unlock();
    }
}