#include <perception_sf/eyes.h>
#include <time.h>

iv::perception::Eyes::Eyes() {
    mgpsindex = 0;
    mgpsreadindex = 0;

}
iv::perception::Eyes::~Eyes() {

    delete sensor_gps;
//    delete sensor_camera;
//    delete sensor_radar;
    delete sensor_lidar;
}

void iv::perception::Eyes::inialize() {

    sensor_lidar = new iv::perception::LiDARSensor();
    sensor_radar = new iv::perception::RadarSensor();
    sensor_camera = new iv::perception::CameraSensor();
    sensor_gps = new iv::perception::GPSSensor();


    sensor_lidar->registerCallBack<iv::perception::LiDARSensor::sig_cb_lidar_sensor_obstacle_grid>(boost::bind(&iv::perception::Eyes::cb_lidar_grid, this, _1));



    //sensor_radar->registerCallBack<iv::perception::RadarSensor::sig_cb_radar_sensor_obstacle>(boost::bind(&iv::perception::Eyes::cb_radar, this, _1));
    //sensor_camera->registerCallBack<iv::perception::CameraSensor::sig_cb_camera_sensor_obstacle>(boost::bind(&iv::perception::Eyes::cb_camera, this, _1));
    sensor_gps->registerCallBack<iv::perception::GPSSensor::sig_cb_gps_sensor_data>(boost::bind(&iv::perception::Eyes::cb_gps, this, _1));
    sensor_lidar->start();
    sensor_radar->start();
    sensor_camera->start();
    sensor_gps->start();
    obs_lidar_gridptr_ = NULL;
    obs_camera_ = NULL;
}

void iv::perception::Eyes::cb_lidar_grid(iv::LidarGridPtr obs) {
    mutex_.lock();
    if (obs_lidar_gridptr_ != NULL)
    {
        free(obs_lidar_gridptr_);
    }
    obs_lidar_gridptr_ = obs;
    obs = NULL;
    mutex_.unlock();
}

void iv::perception::Eyes::cb_radar(iv::ObsRadar obs) {
    boost::mutex::scoped_lock(mutex_);
    obs_radar_ = obs;
}

void iv::perception::Eyes::cb_camera(iv::CameraInfoPtr obs) {
    mutex_.lock();
    //boost::mutex::scoped_lock(mutex_);
    if (obs_camera_ != NULL)
    {
        free(obs_camera_);
    }
    obs_camera_ = obs;
    obs = NULL;
    mutex_.unlock();
}

void iv::perception::Eyes::cb_gps(iv::GPSData gps_data) {
    if((gps_data->gps_lat<0.01)||(gps_data->gps_lat>90))return;
    boost::mutex::scoped_lock(mutex_);
//    gps_ins_data_ = gps_data;

    if(gps_data == NULL)return;
    iv::GPSData data(new iv::GPS_INS);
    if(data == NULL)return;
    *data = *gps_data;
    gps_ins_data_ = data;
    mgpsindex++;

}

bool iv::perception::Eyes::isAllSensorRunning() {
    return true;
    bool bx;
    //bx = sensor_lidar->isRunning() && sensor_radar->isRunning() && sensor_camera->isRunning() && sensor_gps->isRunning();
    bx = sensor_gps->isRunning();
//	ODS("Get Running time = %d", GetTickCount() - ticks);
    return bx;
}

void iv::perception::Eyes::stopAllSensor() {
    sensor_lidar->stop();
    sensor_radar->stop();
    sensor_camera->stop();
    sensor_gps->stop();
}

void iv::perception::Eyes::getSensorObstacle(iv::ObsRadar& brain_obs_radar, iv::CameraInfoPtr& brain_obs_camera, iv::GPSData& brain_gps_data, iv::LidarGridPtr& brain_obs_lidar_gridptr) {
    //brain_obs_camera = NULL;
    brain_gps_data = NULL;
    brain_obs_radar = NULL;
    while (true) {
        if (mutex_.try_lock()) {
//            ServiceLidar.copylidarobsto(brain_obs_lidar_gridptr);
            if(obs_lidar_gridptr_ != NULL)
            {
                brain_obs_lidar_gridptr  = (Obs_grid *)malloc(sizeof(Obs_grid[iv::grx][iv::gry]));
                memcpy(brain_obs_lidar_gridptr,obs_lidar_gridptr_,sizeof(Obs_grid[iv::grx][iv::gry]));
             //   brain_obs_lidar_gridptr = obs_lidar_gridptr_;
                free(obs_lidar_gridptr_);
                obs_lidar_gridptr_ = NULL;
            }
            obs_lidar_gridptr_ = NULL;

 //           obs_radar_.swap(brain_obs_radar);
            brain_obs_radar = NULL;

            //obs_camera_.swap(brain_obs_camera);
            brain_obs_camera = NULL;

            if(mgpsindex != mgpsreadindex)
  //          if(gps_ins_data_ != NULL)
            {
                iv::GPSData data(new iv::GPS_INS);
                *data = *gps_ins_data_;
                brain_gps_data =  data;
                mgpsreadindex = mgpsindex;
            }

//            brain_gps_data = gps_ins_data_;
//            gps_ins_data_ = NULL;


            obs_camera_ = NULL;
            mutex_.unlock();
            break;
        }
    }
}