#include "ivdriver_lidar_rs16.h"

namespace iv {


ivdriver_lidar_rs16::ivdriver_lidar_rs16()
{

}

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

    int nrtn = 0;

    float Ang = 0;
    float Range = 0;
    int Group = 0;
    int pointi = 0;
    float wt = 0;
    int wt1 = 0;

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

    char * pstr = ba.data();

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

    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 <16; pointi++)
        {
//                        Ang = (0 - wt - w * T[pointi] - H_BETA[pointi]+213) / 180.0 * Lidar_Pi;
            Ang = (0 - wt) / 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;
            }
        }


        wt = wt + 0.18;



        for (pointi = 0; pointi < 16; pointi++)
        {
            Ang = (0 - wt) / 180.0 * M_PI;
   //         Ang = Ang+angdiff;
            Range = ((pstr[ Group * 100 + 52 + 3 * pointi] << 8) + pstr[Group * 100 + 53 + 3 * pointi]);
            unsigned char intensity = pstr[ Group * 100 + 54 + 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;

}

}