#include "ivdriver_lidar.h"

#include <iostream>

namespace  iv {
ivdriver_lidar::ivdriver_lidar()
{
    strncpy(mstr_hostip,"0.0.0.0",256);
    strncpy(mstr_port,"6699",256);
    strncpy(mstr_memname,"lidar_pc",256);
}

void ivdriver_lidar::RegisterPointMsg(const char * strmemname,iv::modulecomm::ModuleComm_TYPE xtype)
{
    mpa = iv::modulecomm::RegisterSend(strmemname,10000000,1,xtype);
}



void ivdriver_lidar::SharePointCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr 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(mpa,strOut,4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->width * sizeof(pcl::PointXYZI));
    delete strOut;
}


void ivdriver_lidar::modulerun()
{

    if(mfinclinationang_xaxis != 0.0)mbinclix = true;
    if(mfinclinationang_yaxis != 0.0)mbincliy = true;
    mcos_finclinationang_xaxis= cos(mfinclinationang_xaxis);
    msin_finclinationang_xaxis = sin(mfinclinationang_xaxis);
    mcos_finclinationang_yaxis = cos(mfinclinationang_yaxis);
    msin_finclinationang_yaxis = sin(mfinclinationang_yaxis);

    RegisterPointMsg(mstr_memname,getdefefaultcommtype());

    mbrunudpthread = true;
    std::thread * xudpthread = new std::thread(&ivdriver_lidar::threadudp,this);

    QDateTime dt = QDateTime::currentDateTime();
    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
                new pcl::PointCloud<pcl::PointXYZI>());

    point_cloud->header.frame_id = "velodyne";
    point_cloud->height = 1;
    point_cloud->header.stamp = dt.currentMSecsSinceEpoch();
    point_cloud->width = 0;
    point_cloud->header.seq =m_seq;
    m_seq++;

    while(mbrun)
    {
        if(mvectorba.size()>0)
        {
            mMutexba.lock();
            QByteArray ba = mvectorba[0];
            mvectorba.erase(mvectorba.begin());
            mMutexba.unlock();
            if(processudpData(ba,point_cloud) == 1)
            {
                SharePointCloud(point_cloud);
                point_cloud->clear();
                point_cloud->height = 1;
                point_cloud->header.stamp = dt.currentMSecsSinceEpoch();
                point_cloud->width = 0;
                point_cloud->header.seq =m_seq;
                m_seq++;
            }
        }
        else
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(1));
        }
    }

    mbrunudpthread = false;
    xudpthread->join();


}

void ivdriver_lidar::threadudp()
{
    QUdpSocket * udpSocket = new QUdpSocket( );
    bool bbind =     udpSocket->bind(QHostAddress(mstr_hostip), atoi(mstr_port));
    if(bbind == false)
    {
        qDebug("bind socket %s:%s error.",mstr_hostip,mstr_port);
        return;
    }
    int nnotrecv = 0;
    while(mbrunudpthread)
    {
        if(udpSocket->hasPendingDatagrams())
        {
            if(nnotrecv > 10000)
            {
                std::cout<<" Recv Data."<<std::endl;
            }
            QNetworkDatagram datagram = udpSocket->receiveDatagram();
            mMutexba.lock();
            if(mvectorba.size()<1000)
            {
                mvectorba.push_back(datagram.data());
            }
            else
            {
                std::cout<<" proc buffer is full."<<std::endl;
            }
            mMutexba.unlock();
            datagram.clear();
            nnotrecv = 0;
        }
        else
        {
            nnotrecv++;
//            std::cout<<"running."<<std::endl;
            std::this_thread::sleep_for(std::chrono::microseconds(100));
            if(nnotrecv == 10000)
            {
                std::cout<<"not recv datagram."<<std::endl;
            }
        }
    }
}

}