//
// Created by zx on 2019/12/6.
//
#include <iostream>
#include <string>
#include <fstream>
#include <string.h>
#include <thread>
#include <unistd.h>

#include <google/protobuf/io/coded_stream.h>
#include <google/protobuf/io/zero_copy_stream_impl.h>
#include <google/protobuf/text_format.h>
using google::protobuf::io::FileInputStream;
using google::protobuf::io::FileOutputStream;
using google::protobuf::io::ZeroCopyInputStream;
using google::protobuf::io::CodedInputStream;
using google::protobuf::io::ZeroCopyOutputStream;
using google::protobuf::io::CodedOutputStream;
using google::protobuf::Message;

#include <ros/ros.h>

#include "EleFence.pb.h"
#include "globalmsg.pb.h"

#include "LogFiles.h"
#include "Lidar.h"
#include "region_worker.h"
#include "unistd.h"
#include "s7_plc.h"
#include "PlcData.h"

#include <nnxx/message.h>
#include <nnxx/message_control.h>
#include <nnxx/socket.h>
#include <nnxx/reqrep.h>
//#include <nnxx/unittest.h>
#include <nnxx/timeout.h>
#include <nnxx/error.h>
#include <cstring>
#include <nnxx/message>

#include "TaskQueue/TQFactory.h"
#include "wheelCalcTask.h"

#ifndef FENCECONTROLLER_H
#define FENCECONTROLLER_H

#define CONNECTSTRING "tcp://127.0.0.1:9000"

class FenceController{
public:
    FenceController(std::string path);
    ~FenceController();
    // 点云拼接线程
    static void cloudMergeUpdate(FenceController* fc);
    // 外部获取点云
    void GetCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out);
    // 中心轮距指令接收线程
    static void wheelMsgHandlingThread(FenceController* fc);
    // 获取轮距回调
    static void sendWheelResultCallback(int termID, nnxx::message_control* ctl, void *fc);

private:
    bool ReadProtoParam(std::string path);

public:
    bool                                    m_initialized;
    ros::NodeHandle                         m_handle;
    std::mutex                              m_cloud_mutex;
    // wheel msg rep
    nnxx::socket                            m_wheel_soc{nnxx::SP_RAW, nnxx::REP};
    // wheel handling lock
    std::mutex                              m_mutex_wheel_handling;
private:
    // 全局系统配置
    EleFence::globalParam                   m_global_param;

    // 雷达与区域句柄
    std::vector<Lidar*>                     p_lidars;
    std::vector<Region_worker*>             p_region_workers;
    // plc 句柄
    PlcData*                                p_plc_data;
    // 更新线程
    StdCondition                            m_cond_exit;
    std::thread*                            m_update_thread;
    // 拼接后点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr     p_merged_cloud;

    // 中心轮距相关变量
    std::thread*                            m_wheel_recv_thread;
    StdCondition                            m_cond_wheel_exit;
    // wheel calculate queue
    tq::IQueue                              *m_wheel_calc_queue;

};

#endif //FENCECONTROLLER_H
