#include <perception_sf/sensor_gps.h>
#include "ivlog.h"

extern std::string gstrmemgps;
extern iv::Ivlog * givlog;

namespace iv {
namespace perception {
    GPSSensor * ggpsimu;

    void Listengpsimu(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
    {
//        ggpsimu->UpdateGPSIMU(strdata,nSize);

        iv::gps::gpsimu xgpsimu;
        if(!xgpsimu.ParseFromArray(strdata,nSize))
        {
            std::cout<<"ListenRaw Parse error."<<std::endl;
        }
//        qDebug(" gps time is %ld",QDateTime::currentMSecsSinceEpoch());
        ggpsimu->UpdateGPSIMU(xgpsimu);

        static qint64 oldtime;
        if((oldtime - QDateTime::currentMSecsSinceEpoch())<-100)
        {
            qDebug("gps diff time is %ld diff is %ld ",QDateTime::currentMSecsSinceEpoch(),oldtime - QDateTime::currentMSecsSinceEpoch());
            givlog->warn("gps diff time is %ld diff is %ld ",QDateTime::currentMSecsSinceEpoch(),oldtime - QDateTime::currentMSecsSinceEpoch());
        }
        givlog->verbose("gps diff time is %ld diff is %ld ",QDateTime::currentMSecsSinceEpoch(),oldtime - QDateTime::currentMSecsSinceEpoch());
        oldtime = QDateTime::currentMSecsSinceEpoch();

    }
}
}

iv::perception::GPSSensor::GPSSensor() {
	signal_gps_data = createSignal<sig_cb_gps_sensor_data>();
    ggpsimu = this;
}
iv::perception::GPSSensor::~GPSSensor() {

}

void iv::perception::GPSSensor::start()
{
    thread_sensor_run_ = new boost::thread(boost::bind(&iv::perception::GPSSensor::processSensor, this));

    mpagpsimu = iv::modulecomm::RegisterRecv(gstrmemgps.data(),iv::perception::Listengpsimu);
    //OutputDebugString(TEXT("\nGPS thread Run\n"));
}

void iv::perception::GPSSensor::stop()
{
	thread_sensor_run_->interrupt();
	thread_sensor_run_->join();
    //OutputDebugString(TEXT("\nGPS thread Stop\n"));
}

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