#include "vtd_pilot.h"


static void CalcXY(const double lat0,const double lon0,
              const double lat,const double lon,
              double & x,double & y)
{

    double x0,y0;
    GaussProjCal(lon0,lat0,&x0,&y0);
    GaussProjCal(lon,lat,&x,&y);
    x = x - x0;
    y = y-  y0;
}

vtd_pilot::vtd_pilot(std::string strfromvtd,std::string strtovtd)
{
    (void)strfromvtd;
    ModuleFun funvtd =std::bind(&vtd_pilot::UpdateVTD,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mpfromvtd = iv::modulecomm::RegisterRecvPlus(strfromvtd.data(),funvtd);

    mptovtd = iv::modulecomm::RegisterSend(strtovtd.data(),10000,1) ;

    ModuleFun fungpsimu =std::bind(&vtd_pilot::UpdateGPSIMU,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mpagpsimu = iv::modulecomm::RegisterRecvPlus("hcp2_gpsimu",fungpsimu);

    mpaegostate = iv::modulecomm::RegisterSend("egostate",10000,1);

    mpthread = new std::thread(&vtd_pilot::threadego,this);

    memset( &sOwnObjectState, 0, sizeof( RDB_OBJECT_STATE_t ) );
}

void vtd_pilot::UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{

    (void)index;
    (void)dt;
    (void)strmemname;
    iv::gps::gpsimu  xgpsimu;
    if(!xgpsimu.ParseFromArray(strdata,nSize))
    {
        std::cout<<"parse gpsimu fail"<<std::endl;
        return;
    }

    mmutex.lock();
    mgpsimu.CopyFrom(xgpsimu);
    mbupdate_gps = true;
    mmutex.unlock();

    mcv.notify_all();


}

void vtd_pilot::ConvertToObjectArray(std::vector<iv::vtd::rdbdata> &xvectorrdbdata, iv::fusion::fusionobjectarray &xfusionarray)
{
    int i;
    int j;
    for(i=0;i<(int)xvectorrdbdata.size();i++)
    {
        iv::vtd::rdbdata * prdbdata = &xvectorrdbdata[i];
        for(j=0;j<prdbdata->mrdbitem_size();j++)
        {
            iv::vtd::rdbitem * pitem = prdbdata->mutable_mrdbitem(j);
            if(pitem->pkgid() == RDB_PKG_ID_OBJECT_STATE)
            {
                RDB_OBJECT_STATE_t xobj;
    //            std::cout<<" structure RDB_OBJECT_STATE_t size: "<<sizeof(xobj)<<"data size: "<<xrdbitem.mnpkgdatasize<<std::endl;
                memcpy(&xobj,pitem->pkgdata().data(),sizeof(RDB_OBJECT_STATE_t));
                std::cout<<"id:"<<xobj.base.id<<" name:"<<xobj.base.name<< " pos : x "<<xobj.base.pos.x<<" y "<<xobj.base.pos.y<<" z "<<xobj.base.pos.z<<std::endl;
                iv::fusion::fusionobject * pobject = NULL;
                int k;
                for(k=0;k<xfusionarray.obj_size();k++)
                {
                    if(xfusionarray.mutable_obj(k)->id() == xobj.base.id)
                    {
                        pobject = xfusionarray.mutable_obj(k);
                        break;
                    }
                }
                if(pobject  == NULL)
                {
                    pobject = xfusionarray.add_obj();
                }
                pobject->set_id(xobj.base.id);
                double x,y;
                x = xobj.base.pos.x - mfX;
                y = xobj.base.pos.y - mfY;
                double relx,rely;
                double beta  = mfHeading*(-1.0);
                relx = x*cos(beta) - y*sin(beta);
                rely = x*sin(beta) + y*sin(beta);
                double vx,vy;
                vx = xobj.ext.speed.x;
                vy = xobj.ext.speed.y;
                vx = vx - mfvx;
                vy = vy - mfvy;
                double relvx,relvy;
                relvx = vx*cos(beta) - vy*sin(beta);
                relvy = vx*sin(beta) + vy*sin(beta);
                double fobjheading = xobj.base.pos.h;
                fobjheading = fobjheading - mfHeading;
                pobject->set_lifetime(100);
                pobject->set_prob(1.0);
                pobject->set_sensor_type(1);
                pobject->set_yaw(fobjheading);
                iv::fusion::PointXYZ * ppointxyz = new iv::fusion::PointXYZ;
                ppointxyz->set_x(relx);
                ppointxyz->set_y(rely);
                ppointxyz->set_z(0);
                iv::fusion::Dimension * pdim = new iv::fusion::Dimension;
                pdim->set_x(xobj.base.geo.dimX);
                pdim->set_y(xobj.base.geo.dimY);
                pdim->set_z(xobj.base.geo.dimZ);
                pobject->set_allocated_centroid(ppointxyz);
                pobject->set_allocated_dimensions(pdim);
                pobject->set_velocity_linear_x(sqrt(pow(relvx,2)+pow(relvy,2)));


            }
        }
    }
}

void vtd_pilot::threadobject()
{
    std::vector<iv::vtd::rdbdata> xvectorrdbdata;
    int64_t nlastsharetime = std::chrono::system_clock::now().time_since_epoch().count();
//    int nmaxobjid = 2;
    int nshareinter = 100;  //100 ms share a data.
    bool bshare = false;
    while(mbRun)
    {
        int64_t now = std::chrono::system_clock::now().time_since_epoch().count();
        int64_t timediff = abs(now-nlastsharetime)/1000000;
        if(timediff >= nshareinter)
        {
            bshare = true;
        }
        if(bshare)
        {
            //share object data.
            bshare = false;
            nlastsharetime = now;
            xvectorrdbdata.clear();
        }
        std::unique_lock<std::mutex> lk(mmutexcvrdb);
        if(mcvrdb.wait_for(lk,std::chrono::milliseconds(10)) == std::cv_status::timeout)
        {
            lk.unlock();
            continue;
        }
        else
        {
            lk.unlock();

        }
        mmutexrdb.lock();
        int i;
        for(i=0;i<(int)mvectorrdbdata.size();i++)
        {
            xvectorrdbdata.push_back(mvectorrdbdata[i]);
        }
        mmutexrdb.unlock();
    }
}

void vtd_pilot::UpdateVTD(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
{
    (void)index;
    (void)dt;
    (void)strmemname;

    iv::vtd::rdbdata xrdbdata;
    if(xrdbdata.ParseFromArray(strdata,nSize))
    {
        mmutexrdb.lock();
        mvectorrdbdata.push_back(xrdbdata);
        mmutexrdb.unlock();
        mcvrdb.notify_all();
        if(xrdbdata.mrdbitem_size()>0)
        {
            iv::vtd::rdbitem * pitem = xrdbdata.mutable_mrdbitem(0);
            msimFrame = pitem->simframe();
            msimTime = pitem->simtime();
            if(pitem->pkgid() == RDB_PKG_ID_OBJECT_STATE)
            {

            }
        }
    }
}

void vtd_pilot::setOwnState(double x, double y, double z,double vx,double vy,double fheading)
{
    sOwnObjectState.base.id       = 1;
    sOwnObjectState.base.category = RDB_OBJECT_CATEGORY_PLAYER;
    sOwnObjectState.base.type     = RDB_OBJECT_TYPE_PLAYER_CAR;
    strcpy( sOwnObjectState.base.name, "Ego" );

    // dimensions of own vehicle
    sOwnObjectState.base.geo.dimX = 4.60 ;//* dimFactor;
    sOwnObjectState.base.geo.dimY = 1.86 ;//* dimFactor;
    sOwnObjectState.base.geo.dimZ = 1.60 ;//* dimFactor;


    // offset between reference point and center of geometry
    sOwnObjectState.base.geo.offX = 0.80;
    sOwnObjectState.base.geo.offY = 0.00;
    sOwnObjectState.base.geo.offZ = 0.30;

    sOwnObjectState.base.pos.x     = x;
    sOwnObjectState.base.pos.y     = y;
    sOwnObjectState.base.pos.z     = z;
    sOwnObjectState.base.pos.h     = fheading;
    sOwnObjectState.base.pos.p     = 0.0;
    sOwnObjectState.base.pos.r     = 0.0;
    sOwnObjectState.base.pos.flags = RDB_COORD_FLAG_POINT_VALID | RDB_COORD_FLAG_ANGLES_VALID;

    sOwnObjectState.ext.speed.x     = vx;
    sOwnObjectState.ext.speed.y     = vy;
    sOwnObjectState.ext.speed.z     = 0;
    sOwnObjectState.ext.speed.h     = 0.0;
    sOwnObjectState.ext.speed.p     = 0.0;
    sOwnObjectState.ext.speed.r     = 0.0;
    sOwnObjectState.ext.speed.flags = RDB_COORD_FLAG_POINT_VALID | RDB_COORD_FLAG_ANGLES_VALID;

    sOwnObjectState.ext.accel.x     = 0.0;
    sOwnObjectState.ext.accel.y     = 0.0;
    sOwnObjectState.ext.accel.z     = 0.0;
    sOwnObjectState.ext.accel.flags = RDB_COORD_FLAG_POINT_VALID;

    sOwnObjectState.base.visMask    =  RDB_OBJECT_VIS_FLAG_TRAFFIC | RDB_OBJECT_VIS_FLAG_RECORDER;
}

void vtd_pilot::threadego()
{
    while(mbRun)
    {
        if(mbupdate_gps == false)
        {
            std::unique_lock<std::mutex> lk(mmutexcv);
            if(mcv.wait_for(lk,std::chrono::milliseconds(100)) == std::cv_status::timeout)
            {
                lk.unlock();
                continue;
            }
            else
            {
                lk.unlock();
                continue;
            }
        }

        iv::gps::gpsimu xgpsimu;
        mmutex.lock();
        xgpsimu.CopyFrom(mgpsimu);
        mbupdate_gps = false;
        mmutex.unlock();

        double x,y,z;
        CalcXY(mflat0,mflon0,xgpsimu.lat(),xgpsimu.lon(),x,y);
        double vx = xgpsimu.ve();
        double vy = xgpsimu.vn();
        z = 0;
        double fheading = (90 - xgpsimu.heading())*M_PI/180.0;
        setOwnState(x,y,z,vx,vy,fheading);

        mfX = x;
        mfY = y;
        mfHeading = fheading;
        mfvx = vx;
        mfvy = vy;

 //       RDB_OBJECT_STATE_t * pr = &sOwnObjectState;

        iv::modulecomm::ModuleSendMsg(mpaegostate,(char *)&sOwnObjectState,sizeof(RDB_OBJECT_STATE_t));



        //convert to x,y,z

        //send data.
    }
}