#include "ivdecision.h"

#include "gnss_coordinate_convert.h"

#include "decition_type.h"
#include "decition.pb.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);    
    ModuleFun funp900 = std::bind(&iv::ivdecision::Updatep900,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mpap900msg = iv::modulecomm::RegisterRecvPlus(mstrp900msgname.data(),funp900);
    ModuleFun funv2x = std::bind(&iv::ivdecision::Updatev2x,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mpav2xmsg = iv::modulecomm::RegisterRecvPlus(mstrv2xmsgname.data(),funv2x);
    ModuleFun funultra = std::bind(&iv::ivdecision::Updateultrasonic,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mpaultramsg = iv::modulecomm::RegisterRecvPlus(mstrultramsgname.data(),funultra);
    ModuleFun funvbox = std::bind(&iv::ivdecision::UpdateVbox,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mpavboxmsg = iv::modulecomm::RegisterRecvPlus(mstrvboxmsgname.data(),funvbox);
    ModuleFun funchassis = std::bind(&iv::ivdecision::UpdatepChassis,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mpachassismsg = iv::modulecomm::RegisterRecvPlus(mstrchassismsgname.data(),funchassis);

    mpaDecition = iv::modulecomm::RegisterSend(mstrdecisionmsgname.data(),10*sizeof(iv::decition::DecitionBasic),1);
    mpaVechicleState = iv::modulecomm::RegisterSend(mstrbrainstatemsgname.data(),10000,10);

    iv::brain::brainstate xbs;
    iv::brain::decition xdecition;

    qint64 nSpace = 10;
    qint64 nLastDecTime = 0;

    while(mbrun)
    {

        if((QDateTime::currentMSecsSinceEpoch() - nLastDecTime)<nSpace)
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(2));
            continue;
        }
        nLastDecTime = QDateTime::currentMSecsSinceEpoch();
        std::cout<<"running."<<std::endl;
        int nrtn = getdecision(xdecition,xbs);
        if(nrtn == 1)
        {
            sharemsg(xdecition,xbs);
        }
        //RunDecision
        //ShareMsg
    }

    iv::modulecomm::Unregister(mpaVechicleState);
    iv::modulecomm::Unregister(mpaDecition);
    iv::modulecomm::Unregister(mpachassismsg);
    iv::modulecomm::Unregister(mpavboxmsg);
    iv::modulecomm::Unregister(mpaultramsg);
    iv::modulecomm::Unregister(mpav2xmsg);
    iv::modulecomm::Unregister(mpap900msg);

    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::sharemsg(brain::decition &xdecition, brain::brainstate &xbs)
{
    int nsize = xdecition.ByteSize();
    char * str = new char[nsize];
    std::shared_ptr<char> pstr;
    pstr.reset(str);
    if(xdecition.SerializeToArray(str,nsize))
    {
//        if(ServiceCarStatus.mbRunPause == false)
//        {

            iv::modulecomm::ModuleSendMsg(mpaDecition,str,nsize);
//        }
    }
    else
    {
        std::cout<<"iv::decition::BrainDecition::ShareDecition serialize error."<<std::endl;
    }

    nsize = xbs.ByteSize();
    str = new char[nsize];
 //   std::shared_ptr<char> pstr;
    pstr.reset(str);
    if(xbs.SerializeToArray(str,nsize))
    {
        iv::modulecomm::ModuleSendMsg(mpaVechicleState,str,nsize);
    }
    else
    {
        std::cout<<"iv::decition::BrainDecition::ShareBrainstate serialize error."<<std::endl;
    }


}

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);
        int i;
        for(i=0;i<nMapSize;i++)
        {
            iv::GPS_INS x;
            memcpy(&x,strdata + i*gpsunitsize,gpsunitsize);
            iv::GPSData data(new iv::GPS_INS);
            *data = x;
            mnavigation_data.push_back(data);

        }
//        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();
    xhmimsg = mHMImsg;
    mMutexHMI.unlock();
    return true;
}

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

    std::shared_ptr<iv::vbox::vbox> vbox_ptr = std::shared_ptr<iv::vbox::vbox>(new iv::vbox::vbox);
    if(!vbox_ptr->ParseFromArray(strdata,nSize))
    {
        std::cout<<"vdecision::UpdateVbox Parse Error"<<std::endl;
        return;
    }
    mMutexvbox.lock();
    mvboxmsg = vbox_ptr;
    mnvboxUpdateTime = QDateTime::currentMSecsSinceEpoch();
    mMutexvbox.unlock();
}

