#include "ivdecision.h"

#include "gnss_coordinate_convert.h"

namespace iv {


ivdecision::ivdecision()
{

}

void ivdecision::modulerun()
{


    while(mbrun)
    {
        //RunDecision
        //ShareMsg
    }
}

void ivdecision::UpdateGPSMsg(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
{
    (void )&index;
    (void )&dt;
    (void )strmemname;

    if(nSize<1)return;

    iv::gps::gpsimu xgpsimu;
    if(!xgpsimu.ParseFromArray(strdata,nSize))
    {
       std::cout<<"ivdecision::UpdateGPSMsg error."<<std::endl;
       return;
    }

    iv::GPSData data(new iv::GPS_INS);
    data->ins_status = xgpsimu.ins_state();
    data->rtk_status = xgpsimu.rtk_state();

    data->accel_x = xgpsimu.acce_x();
    data->accel_y = xgpsimu.acce_y();
    data->accel_z = xgpsimu.acce_z();
    data->ang_rate_x = xgpsimu.gyro_x();
    data->ang_rate_y = xgpsimu.gyro_y();
    data->ang_rate_z = xgpsimu.gyro_z();

    data->gps_lat = xgpsimu.lat();
    data->gps_lng = xgpsimu.lon();
    data->gps_z = xgpsimu.height();

    data->ins_heading_angle = xgpsimu.heading();
    data->ins_pitch_angle = xgpsimu.pitch();
    data->ins_roll_angle = xgpsimu.roll();

    data->speed = sqrt(pow(xgpsimu.ve(),2)+pow(xgpsimu.vn(),2)) * 3.6;

    GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y);

    mMutexGPS.lock();
    iv::gps::gpsimu * pgpsimu = new iv::gps::gpsimu;
    pgpsimu->CopyFrom(xgpsimu);
    mGPSIMUptr.reset(pgpsimu);
    mNowGPS = data;
    mnGPSUpdateTime = QDateTime::currentMSecsSinceEpoch();
    mMutexGPS.unlock();




}

bool ivdecision::GetGPS(GPSData &xGPSData)
{
    if((QDateTime::currentMSecsSinceEpoch() - mnGPSUpdateTime)>mnNewThresh)
    {
        xGPSData = NULL;
        return false;
    }
    xGPSData = mNowGPS;
    return true;
}

void ivdecision::UpdateRADARMsg(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
{
    (void )&index;
    (void )&dt;
    (void )strmemname;

    if(nSize<1)return;

    iv::radar::radarobjectarray * pradar = new iv::radar::radarobjectarray;
    if(!pradar->ParseFromArray(strdata,nSize))
    {
        std::cout<<"ivdecision::UpdateRADARMsg Parse Error"<<std::endl;
        return;
    }

    mMutexRADAR.lock();
    mRADAR.reset(pradar);
    mnRADARUpdateTime = QDateTime::currentMSecsSinceEpoch();
    mMutexRADAR.unlock();

}

bool ivdecision::GetRADAR(std::shared_ptr<radar::radarobjectarray> &xRADAR)
{
    if((QDateTime::currentMSecsSinceEpoch() - mnRADARUpdateTime)>mnNewThresh)
    {
        xRADAR = NULL;
        return false;
    }
    xRADAR = mRADAR;
    return true;
}



}