#include <perception_sf/sensor_lidar.h>

namespace iv {
namespace perception {
    LiDARSensor * gipl;

}
}

iv::perception::LiDARSensor::LiDARSensor() {
    std::shared_ptr<std::vector<iv::ObstacleBasic>> lidar_obs(new std::vector<iv::ObstacleBasic>);
    mobs = lidar_obs;
    mbRun = false;
    gipl = this;
	signal_lidar_obstacle_grid = createSignal<sig_cb_lidar_sensor_obstacle_grid>();
//    signal_fusion_obstacle_grid = createSignal<sig_cb_fusion_sensor_obstacle_grid>();
    JiGuangLeiDa = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
}
iv::perception::LiDARSensor::~LiDARSensor() {

}

void ListenPer(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{

//    qDebug("size is %d",nSize);

    std::shared_ptr<std::vector<iv::Perception::PerceptionOutput>> lidar_per(new std::vector<iv::Perception::PerceptionOutput>);


    iv::Perception::PerceptionOutput * pdata = (iv::Perception::PerceptionOutput *)strdata;
    int nCount = nSize/sizeof(iv::Perception::PerceptionOutput);
    int i;
    for(i=0;i<nCount;i++)
    {
        iv::Perception::PerceptionOutput temp;
        memcpy(&temp,pdata,sizeof(iv::Perception::PerceptionOutput));
        lidar_per->push_back(temp);
        pdata++;
    }
    iv::perception::gipl->UpdatePer(lidar_per);

  //  gw->UpdateOBS(lidar_obs);
}



void ListenFusion(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
{
    std::shared_ptr<iv::fusion::fusionobjectarray> xfusion (new iv::fusion::fusionobjectarray);

    if(nSize<1)return;
    if(false == xfusion->ParseFromArray(strdata,nSize))
    {
        std::cout<<" Listenesrfront fail."<<std::endl;
        return;
    }
    else{
        //std::cout<<"srfront byte size:  "<<radarobjvec.ByteSize()<<std::endl;
    }

//    for(int i=0;i<xfusion->obj_size();i++)
//    {
//        std::cout<<"x    y      vx    vy   w "<<xfusion->obj(i).centroid().x()<<"          "
//                <<xfusion->obj(i).centroid().y()<<"         "
//                <<xfusion->obj(i).vel_relative().x()<<"     "
//                <<xfusion->obj(i).vel_relative().y()<<"     "
//                <<xfusion->obj(i).dimensions().x()<<std::endl;
//    }

    iv::perception::gipl->updataFusion(xfusion);


}



void ListenOBS(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{

//    qDebug("size is %d",nSize);

    std::shared_ptr<std::vector<iv::ObstacleBasic>> lidar_obs(new std::vector<iv::ObstacleBasic>);


    iv::ObstacleBasic * pdata = (iv::ObstacleBasic *)strdata;
    int nCount = nSize/sizeof(iv::ObstacleBasic);
    int i;
    for(i=0;i<nCount;i++)
    {
        iv::ObstacleBasic temp;
        memcpy(&temp,pdata,sizeof(iv::ObstacleBasic));
        lidar_obs->push_back(temp);
        pdata++;
    }
    iv::perception::gipl->UpdateOBS(lidar_obs);

  //  gw->UpdateOBS(lidar_obs);
}


void iv::perception::LiDARSensor::start() {


 //   iv::modulecomm::RegisterRecv("lidar_obs",ListenOBS);
//    mpa = new adcmemshare("lidar_obs",20000000,3);
//    mpa->listenmsg(ListenOBS);

//    iv::modulecomm::RegisterRecv("lidar_per",ListenPer);
//    mpb = new adcmemshare("lidar_per",20000000,3);
//    mpb->listenmsg(ListenPer);


    // txs

//    iv::modulecomm::RegisterRecv("li_ra_fusion",ListenFusion);

    //txs

    mbRun = true;
    return;

//	thread_sensor_run_ = new boost::thread(boost::bind(&iv::perception::LiDARSensor::processSensor, this));
//	thread_sensor_run_collect = new boost::thread(boost::bind(&iv::perception::LiDARSensor::lidarcollect, this));
//	std::cout << "LiDAR thread Run" << std::endl;

}

void iv::perception::LiDARSensor::stop() {
    delete mpa;
    mbRun = false;
    return;
	thread_sensor_run_->interrupt();
	thread_sensor_run_->join();
	thread_sensor_run_collect->interrupt();
	thread_sensor_run_collect->join();
	std::cout << "LiDAR thread Stop" << std::endl;


}

bool iv::perception::LiDARSensor::isRunning() const {
    return mbRun;
	return (thread_sensor_run_ != NULL && !thread_sensor_run_->timed_join(boost::posix_time::milliseconds(10)) && thread_sensor_run_collect != NULL && !thread_sensor_run_collect->timed_join(boost::posix_time::milliseconds(10)));
}