bool ivdecision::Getvboxmsg(std::shared_ptr<vbox::vbox> &xvboxmsg)
{
    if((QDateTime::currentMSecsSinceEpoch() - mnvboxUpdateTime)>mnNewThresh)
    {
        xvboxmsg = NULL;
        return false;
    }
    mMutexvbox.lock();
    xvboxmsg = mvboxmsg;
    mMutexvbox.unlock();
    return true;
}

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

    std::shared_ptr<iv::v2x::v2x> xv2x_ptr = std::shared_ptr<iv::v2x::v2x>(new iv::v2x::v2x);
    if(!xv2x_ptr->ParseFromArray(strdata,nSize))
    {
        std::cout<<"ivdecision::Updatev2x Parse Error."<<std::endl;
        return;
    }

    mMutexv2x.lock();
    mv2xmsg = xv2x_ptr;
    mnv2xUpdateTime = QDateTime::currentMSecsSinceEpoch();
    mMutexv2x.unlock();
}

bool ivdecision::Getv2xmsg(std::shared_ptr<v2x::v2x> &xv2xmsg)
{
    if((QDateTime::currentMSecsSinceEpoch() - mnv2xUpdateTime)>mnNewThresh)
    {
        xv2xmsg = NULL;
        return false;
    }
    mMutexv2x.lock();
    xv2xmsg = mv2xmsg;
    mMutexv2x.unlock();
    return true;
}

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

    std::shared_ptr<iv::ultrasonic::ultrasonic> xultrasonic_ptr
            = std::shared_ptr<iv::ultrasonic::ultrasonic>(new iv::ultrasonic::ultrasonic);

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

    mMutexultrasonic.lock();
    multrasonicmsg = xultrasonic_ptr;
    mnultrasonicUpdateTime = QDateTime::currentMSecsSinceEpoch();
    mMutexultrasonic.unlock();
}

bool ivdecision::Getultrasonic(std::shared_ptr<ultrasonic::ultrasonic> &xultramsg)
{
    if((QDateTime::currentMSecsSinceEpoch() - mnultrasonicUpdateTime)>mnNewThresh)
    {
        xultramsg = NULL;
        return false;
    }
    mMutexultrasonic.lock();
    xultramsg = multrasonicmsg;
    mMutexultrasonic.unlock();
    return true;
}

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

    std::shared_ptr<iv::radio::radio_send> xp900_ptr
            = std::shared_ptr<iv::radio::radio_send>(new iv::radio::radio_send);

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

    mMutexp900.lock();
    mp900msg = xp900_ptr;
    mnp900UpdateTime = QDateTime::currentMSecsSinceEpoch();
    mMutexp900.unlock();
}

bool ivdecision::Getp900(std::shared_ptr<iv::radio::radio_send> & xp900msg)
{
    if((QDateTime::currentMSecsSinceEpoch() - mnp900UpdateTime)>mnNewThresh)
    {
        xp900msg = NULL;
        return false;
    }
    mMutexp900.lock();
    xp900msg = mp900msg;
    mMutexp900.unlock();
    return true;
}

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

    std::shared_ptr<iv::chassis> xchassis_ptr
            = std::shared_ptr<iv::chassis>(new iv::chassis);


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

    mMutexchassis.lock();
    mchassismsg = xchassis_ptr;
    mnchassisUpdateTime = QDateTime::currentMSecsSinceEpoch();
    mMutexchassis.unlock();
}

bool ivdecision::Getchassis(std::shared_ptr<iv::chassis> & xchassismsg)
{
    if((QDateTime::currentMSecsSinceEpoch() - mnchassisUpdateTime)>mnNewThresh)
    {
        xchassismsg = NULL;
        return false;
    }
    mMutexchassis.lock();
    xchassismsg = mchassismsg;
    mMutexchassis.unlock();
    return true;
}

}