#include "lidar_16z.h"


static float V_theta[16] = {-15,-13,-11,-9,-7,-5,-3,-1,1,3,5,7,9,11,13,15};
static float H_Beta[16] = {-2.5,-2.5 ,-2.5 ,-2.5,-2.5,-2.5 ,-2.5 ,-2.5,
                    2.5,-2.5 ,-2.5 ,-2.5, -2.5,-2.5 ,-2.5 ,-2.5};

lidar_16z::lidar_16z() {

    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 = static_cast<uint64_t>(std::chrono::system_clock::now().time_since_epoch().count()/1000);
    point_cloud->width = 0;
    point_cloud->header.seq =0;
    mpoint_cloud_temp = point_cloud;

    mpdriver = new lidar_bkdriver();
    mpTheadDecode = new  std::thread(&lidar_16z::ThreadDecode,this);
}

lidar_16z::~lidar_16z(){
    mbRun = false;
    mpTheadDecode->join();
    delete mpdriver;
}

void lidar_16z::ThreadDecode()
{
    static double azutotal = 0.0;
    static int g_seq = 0;
    while(mbRun)
    {
        std::shared_ptr<unsigned char> pdata_ptr;
        int64_t nrecvtime;
        int ndatasize = 0;
        int nread = mpdriver->GetRecvData(pdata_ptr,nrecvtime,ndatasize);
        if(nread == 1)
        {
            std::cout<<"read "<<ndatasize<<std::endl;
            if(ndatasize % 1184 != 0)
            {
                std::cout<<" data not complete. datasize: "<<ndatasize<<std::endl;
            }
            else
            {
                unsigned char * p = pdata_ptr.get();
                unsigned short * pang_start = (unsigned short * )(p+12);
                unsigned short * pang_end = (unsigned short *)(p+14);
                (void)pang_end;
                double ang_space = 0.2;
                int i;
                int nheadlen = 32;
                double fAzuAng = static_cast<double>(*pang_start) * 0.01;
                for(i=0;i<12;i++)
                {
                    int j;
                    for(j=0;j<16;j++)
                    {
                        unsigned char * pecho = (unsigned char *)(p +nheadlen +  i*96 + j*6);
                        unsigned short * prange;
                        unsigned char * pchch;
                        double frange;
                        unsigned char  nch;
                        unsigned char intensity;
                        pchch = pecho;
                        prange = (unsigned short *)(pecho +3);
                        intensity = *(pecho + 5);
                        nch = * pchch;
                        frange = static_cast<double>(*prange);
                        frange = frange * 0.004;
                        if(nch<=16){
                            double fAng = (-fAzuAng - H_Beta[nch] - mfRollAng) * M_PI/180.0;
                            double vtheta = V_theta[nch] * M_PI/180.0;
                            double x = frange * cos(fAng)* cos(vtheta);
                            double y = frange * sin(fAng)* cos(vtheta);
                            double z = frange * sin(vtheta);

                            pcl::PointXYZI point;
                            point.x = x;
                            point.y = y;
                            point.z = z;
                            point.intensity = intensity;

                            mpoint_cloud_temp->points.push_back(point);
                            ++mpoint_cloud_temp->width;
                        }
                        else
                        {
                            std::cout<<" channel error."<<std::endl;
                        }
                    }
                    fAzuAng = fAzuAng + ang_space;

                    azutotal = azutotal + ang_space;
                    if(azutotal>=360.0)
                    {
                        //share pointcloud
                        mmutexpc.lock();
                        mpoint_cloud = mpoint_cloud_temp;
                        mbpcupdate = true;
                        mmutexpc.unlock();
                        mcv.notify_all();
                        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 = static_cast<uint64_t>(std::chrono::system_clock::now().time_since_epoch().count()/1000);
                        point_cloud->width = 0;
                        point_cloud->header.seq =g_seq;
                        g_seq++;
                        mpoint_cloud_temp = point_cloud;
                        azutotal = 0;
                    }
                }
            }
        }
    }
}

int lidar_16z::GetPointCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud)
{
    return 1;
}