//
// 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){
        if(m_update_thread->joinable())
            m_update_thread->join();
        delete m_update_thread;
    }
    if(m_detector){
        delete m_detector;
    }
}

int Region_worker::get_id(){
    if(m_detector){
        return m_detector->m_region_id;
    }else{
        return -1;
    }
}

bool Region_worker::get_wheel_result(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in, double &x, double &y, double &c, double &wheelbase, double &width){
    if(m_detector){
        // std::cout<<"worker getting result"<<std::endl;
        return m_detector->detect(cloud_in, x, y, c, wheelbase, width);
    }else{
        return false;
    }
}

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)){
        stateCode code = stateCode::eNoCar;
        worker->m_mutex.lock();
        if(worker->b_cloud_updated){
            bool result = worker->m_detector->detect(worker->m_cloud);
            worker->b_cloud_updated = false;
            if(result){
                code = stateCode ::eOk;
            }else{
                code = stateCode ::eOutOfPlace;
            }
        }
        PlcData* p = PlcData::GetInstance();
        if(p) {
            p->UpdateData(code, worker->m_detector->m_region_id);
        }
        worker->m_mutex.unlock();
    }
}