#include "rsdriver.h"

#define RS_Grabber_toRadians(x) ((x)*M_PI / 180.0)
extern char gstr_memname[256];

namespace lidar {
char gstr_hostip[256];
char gstr_port[256];
bool g_rs_m1_run =false;
bool g_rs_m1_running = false;
bool g_brs_m1_Proc_running = false;
int index;
uint32_t point_index;

rsM1_Buf *rs_m1_buf;
char * g_RawData_Buf;
int gnRawPos = 0;
int g_seq = 0;
int g_pcl =0;
float pitch_rate;
int pitch_max;
int skip_block;

float yaw_rate;
float distance_max_thd;
float distance_min_thd;

std::vector<float> tan_lookup_table;

std::thread * g_prsm1Thread;
std::thread * g_prsm1ProcThread;

 QTime gTime;
 void *g_lidar_pc;

 static const int MEMS_BLOCKS_PER_PACKET =25;
 static const int MEMS_SCANS_PER_FIRING = 6;
 static const int POINT_COUNT_PER_VIEWFIELD = 15750;
 static const float ANGLE_RESOLUTION = 0.01f;
 static const float DISTANCE_RESOLUTION = 0.01f;
 int last_bag_index = -1;
 int last_pitch_index = 0;

 static float channel_cor[6] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};  // distance correction offset
 static float channel_pitch_offset[6] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};   // pitch offset
 static float channel_yaw_offset[6] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};     //yaw offset

 Eigen::Matrix<float, 3, 5> input_ref_;
 Eigen::Matrix<float, 3, 3> rotate_gal_;

 void loadParam(){
     input_ref_<< 0.748956, 0.414693, -0.414693, -0.748956, 0,
             0.33131,  0.454981,  0.454981,  0.33131,  0.5,
            -0.573846,-0.78805,  -0.78805,  -0.573846,-0.866025;

     rotate_gal_<< 1, 0, 0, 0, 0.965926, -0.258819, 0, 0.258819, 0.965926;
     pitch_rate = 1.0f;
     yaw_rate = 1.0f;
     distance_max_thd = 200.0;
     distance_min_thd = 0.0;
     pitch_max = 25000;
     skip_block = 0;
     point_index = 0;
     tan_lookup_table.resize(2000);
     for(int i = -1000; i<1000; i++)
     {
         double rad =RS_Grabber_toRadians(i/100.0f);
         tan_lookup_table[i+1000] = std::tan(rad);
     }
 }

 inline float pixlToDistance(int distance, int dsr){
     float result;
     int cor = channel_cor[dsr];
     if(distance <= cor)
     {
         result = 0.0;
     }else{
         result = distance -cor;
     }
     return result;
 }

inline  float yawToDeg(int deg){
     float result_f;
     float deg_f =deg;
     result_f = (deg_f * (1350.0f / 65534.0f) - 675.0f);
     return result_f;
 }

 inline float pitchToDeg(int deg){
     float result_f;
     float deg_f = deg;
     result_f = (deg_f * (1250.0f / pitch_max ) - 625.0f);
     return result_f;
 }

 inline void processrs_M1_Data(QByteArray ba)
 {
     unsigned char * pdata;
     float fa,fb;
     pdata = reinterpret_cast<unsigned char*>(ba.data());
     ++index;
     if(ba.length() != 1400 && pdata[0] != 0x55 && pdata[0] != 0xa5)
     {
         std::cout<<"No right packets!"<<std::endl;
     }else {
         if(!((int)(pdata[4]*256 +pdata[5]) < (POINT_COUNT_PER_VIEWFIELD/MEMS_BLOCKS_PER_PACKET)))
         {
             memcpy(g_RawData_Buf+gnRawPos,pdata,1400);
             gnRawPos= gnRawPos+1400;
             rs_m1_buf->WriteData(g_RawData_Buf,gnRawPos);
             gnRawPos = 0;
             index = 0;
         }else {
             if((gnRawPos+1400)<= BK32_DATA_BUFSIZE)
             {
                 memcpy(g_RawData_Buf+gnRawPos,pdata,1400);
                 gnRawPos= gnRawPos+1400;
             }
             else
             {
                 std::cout<<"lidar_rsM1 processrs_M1_Data data is very big gnRawPos = "<<gnRawPos<<std::endl;
             }
        }
     }
 }

