#include <QCoreApplication>


#include "modulecomm.h"

#include "input.h"

#include "rsdriver.h"
#include "convert.h"

#include <QMutex>
#include <QWaitCondition>
#include <thread>

volatile sig_atomic_t flag = 1;


std::shared_ptr<rs_driver::lidarscan>  grawscan;
QMutex gMutexScan;
bool gbUpdate = false;

QMutex gMutexWait;
QWaitCondition gWait;

rslidar_pointcloud::Convert * gpConvert;

void * g_lidar_pc;

std::thread * gthreadpoll,*gthreadconvert;


void PublishPointCloud(std::shared_ptr<rs_driver::lidarscan>  rawscan)
{
    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud;
    gpConvert->processScan(rawscan,point_cloud);

    char * strOut = new char[4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->width * sizeof(pcl::PointXYZI)];

    int * pHeadSize = (int *)strOut;
    *pHeadSize = 4 + point_cloud->header.frame_id.size()+4+8;
    memcpy(strOut+4,point_cloud->header.frame_id.c_str(),point_cloud->header.frame_id.size());
    pcl::uint32_t * pu32 = (pcl::uint32_t *)(strOut+4+sizeof(point_cloud->header.frame_id.size()));
    *pu32 = point_cloud->header.seq;
    memcpy(strOut+4+sizeof(point_cloud->header.frame_id.size()) + 4,&point_cloud->header.stamp,8);
    pcl::PointXYZI * p;
    p = (pcl::PointXYZI *)(strOut +4+sizeof(point_cloud->header.frame_id.size()) + 4+8 );
    memcpy(p,point_cloud->points.data(),point_cloud->width * sizeof(pcl::PointXYZI));

    iv::modulecomm::ModuleSendMsg(g_lidar_pc,strOut,4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->width * sizeof(pcl::PointXYZI));
    delete strOut;
}

void pollthread()
{
    rs_driver::rslidarDriver lidarm1;
    while(true)
    {
        std::shared_ptr<rs_driver::lidarscan>  rawscan;
        if(true == lidarm1.poll(rawscan))
        {
            gMutexScan.lock();
            grawscan = rawscan;
            gbUpdate = true;
            gMutexScan.unlock();
            gWait.wakeAll();

        }
    }
}

void convertthread()
{
    while(true)
    {
        gMutexWait.lock();
        gWait.wait(&gMutexWait,300);
        gMutexWait.unlock();
        if(gbUpdate)
        {
            gMutexScan.lock();
            std::shared_ptr<rs_driver::lidarscan>  rawscan;
            rawscan = grawscan;
            gbUpdate = false;
            gMutexScan.unlock();
            PublishPointCloud(rawscan);
        }
    }
}

int main(int argc, char *argv[])
{
    QCoreApplication a(argc, argv);

    g_lidar_pc = iv::modulecomm::RegisterSend("lidar_pc",20000000,1);

    gpConvert = new rslidar_pointcloud::Convert("ChannelNum.csv","limit.csv");

    gthreadpoll = new std::thread(pollthread);
    gthreadconvert = new std::thread(convertthread);

    return a.exec();
}