//
// 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 wheel handling thread and queue
    m_cond_wheel_exit.Notify(false);
    m_wheel_calc_queue = tq::TQFactory::CreateDefaultQueue();
    m_wheel_calc_queue->Start(3);
    if(m_wheel_soc){
        m_wheel_soc.bind(CONNECTSTRING);
    }
    m_wheel_recv_thread = new std::thread(wheelMsgHandlingThread, this);

    // 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);
    m_cond_wheel_exit.Notify(true);
    if(m_update_thread){
        if(m_update_thread->joinable()){
            m_update_thread->join();
        }
        delete m_update_thread;
        m_update_thread = 0;
    }
    std::cout<<"exit 00"<<std::endl;
    if(m_wheel_recv_thread){
        if(m_wheel_recv_thread->joinable()){
            m_wheel_recv_thread->join();
        }
        delete m_wheel_recv_thread;
        m_wheel_recv_thread = 0;
    }
    std::cout<<"exit 01"<<std::endl;
    if (m_wheel_calc_queue)
    {
        m_wheel_calc_queue->CancelAll();
        m_wheel_calc_queue->Stop();
        delete m_wheel_calc_queue;
        m_wheel_calc_queue = 0;
    }
    std::cout<<"exit 02"<<std::endl;
    // 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];
    }
    std::cout<<"exit 03"<<std::endl;
    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);
            }
        }
        if(fc->p_merged_cloud->size() > 0) {
            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();
    }
}

void FenceController::wheelMsgHandlingThread(FenceController* fc){
    if(fc==0) return;
    while(!fc->m_cond_wheel_exit.WaitFor(1)){
        fc->m_mutex_wheel_handling.lock();
        if(fc->m_wheel_soc){
            nnxx::message_control* c1 = new nnxx::message_control();
            auto m1 = fc->m_wheel_soc.recv(1, *c1);
            if(!m1.empty() && fc->m_wheel_calc_queue){
                std::cout<<"-------not empty-------"<<std::endl;
                tq::BaseTask* task = new WheelCalcTask(nnxx::to_string(m1), c1, fc->sendWheelResultCallback, (void *)fc);
                fc->m_wheel_calc_queue->AddTask(task);
                // std::cout<<"-------task added-------"<<std::endl;
            } 
        }
        // std::cout<<"++++++++++++++++"<<std::endl;
        fc->m_mutex_wheel_handling.unlock();
    }
}

// termID start from 0
void FenceController::sendWheelResultCallback(int termID, nnxx::message_control* ctl, void *p){
    std::cout<<"--------callback start------"<<std::endl;
    FenceController * fc = ((FenceController *)p);
    fc->m_mutex_wheel_handling.lock();
    std::cout<<"--------callback start2------"<<std::endl;
    for (int i = 0; i < fc->p_region_workers.size(); ++i) {
        if(termID>=0 && fc->p_region_workers[i]->get_id() == termID){
            std::cout<<"--------callback find terminal------"<<std::endl;
            double x=0,y=0,c=0,wheelbase=0, width=0;
            globalmsg::resultInfo* result = new globalmsg::resultInfo();
            time_t tt = time(0);
            struct tm *tc = localtime(&tt);
            char buf[255] = {0};
            memset(buf, 0, 255);
            sprintf(buf, "%d-%02d-%02d %02d:%02d:%02d ", tc->tm_year + 1900,
                    tc->tm_mon + 1, tc->tm_mday, tc->tm_hour, tc->tm_min, tc->tm_sec);
            result->set_time(buf);
            std::cout<<"--------callback get time------"<<std::endl;
            fc->m_cloud_mutex.lock();
            pcl::PointCloud<pcl::PointXYZ>::Ptr total_cloud(new pcl::PointCloud<pcl::PointXYZ>);
            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)) {
                    total_cloud->operator+=(*cloud);
                }
            }
            fc->m_cloud_mutex.unlock();
            std::cout<<"--------callback get cloud------"<<std::endl;
            if(total_cloud->size() > 0 && fc->p_region_workers[i]->get_wheel_result(total_cloud, x, y, c, wheelbase, width)){
                result->set_correctness(true);
            }else{
                result->set_correctness(false);
            }
            std::cout<<"--------callback get result------"<<std::endl;
            result->set_x(x);
            result->set_y(y);
            result->set_c(c);
            result->set_wheel_base(wheelbase);
            result->set_width(width);
            std::cout<<"--------callback set result------"<<std::endl;

            // std::cout<<"--------callback serializing------"<<std::endl;
            std::string result_str = result->SerializeAsString();
            std::cout<<"--------callback serialized------"<<std::endl;
            nnxx::message_ostream s;
            s << result_str;
            std::cout<<"--------callback handle message------"<<std::endl;
            nnxx::message m = s.move_msg();
            
            if (fc->m_wheel_soc)
            {
                fc->m_wheel_soc.send(std::move(m), 0, std::move(*ctl));
                std::cout<<"------- callback msg sent -------"<<std::endl;
            }
            if(ctl){
                delete ctl;
            }
        }
    }
    std::cout<<"--------callback end------"<<std::endl;
    fc->m_mutex_wheel_handling.unlock();
}