void getPacket(int n){
    gTime.start();
    QUdpSocket * udpSocket = new QUdpSocket( );
    bool bbind = udpSocket->bind(QHostAddress(gstr_hostip), atoi(gstr_port));
    int i = 0;
   while(g_rs_m1_run)
   {
       if(udpSocket->hasPendingDatagrams())
       {
           QNetworkDatagram datagram = udpSocket->receiveDatagram();
           processrs_M1_Data(datagram.data());
           datagram.clear();
       }
       else
       {
           std::this_thread::sleep_for(std::chrono::microseconds(50));
 //          std::this_thread::sleep_for(std::chrono::milliseconds(1));
       }
   }
   udpSocket->close();
   delete udpSocket;
   g_rs_m1_running = false;
}


void process_rs_M1_obs(char * strdata,int nLen)
{
    loadParam();
    float pitch;
    float yaw;
    int bag = 0;
    int block = 0;
    int channel = 0;
    int buf1len = nLen/1400;
//    std::cout<<"   len    :"<<nLen<<"ooooooooooooooo"<<buf1len<<std::endl;
    QDateTime dt = QDateTime::currentDateTime();
    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
                new pcl::PointCloud<pcl::PointXYZI>());
    pcl::PointXYZI point;
    point_cloud->header.frame_id = "velodyne";
    point_cloud->height = 5;
    point_cloud->header.stamp = dt.currentMSecsSinceEpoch();
    point_cloud->width = 15750;
//    point_cloud->is_dense = false;
    point_cloud->resize(point_cloud->height*point_cloud->width);
    unsigned char * pstr = (unsigned char *)strdata;

//    std::cout<<"len is "<<buf1len<<std::endl;

    for ( bag = 0; bag < buf1len; bag++)
    {
        int bag_index = (pstr[bag*1400 +4]*256 + pstr[bag*1400 + 5]);
        if(bag_index - last_bag_index > 1 && (bag_index - last_bag_index) < POINT_COUNT_PER_VIEWFIELD / MEMS_BLOCKS_PER_PACKET
                && last_bag_index != -1)
        {
            int lose_bag_count = bag_index - last_bag_index - 1;
            while(lose_bag_count --)
            {
                for(block = 0; block < MEMS_BLOCKS_PER_PACKET; block++)
                {
                    for (channel = 1; channel <MEMS_SCANS_PER_FIRING; channel++){
                        point.y = NAN;
                        point.x = NAN;
                        point.z = NAN;
                        point.intensity = 0;
                        point_cloud->at(point_index, channel - 1) = point;
//                        point_cloud->push_back(point);
//                        ++point_cloud->width;
                    }
                    point_index++;
                }
            }
        }
        for (block = skip_block; block < MEMS_BLOCKS_PER_PACKET; block++)
        {
            int pitch_t = ((pstr[bag*1400+20  + block * 52]   *256) + pstr[bag*1400 + 21 + block * 52]) ;

            if(last_pitch_index > pitch_t){
                 if(last_bag_index = POINT_COUNT_PER_VIEWFIELD / MEMS_BLOCKS_PER_PACKET){
                  pitch_max = last_pitch_index;
             }
                   point_index= 0;
 //                 point_cloud->width = 0;
                  skip_block = block;
                  last_pitch_index = pitch_t;
                  break;
             }
             last_pitch_index = pitch_t;
             pitch = pitchToDeg(pitch_t) * ANGLE_RESOLUTION;
             int yaw_t = ((pstr[bag*1400 +22 + block * 52] *256) + pstr[bag*1400 + 23 + block * 52]) ;
             yaw = yawToDeg(yaw_t) * ANGLE_RESOLUTION;
             Eigen::Matrix<float, 3, 1> n_gal;
             Eigen::Matrix<float, 3, 1> i_out;
             for (channel = 0; channel <(MEMS_SCANS_PER_FIRING-1); channel++){
                 int ax, ay;
                 float tanax, tanay;
                 ax = (int)(100 * pitch_rate * (pitch + channel_pitch_offset[channel+1]));
                 ay = (int)(100 * yaw_rate * (yaw + channel_yaw_offset[channel+1]));
                 tanax = tan_lookup_table[ax + 1000];
                 tanay = tan_lookup_table[ay + 1000];
                 tanax = -tanax;
                 tanay = -tanay;
                 n_gal << tanay, -tanax, 1;
                 n_gal.normalize();
                 n_gal = rotate_gal_ * n_gal;
                 i_out = input_ref_.col(channel ) - 2 * n_gal * n_gal.transpose() * input_ref_.col(channel);
                 int Range_t = ((pstr[bag*1400 + block * 52 + 34 + 8 * channel] << 8) + pstr[bag*1400+block * 52 + 35 + 8 * channel]);
                 float Range=pixlToDistance(Range_t,(channel+1)) *DISTANCE_RESOLUTION ;
                 unsigned char intensity = ((pstr[bag*1400 + block * 52 + 32 + 8 * channel] << 8)
                            + pstr[bag*1400 + block* 52 + 33 + 8 * channel]);
                 if(Range > distance_max_thd || Range < distance_min_thd)
                 {
                     point.y = NAN;
                     point.x = NAN;
                     point.z = NAN;
                     point.intensity = 0;
//                     point_cloud->push_back(point);
//                     ++point_cloud->width;
                 }else {

                     point.y = i_out(2) * Range;
                     point.x = -i_out(0) * Range;
                     point.z = i_out(1) * Range;
                     point.intensity = intensity;
//                     point_cloud->push_back(point);
//                     ++point_cloud->width;
//                     std::cout<<" x   y   z    "<<point.x<<"   ,   "<<point.y<<"   ,  "<<point.z<< "   ,  "<<Range<<std::endl;
                  }
                 if(point_index < POINT_COUNT_PER_VIEWFIELD){
                     point_cloud->at(point_index, channel) = point;
//                     point_cloud->push_back(point);
//                     ++point_cloud->width;
                 }
             }
                point_index++;
                skip_block = 0;
            }
        last_bag_index = bag_index;
    }

