#include "ivdecision.h"

#include "gnss_coordinate_convert.h"

namespace iv {


ivdecision::ivdecision()
{

}

void ivdecision::modulerun()
{
    ModuleFun fungps = std::bind(&iv::ivdecision::UpdateGPS,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mpagpsmsg = iv::modulecomm::RegisterRecvPlus(mstrgpsmsgname.data(),fungps);
    ModuleFun funlidar = std::bind(&iv::ivdecision::UpdateLIDAR,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mpalidarmsg = iv::modulecomm::RegisterRecvPlus(mstrlidarmsgname.data(),funlidar);
    ModuleFun funradar = std::bind(&iv::ivdecision::UpdateRADAR,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mparadarmsg = iv::modulecomm::RegisterRecvPlus(mstrradarmsgname.data(),funradar);
    ModuleFun funmap = std::bind(&iv::ivdecision::UpdateMAP,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mpamapmsg = iv::modulecomm::RegisterRecvPlus(mstrmapmsgname.data(),funmap);
    ModuleFun funmobileye = std::bind(&iv::ivdecision::UpdateMobileye,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mpamobileyemsg = iv::modulecomm::RegisterRecvPlus(mstrmobileyemsgname.data(),funmobileye);
    ModuleFun funhmi = std::bind(&iv::ivdecision::UpdateHMI,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mpahmimsg = iv::modulecomm::RegisterRecvPlus(mstrhmimsgname.data(),funhmi);
    mpapadmsg = iv::modulecomm::RegisterRecvPlus(mstrpadmsgname.data(),funhmi);

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

    iv::modulecomm::Unregister(mpapadmsg);
    iv::modulecomm::Unregister(mpahmimsg);
    iv::modulecomm::Unregister(mpamobileyemsg);
    iv::modulecomm::Unregister(mpamapmsg);
    iv::modulecomm::Unregister(mparadarmsg);
    iv::modulecomm::Unregister(mpalidarmsg);
    iv::modulecomm::Unregister(mpagpsmsg);
}

void ivdecision::UpdateGPS(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::UpdateRADAR(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;
}

void ivdecision::UpdateLIDAR(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;

    unsigned int nCount = nSize/sizeof(iv::ObstacleBasic);
    std::shared_ptr<std::vector<iv::ObstacleBasic>> lidar_obs = std::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
    lidar_obs->resize(nCount);
    memcpy(lidar_obs->data(),strdata,nCount*sizeof(iv::ObstacleBasic));
    mMutexLIDAR.lock();
    mlidar_obs = lidar_obs;
    mnLIDARUpdateTime = QDateTime::currentMSecsSinceEpoch();
    mMutexLIDAR.unlock();
}

bool ivdecision::GetLIDARGrid(LidarGridPtr &lidargridptr)
{
    if((QDateTime::currentMSecsSinceEpoch() - mnLIDARUpdateTime)>mnNewThresh)
    {
        lidargridptr = NULL;
        return false;
    }

    LidarGridPtr ptr = (Obs_grid *)malloc(sizeof(Obs_grid[iv::grx][iv::gry]));
    memset(ptr, 0, sizeof(Obs_grid[iv::grx][iv::gry]));
    for (int i = 0; i <iv::grx; i++)//复制到指针数组
    {
        for (int j = 0; j <iv::gry; j++)
        {
//            ptr[i * (iv::gry) + j].high = obs_grid[i][j].high;
//            ptr[i * (iv::gry) + j].low = obs_grid[i][j].low;
            ptr[i * (iv::gry) + j].ob = 0;
//            ptr[i * (iv::gry) + j].obshight = obs_grid[i][j].obshight;
//            ptr[i * (iv::gry) + j].pointcount = obs_grid[i][j].pointcount;
        }
    }

    mMutexLIDAR.lock();

    for(unsigned int i=0;i<mlidar_obs->size();i++)
    {
        iv::ObstacleBasic xobs = mlidar_obs->at(i);
        int dx,dy;
        dx = (xobs.nomal_x + gridwide * (double)centerx)/gridwide;
        dy = (xobs.nomal_y + gridwide * (double)centery)/gridwide;
        if((dx>=0)&&(dx<iv::grx)&&(dy>=0)&&(dy<iv::gry))
        {
            ptr[dx*(iv::gry) +dy].high = xobs.high;
            ptr[dx*(iv::gry) +dy].low = xobs.low;
            ptr[dx*(iv::gry) + dy].ob = 2;
            ptr[dx*(iv::gry) + dy].obshight = xobs.nomal_z;
            ptr[dx*(iv::gry) + dy].pointcount = 5;
        }

    }
    lidargridptr = ptr;


    mMutexLIDAR.unlock();

    return true;
}

bool ivdecision::GetLIDARObs(std::shared_ptr<std::vector<ObstacleBasic> > &xlidar_obs)
{
    if((QDateTime::currentMSecsSinceEpoch() - mnLIDARUpdateTime)>mnNewThresh)
    {
        xlidar_obs = NULL;
        return false;
    }

    mMutexLIDAR.lock();
    xlidar_obs = mlidar_obs;
    mMutexLIDAR.unlock();
    return true;
}

void ivdecision::UpdateMAP(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;

    //    std::cout<<"update map "<<std::endl;
    int gpsunitsize = sizeof(iv::GPS_INS);
    int nMapSize = nSize/gpsunitsize;
    //    std::cout<<"map size is "<<nMapSize<<std::endl;

    if(nMapSize < 1)return;

    bool bUpdate = true;
//    if(nMapSize != mnavigation_data.size())
//    {
//        bUpdate = true;
//    }
//    else
//    {
//        iv::GPS_INS * p = (iv::GPS_INS *)mapdata;
//        if((p->gps_lat == navigation_data.at(0)->gps_lat)&&(p->ins_heading_angle == navigation_data.at(0)->ins_heading_angle))
//        {
//            //           qDebug("same map");
//            bUpdate = false;
//        }
//        else
//        {
//            bUpdate = true;
//        }
//    }

    if(bUpdate)
    {

        mMutexMAP.lock();
        mnavigation_data.clear();
        mnavigation_data.resize(nMapSize);
        memcpy(mnavigation_data.data(),strdata,nMapSize * sizeof(iv::GPS_INS));
        mnMAPUpdateTime = QDateTime::currentMSecsSinceEpoch();
        mMutexMAP.unlock();

    }
    else
    {

    }
}

bool ivdecision::IsMAPUpdate(qint64 &nLastUpdateTime)
{
    if(mnavigation_data.size()<1)
    {
        return false;
    }
    if(nLastUpdateTime != mnMAPUpdateTime)
    {
        return true;
    }
    return false;
}

bool ivdecision::GetMAP(std::vector<GPSData> &navigation_data, qint64 &nLastUpdateTime)
{
    if(mnavigation_data.size()<1)
    {
        return false;
    }
    mMutexMAP.unlock();
    nLastUpdateTime = mnMAPUpdateTime;
    navigation_data = mnavigation_data;
    mMutexMAP.unlock();
    return true;
}

void ivdecision::UpdateMobileye(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;

    std::shared_ptr<iv::mobileye::mobileye> xmobileye = std::shared_ptr<iv::mobileye::mobileye>(new iv::mobileye::mobileye);

    if(!xmobileye->ParseFromArray(strdata,nSize))
    {
        std::cout<<"ivdecision::UpdateMobileye Parse Error."<<std::endl;
        return;
    }

    mMutexmobileye.lock();
    mmobileye = xmobileye;
    mnmobileyeUpdateTime = QDateTime::currentMSecsSinceEpoch();
    mMutexmobileye.unlock();
}

bool ivdecision::GetMobileye(std::shared_ptr<mobileye::mobileye> &xmobileye)
{
    if((QDateTime::currentMSecsSinceEpoch() - mnmobileyeUpdateTime)>mnNewThresh)
    {
        xmobileye = NULL;
        return false;
    }

    mMutexmobileye.lock();
    xmobileye = mmobileye;
    mMutexmobileye.unlock();
    return true;
}

void ivdecision::UpdateHMI(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;

    std::shared_ptr<iv::hmi::hmimsg> xhmiptr = std::shared_ptr<iv::hmi::hmimsg>(new iv::hmi::hmimsg);

    if(!xhmiptr->ParseFromArray(strdata,nSize))
    {
        std::cout<<"ivdecision::UpdateHMI Parse Error."<<std::endl;
        return;
    }

    mMutexHMI.lock();
    mHMImsg = xhmiptr;
    mnHMIUpdateTime = QDateTime::currentMSecsSinceEpoch();
    mMutexHMI.unlock();
}

bool ivdecision::GetHMImsg(std::shared_ptr<hmi::hmimsg> &xhmimsg)
{
    if((QDateTime::currentMSecsSinceEpoch() - mnHMIUpdateTime)>mnNewThresh)
    {
        xhmimsg = NULL;
        return false;
    }
    mMutexHMI.lock();
    mHMImsg = xhmimsg;
    mMutexHMI.unlock();
    return true;
}

}