//
// 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);
    // std::cout<<m_global_param.data_path()<<std::endl;
    delete input;
    close(fd);
    return success;
}

void FenceController::save_cloud_txt(std::string txt, pcl::PointCloud<pcl::PointXYZ>::Ptr pCloud)
{
    std::ofstream os;
    os.open(txt, std::ios::out);
    for (int i = 0; i < pCloud->points.size(); i++)
    {
        pcl::PointXYZ point = pCloud->points[i];
        char buf[255];
        memset(buf, 0, 255);
        sprintf(buf, "%f %f %f\n", point.x, point.y, point.z);
        os.write(buf, strlen(buf));
    }
    os.close();
}

FenceController::FenceController(std::string path):
m_initialized(0)
{
    // char buf[100];
    // getcwd(buf, sizeof(buf));
    //printf("%s\n",buf);
    //FLAGS_log_dir = buf;
    // google::InitGoogleLogging("path");
    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);

    if(m_wheel_soc){
        m_wheel_soc.bind(CONNECTSTRING);
    }
    m_wheel_recv_thread = new std::thread(wheelMsgRecvThread, this);
    m_wheel_send_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_send_thread){
        if(m_wheel_send_thread->joinable()){
            m_wheel_send_thread->join();
        }
        delete m_wheel_send_thread;
        m_wheel_send_thread = 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();
    // google::ShutdownGoogleLogging();
}
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::wheelMsgRecvThread(FenceController* fc)
{
    if(fc==0) return;
    while(!fc->m_cond_wheel_exit.WaitFor(1)){
        if(fc->m_wheel_soc){
            nnxx::message_control* c1=new nnxx::message_control();
            fc->m_mutex_wheel_handling.lock();
            auto m1 = fc->m_wheel_soc.recv(1, *c1);
            fc->m_mutex_wheel_handling.unlock();
            if(!m1.empty() ){

                MsgTask task;
                task.cmd=nnxx::to_string(m1);
                task.sock_controller=c1;
                fc->m_msg_queue.push(task);

                std::cout<<"-------recv msg -------"<<task.cmd<<std::endl;
            }
        }
        // std::cout<<"++++++++++++++++"<<std::endl;
    }
}

void FenceController::wheelMsgHandlingThread(FenceController* fc){
    if(fc==0) return;

    while(!fc->m_cond_wheel_exit.WaitFor(1))
    {
        int count = fc->m_retry_count >0 ? fc->m_retry_count:1;
        MsgTask task;
        if(fc->m_msg_queue.try_pop(task))
        {
            globalmsg::resultInfo result;
            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);
            result.set_correctness(false);

            if(task.cmd.size()>8 && task.cmd.substr(0, 8) == "Terminal")
            {
                int terminal_id = atoi(task.cmd.substr(8, task.cmd.length() - 8).c_str());
                std::cout << "terminal id: " << terminal_id << std::endl;
                while (count-- >= 0 && !result.correctness())
                {
                    std::cout<<" times :"<<count<<std::endl;
                    for (int i = 0; i < fc->p_region_workers.size(); ++i)
                    {
                        if (terminal_id >= 0 && fc->p_region_workers[i]->get_id() == terminal_id)
                        {
                             std::cout << "--------callback find terminal------" << terminal_id << std::endl;
                            double x = 0, y = 0, c = 0, wheelbase = 0, width = 0;
                            // 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);
                                // save cloud txt
                                if (fc->m_global_param.data_path() != "")
                                {
                                    char cloud_txt_filename[255];
                                    memset(cloud_txt_filename, 0, 255);
                                    sprintf(cloud_txt_filename, "%s//%s.txt", fc->m_global_param.data_path().c_str(), result.time().c_str());
                                    fc->save_cloud_txt(cloud_txt_filename, total_cloud);
                                    std::cout << " save cloud   " << cloud_txt_filename << std::endl;
                                }
                            }
                            else
                            {
                                result.set_correctness(false);
                            }
                             std::cout << "--------callback get result------" << std::endl;
                            result.set_x(x * 1000.0);
                            result.set_y(y * 1000.0);
                            result.set_c(c);
                            result.set_wheel_base(wheelbase * 1000.0);
                            result.set_width(width * 1000.0);
                            if(result.correctness())
                                break;
                        }
                    }
                }
            }
            std::string result_str = result.SerializeAsString();
            std::cout << "--------callback serialized: " <<std::endl<< result.DebugString() << "------" << std::endl;
            fc->m_mutex_wheel_handling.lock();
            if (fc->m_wheel_soc) {
                fc->m_wheel_soc.send(result_str.data(), result_str.length(), 0,
                                     std::move(*task.sock_controller));
                std::cout << "------- callback msg sent -------" << std::endl;
            }
            fc->m_mutex_wheel_handling.unlock();
            delete task.sock_controller;
        }

        // std::cout<<"++++++++++++++++"<<std::endl;

    }
}