//    std::cout<<"point size is "<<point_cloud->width<<std::endl;
    char * strOut = new char[4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->size()* 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->size() * sizeof(pcl::PointXYZI));
    iv::modulecomm::ModuleSendMsg(g_lidar_pc,strOut,4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->size() * sizeof(pcl::PointXYZI));
    delete strOut;
}

void  rsm1_Proc_Func(int n)
{
    std::cout<<"Enter rsM1_Proc_Func"<<std::endl;
    char * strdata = new char[BK32_DATA_BUFSIZE];
    int nIndex;
    int nRead;
    while(g_rs_m1_run)
    {
        if((nRead = rs_m1_buf->ReadData(strdata,BK32_DATA_BUFSIZE,&nIndex))>0)
        {
            process_rs_M1_obs(strdata,nRead);
        }
        else
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(1));
        }
    }
    g_rs_m1_running = false;
    std::cout<<"Exit rsm1_Proc_Func"<<std::endl;
}


int StartLidar_rs_m1()
{
    std::cout<<"Now Start rsm1 Listen."<<std::endl;
    g_RawData_Buf = new char[BK32_DATA_BUFSIZE];
    rs_m1_buf = new rsM1_Buf();
    g_rs_m1_run = true;
    g_rs_m1_running = true;
    g_brs_m1_Proc_running = true;
    g_lidar_pc = iv::modulecomm::RegisterSend(gstr_memname,20000000,1);
    g_prsm1Thread = new std::thread(getPacket,0);
    g_prsm1ProcThread = new std::thread(rsm1_Proc_Func,0);
    return 0;
}

void StopLidar_rs16()
{
    std::cout<<"Now Close rsm1. "<<std::endl;
    g_rs_m1_run = false;
    g_prsm1Thread->join();
    g_prsm1ProcThread->join();

    delete g_prsm1ProcThread;
    delete g_prsm1Thread;

    delete rs_m1_buf;
    delete g_RawData_Buf;
}
}