#include "ivdriver_lidar_rs32.h"


namespace iv {


ivdriver_lidar_rs32::ivdriver_lidar_rs32()
{

}

int ivdriver_lidar_rs32::processudpData(QByteArray ba, pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud)
{
    static double fAngle_Total = 0;
    static double fAngle_Last = 0;

    int nrtn = 0;

//    float w = 0.0036;
    float Ang = 0;
    float Range = 0;
    int Group = 0;
    int pointi = 0;
    float wt = 0;
    int wt1 = 0;
    //    int dx;
    //    int dy;
    //    float lidar_32_xdistance = 0.3;			//32线激光雷达X轴补偿
    //    float lidar_32_ydistance = -2.3;			//32线激光雷达Y轴补偿


    float H_BETA[32] = {
        -8,-8,-8,-8,-8,-2.672,2.672,8,
        -8,-2.672,2.672,8,-8,-2.672,2.672,8,
        8,8,8,-8,8,-8,8,-8,
        -8,-2.672,2.672,8,-8,-2.672,2.672,8
    };

    //    float V_theta[16] = {-15,-13,-11,-9,-7,-5,-3,-1,15,13,11,9,7,5,3,1};
    float V_theta[32] = {-25,-14.638,-7.91,-5.407,-3.667,-4,-4.333,-4.667,-2.333,-2.667,-3,-3.333,-1,-1.333,-1.667,-2,
                         -10.281,-6.424,2.333,3.333,4.667,7,10.333,15,0.333,0,-0.333,-0.667,1.667,1.333,1.0,0.667};

    if(ba.size()<1206)
    {
        std::cout<<"packet size is small. size is "<<ba.size()<<std::endl;
        return -1;
    }
    char * pstr = (char * )ba.data();

    wt1 = ((pstr[2 + Group * 100] *256) + pstr[ 3 + Group * 100]) ;
    wt = wt1/ 100.0;

    double fAngX = wt;
    if(fabs(fAngX-fAngle_Last)>300)
    {
        fAngle_Total = fAngle_Total + fabs(fabs(fAngX-fAngle_Last)-360);
    }
    else
    {
        fAngle_Total = fAngle_Total + fabs(fabs(fAngX-fAngle_Last));
    }

    fAngle_Last = fAngX;
    if(fAngle_Total > 360)
    {
        nrtn = 1;
        fAngle_Total = 0;
    }

    for (Group = 0; Group <= 11; Group++)
    {
        wt1 = ((pstr[2 + Group * 100] *256) + pstr[ 3 + Group * 100]) ;
        wt = wt1/ 100.0;

        for (pointi = 0; pointi <32; pointi++)
        {
            //                        Ang = (0 - wt - w * T[pointi] - H_BETA[pointi]+213) / 180.0 * Lidar_Pi;
            Ang = (0 - wt-H_BETA[pointi]) / 180.0 * M_PI;
            Range = ((pstr[ Group * 100 + 4 + 3 * pointi] << 8) + pstr[Group * 100 + 5 + 3 * pointi]);
            unsigned char intensity = pstr[ Group * 100 + 6 + 3 * pointi];
            Range=Range* 5.0/1000.0;

            if(Range<150)
            {
                pcl::PointXYZI point;
                point.x  = Range*cos(V_theta[pointi] / 180 * M_PI)*cos(Ang + mfrollang);
                point.y  = Range*cos(V_theta[pointi] / 180 * M_PI)*sin(Ang + mfrollang);
                point.z  = Range*sin(V_theta[pointi] / 180 * M_PI);
                if(mbinclix)
                {
                    double y,z;
                    y = point.y;
                    z = point.z;
                    point.y = y*mcos_finclinationang_xaxis +z*msin_finclinationang_xaxis;
                    point.z = z*mcos_finclinationang_xaxis - y*msin_finclinationang_xaxis;
                }
                if(mbincliy)
                {
                    double z,x;
                    z = point.z;
                    x = point.x;
                    point.z = z*mcos_finclinationang_yaxis + x*msin_finclinationang_yaxis;
                    point.x = x*mcos_finclinationang_yaxis - z*msin_finclinationang_yaxis;
                }
                point.intensity = intensity;
                point_cloud->points.push_back(point);


                ++point_cloud->width;
            }
        }

    }
    return nrtn;

}

}