#include "collectconvert.h"

#include "gnss_coordinate_convert.h"
#include "geofit.h"

#include "autoconnect.h"

int groadid = 10001;
CollectConvert::CollectConvert(iv::map::collectveh & xcollectveh,OpenDrive * pxodr)
{
    mcollectveh.CopyFrom(xcollectveh);
    mpxodr = pxodr;
}

void CollectConvert::StartConvert()
{
    threadconvert();
}

void CollectConvert::threadconvert()
{
    OpenDrive * pxodr = mpxodr;
    pxodr->SetHeader(1,1,"adc",1.1,"2022",0,0,0,0,mcollectveh.mlat0(),mcollectveh.mlon0(),360);
    int i;
    for(i=0;i<mcollectveh.mroads_size();i++)
    {
        iv::map::collectvehroad * proad = mcollectveh.mutable_mroads(i);
        ConvertRoad(proad,pxodr);

    }
    for(i=0;i<mcollectveh.mconnects_size();i++)
    {
        iv::map::roadconnect * pconnect = mcollectveh.mutable_mconnects(i);
        convertconnect(pconnect,pxodr);
    }
    AutoConnect pAC(pxodr);
    pAC.Connect();
}

void CollectConvert::ConvertRoad(iv::map::collectvehroad *proad, OpenDrive *pxodr)
{
    if(proad->mpoints_size() == 0)return;
    double flon0,flat0;
    pxodr->GetHeader()->GetLat0Lon0(flat0,flon0);
//    pxodr->GetHeader()->GetLat0Lon0(flon0,flat0);
    double x0,y0;
    GaussProjCal(flon0,flat0,&x0,&y0);
    int i;
    std::vector<iv::collectxy> xvectorcollectxy;
    for(i=0;i<proad->mpoints_size();i++)
    {
        iv::map::collectvehroadpoint * ppoint = proad->mutable_mpoints(i);
        double x,y;
//        double fl1 = ppoint->mflon();
//        double fl2 = ppoint->mflat();
        GaussProjCal(ppoint->mflon(),ppoint->mflat(),&x,&y);
        iv::collectxy xc;
        xc.mLaneColor_Left = ppoint->mlanecolor_left();
        xc.mLaneColor_Right = ppoint->mlanecolor_right();
        xc.mLaneType_Left = ppoint->mlanetype_left();
        xc.mLaneType_Right = ppoint->mlanetype_right();
        xc.height = 0;
        xc.width = ppoint->mflanewidth();
        xc.fhdg = (90 -ppoint->mfheading())*M_PI/180.0;
        xc.x = x-x0 + ppoint->mfdis_left()*cos(xc.fhdg +M_PI/2.0);
        xc.y = y-y0 + ppoint->mfdis_left()*sin(xc.fhdg +M_PI/2.0);
        xvectorcollectxy.push_back(xc);
        if(i>0)
        {
            double fdis =sqrt(pow(xvectorcollectxy[i].x - xvectorcollectxy[i-1].x,2)+pow(xvectorcollectxy[i].y-xvectorcollectxy[i-1].y,2));
            xvectorcollectxy[i].fdistolast = fdis;

        }
    }


    double LINE_ERROR = 0.20;


    std::vector<geobase> xvectorgeo;

    bool bComplete = false;
    int j;
    int ncurpos = 0;
    int nrange = xvectorcollectxy.size();
    bool bHaveLastxyhdg = false;
    double fLastX,fLastY,fLastHdg;
    double fEndX,fEndY,fEndHdg;
    double fStartX,fStartY,fStartHdg;
    while(bComplete == false)
    {

        VectorXd x_veh(nrange);
        VectorXd y_veh(nrange);
        for(j=0;j<nrange;j++)
        {
            x_veh[j] =xvectorcollectxy[j+ncurpos].x;//  xvectorvehicle[xvectorvl[j+ncurpos].mvehindex].fx;//mvectorrtkdata.at(j+ncurpos).mfrelx;
            y_veh[j] = xvectorcollectxy[j+ncurpos].y;//xvectorvehicle[xvectorvl[j+ncurpos].mvehindex].fy;//mvectorrtkdata.at(j+ncurpos).mfrely;
        }

        bool bArcOk = false;
        bool bLineOk = false;
        double dismax = 0;
        QPointF sp,ep;
        double fR,flen;
        double fhdgstart,fhdgend;
        if(nrange >= 3)
        {

            geofit::Inst().arcfitanddis(x_veh,y_veh,dismax,fR,sp,ep,fhdgstart,fhdgend,flen);

            if(dismax < LINE_ERROR)
            {
                bArcOk = true;
  //              std::cout<<" a arc is ok "<<"pos is "<<ncurpos<<" range is "<<nrange<<" error is "<<dismax<<std::endl;
            }
        }


        auto coeffs = polyfit(x_veh, y_veh, 1);

        dismax = 0;
        for(j=0;j<nrange;j++)
        {
            double A = coeffs[1];
            double B = -1;
            double C = coeffs[0];
            double dis = fabs(A*x_veh[j] + B*y_veh[j] +C )/sqrt(pow(A,2)+pow(B,2));
            if(dis>dismax)dismax = dis;
        }
        if(dismax < LINE_ERROR)
        {
            bLineOk = true;
 //           std::cout<<" a line is ok "<<"pos is "<<ncurpos<<" range is "<<nrange<<" error is "<<dismax<<std::endl;
        }

        if((nrange <= 2)||bLineOk||bArcOk)
        {
            if((bLineOk)||(nrange<=2))
            {
 //               std::cout<<"use line"<<std::endl;
                geobase xgeo;
                xgeo.mnStartPoint = ncurpos;
                xgeo.mnEndPoint = ncurpos + nrange -1 ;

                double x0,y0,x1,y1;
                geofit::Inst().getcrosspoint(coeffs[1],-1,coeffs[0],x_veh[0],
                        y_veh[0],x0,y0);
                geofit::Inst().getcrosspoint(coeffs[1],-1,coeffs[0],x_veh[nrange -1],
                        y_veh[nrange - 1],x1,y1);
                xgeo.mfA = coeffs[1];
                xgeo.mfB = -1;
                xgeo.mfC = coeffs[0];
                xgeo.mfHdg = geofit::Inst().CalcHdg(x0,y0,x1,y1);
                xgeo.mfX = x0;
                xgeo.mfY = y0;
                xgeo.mfLen = sqrt(pow(x1-x0,2)+pow(y1-y0,2));
                xgeo.mnType = 0;
                fStartX = x0;
                fStartY = y0;
                fStartHdg = xgeo.mfHdg;
                fEndX = x1;
                fEndY = y1;
                fEndHdg = xgeo.mfHdg;

                xvectorgeo.push_back(xgeo);
            }
            else
            {
                if(bArcOk)
                {
                    std::cout<<"use arc"<<std::endl;
                    geobase xgeo;
                    xgeo.mnStartPoint = ncurpos;
                    xgeo.mnEndPoint = ncurpos + nrange -1;
                    xgeo.mfHdg = fhdgstart;
                    xgeo.mfHdgStart = fhdgstart;
                    xgeo.mfHdgEnd = fhdgend;
                    xgeo.mfX = sp.x();
                    xgeo.mfY = sp.y();
                    xgeo.mfLen = flen;
                    xgeo.mnType = 1;
                    xgeo.mfEndX = ep.x();
                    xgeo.mfEndY = ep.y();
                    xgeo.mR = fR;
                    fStartX = xgeo.mfX;
                    fStartY = xgeo.mfY;
                    fStartHdg = xgeo.mfHdgStart;
                    fEndX = xgeo.mfEndX;
                    fEndY = xgeo.mfEndY;
                    fEndHdg = xgeo.mfHdgEnd;
                    xvectorgeo.push_back(xgeo);
                }
            }
            //        std::cout<<" a line is ok "<<"pos is "<<ncurpos<<" range is "<<nrange<<" error is "<<dismax<<std::endl;

            if(bHaveLastxyhdg)
            {
                std::vector<geobase> xvectorgeobe = geofit::CreateBezierGeo(fLastX,fLastY,
                                                                          fLastHdg,fStartX,fStartY,fStartHdg);
                if(xvectorgeobe.size()>0)
                {
                    xvectorgeobe[0].mnStartPoint = ncurpos-1;
                    xvectorgeobe[0].mnEndPoint = ncurpos;
                    xvectorgeo.insert(xvectorgeo.begin()+(xvectorgeo.size()-1),xvectorgeobe[0]);
                }
            }
            bHaveLastxyhdg = true;
            fLastX = fEndX;
            fLastY = fEndY;
            fLastHdg = fEndHdg;


 //           ncurpos = ncurpos + nrange -1;
            ncurpos = ncurpos + nrange;  //Skip 1 point, use bezier
            nrange = xvectorcollectxy.size() - ncurpos;

        }
        else
        {
            if(nrange > 30)
                nrange = nrange/2;
            else
                nrange = nrange -1;
            if(nrange<2)nrange = 2;
        }
        if(ncurpos>=(int)(xvectorcollectxy.size()-1))
        {
            if(ncurpos == (int)(xvectorcollectxy.size()-1))
            {
 //               std::cout<<"Last section, 2 Points, use line."<<std::endl;
                VectorXd x_veh1(2);
                VectorXd y_veh1(2);
                x_veh1[0] = fLastX;
                y_veh1[0] = fLastY;
                x_veh1[1] =  xvectorcollectxy[ncurpos].x;// xvectorvehicle[xvectorvl[ncurpos].mvehindex].fx;
                y_veh1[1] = xvectorcollectxy[ncurpos].y;// xvectorvehicle[xvectorvl[ncurpos].mvehindex].fy;
                auto coeffs = polyfit(x_veh1, y_veh1, 1);

                geobase xgeo;
                xgeo.mnStartPoint = ncurpos-1;
                xgeo.mnEndPoint = ncurpos;

                xgeo.mfA = coeffs[1];
                xgeo.mfB = -1;
                xgeo.mfC = coeffs[0];
                xgeo.mfHdg = geofit::Inst().CalcHdg(x_veh1[0],y_veh1[0],x_veh1[1],y_veh1[1]);
                xgeo.mfX = x_veh1[0];
                xgeo.mfY = y_veh1[0];
                xgeo.mfLen = sqrt(pow(x_veh1[1]-x_veh1[0],2)+pow(y_veh1[1]-y_veh1[0],2));
                xgeo.mnType = 0;
                xvectorgeo.push_back(xgeo);
            }
            bComplete = true;
        }
    }

    double snow = 0;
    double froadlen = 0;
    for(i=0;i<(int)xvectorgeo.size();i++)
    {
        geobase * pgeo = &xvectorgeo[i];
        froadlen = froadlen + xvectorgeo[i].mfLen;
//        std::cout<<"geo len: "<<pgeo->mfLen<<" type"<<pgeo->mnType<<std::endl;
        double tems = 0;
        xvectorcollectxy[pgeo->mnStartPoint].s = snow;
        for(j=(pgeo->mnStartPoint+1);j<=pgeo->mnEndPoint;j++)
        {
            xvectorcollectxy[j].s = snow + tems+xvectorcollectxy[j].fdistolast;
  //          xvectorvehicle[xvectorvl[j].mvehindex].s = snow + tems + xvectorvehicle[xvectorvl[j].mvehindex].fdistolast;
            tems = tems + xvectorcollectxy[j].fdistolast;
        }
        snow = snow + pgeo->mfLen;
    }

    std::vector<iv::widthabcd> xvectorwidthabcd;
    bComplete = false;
    ncurpos = 0;
    nrange = xvectorcollectxy.size();
    double flanewidtherror = 0.1; //1cm
    while(bComplete == false)
    {
        double ele_coff[4];
        for(j=0;j<4;j++)ele_coff[j] = 0;
        if(nrange>0)ele_coff[0] = xvectorcollectxy[ncurpos].width;// xvectorvehicle[xvectorvl[ncurpos].mvehindex].flanewidth;
        int M = nrange;
        VectorXd x_vehhg(M);
        VectorXd y_vehhg(M);
        for(j=0;j<M;j++)
        {
            x_vehhg[j] = xvectorcollectxy[ncurpos+j].s - xvectorcollectxy[ncurpos].s;//   xvectorvehicle[xvectorvl[ncurpos+j].mvehindex].s - xvectorvehicle[xvectorvl[ncurpos].mvehindex].s;
            y_vehhg[j] = xvectorcollectxy[ncurpos+j].width;//    xvectorvehicle[xvectorvl[ncurpos+j].mvehindex].flanewidth;
        }
        int MX = 3;
        if(M<4)MX = M -1;
        if(MX>3)MX = 3;
        if(MX>0)
        {
            VectorXd coeffs = polyfit(x_vehhg, y_vehhg, MX);
            for(j=0;j<=MX;j++)
            {
                ele_coff[j] = coeffs[j];
            }
        }
        double ferror = 0;
        for(j=0;j<M;j++)
        {
            double s = x_vehhg[j];
            double fvalue = fabs(ele_coff[0] + ele_coff[1]*s + ele_coff[2]*s*s+ele_coff[3]*s*s*s - y_vehhg[j]);
            if(fvalue>ferror)ferror = fvalue;
        }
        if(ferror>flanewidtherror)
        {
            if(nrange>10)nrange = nrange/2;
            else nrange = nrange -1;
        }
        else
        {
            iv::widthabcd xabcd;
            xabcd.s =  xvectorcollectxy[ncurpos].s;//  xvectorvehicle[xvectorvl[ncurpos].mvehindex].s;
            xabcd.A = ele_coff[0];
            xabcd.B = ele_coff[1];
            xabcd.C = ele_coff[2];
            xabcd.D = ele_coff[3];
            xvectorwidthabcd.push_back(xabcd);
            ncurpos = ncurpos + nrange-1;
            nrange = xvectorcollectxy.size() - ncurpos;
        }

        if(ncurpos>=(int)(xvectorcollectxy.size()-1))
        {
            bComplete = true;
        }
    }

    pxodr->AddRoad(proad->strroadname(),froadlen, QString::number(groadid).toStdString(),"-1");
    Road * p = pxodr->GetRoad(pxodr->GetRoadCount() - 1);
    groadid++;

  //  p->AddElevation(0,xlaneheightcoff.A,xlaneheightcoff.B,xlaneheightcoff.C,xlaneheightcoff.D);

    double s = 0;
    j= 0;
//        for(j=0;j<4;j++)
    for(j=0;j<(int)xvectorgeo.size();j++)
    {

        p->AddGeometryBlock();
        GeometryBlock * pgb = p->GetGeometryBlock(j);

        geobase * pline;
        geobase * pbez;
        geobase * parc;

        switch(xvectorgeo[j].mnType)
        {
        case 0:
            pline = &xvectorgeo[j];
            pgb->AddGeometryLine(s,pline->mfX,pline->mfY,pline->mfHdg,pline->mfLen);
            break;
        case 1:
            parc = &xvectorgeo[j];
            pgb->AddGeometryArc(s,parc->mfX,parc->mfY,parc->mfHdgStart,parc->mfLen,1.0/parc->mR);
            break;
        case 2:
            pbez = &xvectorgeo[j];
//            std::cout<<"u0:"<<pbez->mfu[0]<<std::endl;
            pgb->AddGeometryParamPoly3(s,pbez->mfX,pbez->mfY,
                                       pbez->mfHdg,pbez->mfLen,pbez->mfu[0],
                                       pbez->mfu[1],pbez->mfu[2],pbez->mfu[3],pbez->mfv[0],
                                       pbez->mfv[1],pbez->mfv[2],pbez->mfv[3],false);
            break;
        }
        s = s + xvectorgeo[j].mfLen;
    }

    p->AddLaneSection(0);

    LaneSection * pLS = p->GetLaneSection(0);

    pLS->AddLane(0,0,"none",false);
    Lane * pCenterLane = pLS->GetLastAddedLane();
    iv::map::collectvehroadpoint_LaneType xLastType;
    if(xvectorcollectxy.size()>0)
    {
        xLastType = xvectorcollectxy[0].mLaneType_Left;
        std::string strlanecolor = "white";
        if(xvectorcollectxy[0].mLaneColor_Left == iv::map::collectvehroadpoint_LaneColor_YELLOW)strlanecolor = "yellow";
        pCenterLane->AddRoadMarkRecord(xvectorcollectxy[0].s,LaneTypeToStr(xLastType),
                "standard",strlanecolor,0.15,"none");
    }
    for(i=1;i<(int)xvectorcollectxy.size();i++)
    {
        if(xvectorcollectxy[i].mLaneType_Left != xLastType)
        {
            xLastType = xvectorcollectxy[i].mLaneType_Left;
            std::string strlanecolor = "white";
            if(xvectorcollectxy[i].mLaneColor_Left == iv::map::collectvehroadpoint_LaneColor_YELLOW)strlanecolor = "yellow";
            pCenterLane->AddRoadMarkRecord(xvectorcollectxy[i].s,LaneTypeToStr(xLastType),
                    "standard",strlanecolor,0.15,"none");
        }
    }
    pLS->AddLane(-1,(-1)*(pLS->GetRightLaneCount()+1),"driving",false);
    Lane * pLane = pLS->GetLastRightLane();
    for(j=0;j<(int)xvectorwidthabcd.size();j++)
    {
        iv::widthabcd * pwa = &xvectorwidthabcd[j];
        pLane->AddWidthRecord(pwa->s,pwa->A,pwa->B,pwa->C,pwa->D);
    }

    if(xvectorcollectxy.size()>0)
    {
        xLastType = xvectorcollectxy[0].mLaneType_Right;
        std::string strlanecolor = "white";
        if(xvectorcollectxy[0].mLaneColor_Right == iv::map::collectvehroadpoint_LaneColor_YELLOW)strlanecolor = "yellow";
        pLane->AddRoadMarkRecord(xvectorcollectxy[0].s,LaneTypeToStr(xLastType),
                "standard",strlanecolor,0.15,"none");
    }
    for(i=1;i<(int)xvectorcollectxy.size();i++)
    {
        if(xvectorcollectxy[i].mLaneType_Right != xLastType)
        {
            xLastType = xvectorcollectxy[i].mLaneType_Right;
            std::string strlanecolor = "white";
            if(xvectorcollectxy[i].mLaneColor_Right == iv::map::collectvehroadpoint_LaneColor_YELLOW)strlanecolor = "yellow";
            pLane->AddRoadMarkRecord(xvectorcollectxy[i].s,LaneTypeToStr(xLastType),
                    "standard",strlanecolor,0.15,"none");
        }
    }




}

std::string CollectConvert::LaneTypeToStr(iv::map::collectvehroadpoint_LaneType xType)
{
    switch (xType) {
    case iv::map::collectvehroadpoint_LaneType_NONE:
        return "none";
        break;
    case iv::map::collectvehroadpoint_LaneType_SOLID:
        return "solid";
        break;
    case iv::map::collectvehroadpoint_LaneType_DASH:
        return "broken";
        break;
    case iv::map::collectvehroadpoint_LaneType_SOLID_SOLID:
        return "solid solid";
        break;
    case iv::map::collectvehroadpoint_LaneType_SOLID_DASH:
        return "solid broken";
        break;
    case iv::map::collectvehroadpoint_LaneType_DASH_SOLID:
        return "broken solid";
        break;
    case iv::map::collectvehroadpoint_LaneType_DASH_DASH:
        return "broken broken";
        break;
    default:
        return "none";
        break;
    }
}

void CollectConvert::convertconnect(iv::map::roadconnect *pconnect, OpenDrive *pxodr)
{
    Road * p1, * p2;
    std::string strroad1name = pconnect->strroad1();
    std::string strroad2name = pconnect->strroad2();
    int i;
    p1 = NULL;
    p2 = NULL;
    double fwidth1,fwidth2;
    for(i=0;i<(int)pxodr->GetRoadCount();i++)
    {
        if(pxodr->GetRoad(i)->GetRoadName() == strroad1name)
        {
            p1 = pxodr->GetRoad(i);
            break;
        }
    }
    for(i=0;i<(int)pxodr->GetRoadCount();i++)
    {
        if(pxodr->GetRoad(i)->GetRoadName() == strroad2name)
        {
            p2 = pxodr->GetRoad(i);
            break;
        }
    }
    if((p1 == NULL)||(p2 == NULL))
    {
        std::cout<<"convertconnect can not find road. "<<std::endl;
        return;
    }
    double startx,starty,starthdg;
    double endx,endy,endhdg;
    p1->GetGeometryCoords(p1->GetRoadLength() - 0.001,startx,starty,starthdg);
    p2->GetGeometryCoords(0,endx,endy,endhdg);

//    std::cout<<"start x : "<<startx<<" start y: "<<starty<<std::endl;

    LaneSection * pLS1 = p1->GetLastLaneSection();
    if(pLS1 == NULL)
    {
        std::cout<<" road p1 no lanesection."<<std::endl;
        return;
    }
    Lane * pLane = NULL;
    for(i=0;i<(int)pLS1->GetLaneCount();i++)
    {
        Lane * pLaneX = pLS1->GetLane(i);
        if(pLaneX->GetId() == -1)
        {
            pLane = pLaneX;
            break;
        }
    }
    if(pLane == NULL)
    {
        std::cout<<"can't get road1 lane."<<std::endl;
        return;
    }
    fwidth1 = pLane->GetWidthValue(p1->GetRoadLength() - 0.001);

     pLS1 = p2->GetLaneSection(0);
    if(pLS1 == NULL)
    {
        std::cout<<" road p2 no lanesection."<<std::endl;
        return;
    }
    pLane = NULL;
    for(i=0;i<(int)pLS1->GetLaneCount();i++)
    {
        Lane * pLaneX = pLS1->GetLane(i);
        if(pLaneX->GetId() == -1)
        {
            pLane = pLaneX;
            break;
        }
    }
    if(pLane == NULL)
    {
        std::cout<<"can't get road2 lane."<<std::endl;
        return;
    }
    fwidth2 = pLane->GetWidthValue(0.001);

    //Create Geo
    double R = 6.0;
    std::vector<geobase> xvectorgeo;
    std::vector<geobase> xvectorgeo1,xvectorgeo2;
    switch(pconnect->mmode())
    {
    case iv::map::roadconnect_ConnectMode_TURN:
        xvectorgeo = geofit::CreateTurnGeo(startx,starty,starthdg,endx,endy,endhdg,R);
        break;
    case iv::map::roadconnect_ConnectMode_STRAIGHT:
        {
            double fdis = sqrt(pow(startx - endx,2) +pow(starty -endy,2));
            if((fdis<3) || (starthdg == endhdg) )
            {
                xvectorgeo = geofit::CreateLineGeo(startx,starty,starthdg,endx,endy,endhdg);
            }
            else
            {
                xvectorgeo = geofit::CreateBezierGeo(startx,starty,starthdg,endx,endy,endhdg);
            }
//
        }
        break;
    case iv::map::roadconnect_ConnectMode_UTURN:
        xvectorgeo = geofit::CreateUTurnGeo(startx,starty,starthdg,endx,endy,endhdg,R);
        break;
    default:
        break;
    }

    if(pconnect->mmode() == iv::map::roadconnect_ConnectMode_UTURN)
    {
        for(i=0;i<(int)xvectorgeo.size()/2;i++)
        {
            xvectorgeo1.push_back(xvectorgeo.at(i));
        }
        for(i=(int)xvectorgeo.size()/2;i<(int)xvectorgeo.size();i++)
        {
            xvectorgeo2.push_back(xvectorgeo.at(i));
        }
    }

    if(xvectorgeo.size() == 0)
    {
        std::cout<<" CollectConvert::convertconnect Create Road Fail."<<std::endl;
//        QMessageBox::warning(this,"warn","Create Road Fail.");
        return;
    }

    double xroadlen = 0;

    if(pconnect->mmode() != iv::map::roadconnect_ConnectMode_UTURN)
    {
        for(i=0;i<(int)xvectorgeo.size();i++)xroadlen = xroadlen + xvectorgeo[i].mfLen;

        pxodr->AddRoad("",xroadlen, QString::number(groadid).toStdString(),"-1");
        groadid++;
        Road * p = pxodr->GetRoad(pxodr->GetRoadCount() - 1);

//        p->AddElevation(0,startheight,(endheight-startheight)/xroadlen,0,0);


        double s = 0;
        int j;
        j= 0;
        for(j=0;j<(int)xvectorgeo.size();j++)
        {
            p->AddGeometryBlock();
            GeometryBlock * pgb = p->GetGeometryBlock(j);

            geobase * pline;
            geobase * pbez;
            geobase * parc;
            switch(xvectorgeo[j].mnType)
            {
            case 0:
                pline = &xvectorgeo[j];
                pgb->AddGeometryLine(s,pline->mfX,pline->mfY,pline->mfHdg,pline->mfLen);
                break;
            case 1:
                parc = &xvectorgeo[j];
                pgb->AddGeometryArc(s,parc->mfX,parc->mfY,parc->mfHdgStart,parc->mfLen,1.0/parc->mR);
                break;
            case 2:
                pbez = &xvectorgeo[j];
                pgb->AddGeometryParamPoly3(s,pbez->mfX,pbez->mfY,
                                           pbez->mfHdg,pbez->mfLen,pbez->mfu[0],
                        pbez->mfu[1],pbez->mfu[2],pbez->mfu[3],pbez->mfv[0],
                        pbez->mfv[1],pbez->mfv[2],pbez->mfv[3],false);
                break;
            }
            s = s + xvectorgeo[j].mfLen;
        }

        p->AddLaneSection(0);
        LaneSection * pLS = p->GetLaneSection(0);
        pLS->SetS(0);
        pLS->AddLane(0,0,"none",false);

        double b = (fwidth2 - fwidth1)/xroadlen;

        pLS->AddLane(-1,-1,"driving",false);
        pLane = pLS->GetLastAddedLane();
        pLane->AddWidthRecord(0,fwidth1,b,0,0);



    }
    else
    {
        double xroadlen1 = 0;
        double xroadlen2 = 0;
        for(i=0;i<(int)xvectorgeo1.size();i++)xroadlen1 = xroadlen1 + xvectorgeo1[i].mfLen;
        for(i=0;i<(int)xvectorgeo2.size();i++)xroadlen2 = xroadlen2 + xvectorgeo2[i].mfLen;

        int index1 = pxodr->AddRoad("",xroadlen1, QString::number(groadid).toStdString(),"-1");groadid++;
        int index2 = pxodr->AddRoad("",xroadlen2, QString::number(groadid).toStdString(),"-1");groadid++;
        Road * proad2 = pxodr->GetRoad(index2);
        Road * proad1 = pxodr->GetRoad(index1);


//        OpenDrive * px = &mxodr;
        double s = 0;
        int j;
        j= 0;
        for(j=0;j<(int)xvectorgeo1.size();j++)
        {
            proad1->AddGeometryBlock();
            GeometryBlock * pgb = proad1->GetGeometryBlock(j);

            geobase * pline;
            geobase * pbez;
            geobase * parc;
            switch(xvectorgeo1[j].mnType)
            {
            case 0:
                pline = &xvectorgeo1[j];
                pgb->AddGeometryLine(s,pline->mfX,pline->mfY,pline->mfHdg,pline->mfLen);
                break;
            case 1:
                parc = &xvectorgeo1[j];
                pgb->AddGeometryArc(s,parc->mfX,parc->mfY,parc->mfHdgStart,parc->mfLen,1.0/parc->mR);
                break;
            case 2:
                pbez = &xvectorgeo1[j];
                std::cout<<"u0:"<<pbez->mfu[0]<<std::endl;
                pgb->AddGeometryParamPoly3(s,pbez->mfX,pbez->mfY,
                                           pbez->mfHdg,pbez->mfLen,pbez->mfu[0],
                        pbez->mfu[1],pbez->mfu[2],pbez->mfu[3],pbez->mfv[0],
                        pbez->mfv[1],pbez->mfv[2],pbez->mfv[3],false);
                break;
            }
            s = s + xvectorgeo1[j].mfLen;
        }

        s=0.0;
        for(j=0;j<(int)xvectorgeo2.size();j++)
        {
            proad2->AddGeometryBlock();
            GeometryBlock * pgb = proad2->GetGeometryBlock(j);

            geobase * pline;
            geobase * pbez;
            geobase * parc;
            switch(xvectorgeo2[j].mnType)
            {
            case 0:
                pline = &xvectorgeo2[j];
                pgb->AddGeometryLine(s,pline->mfX,pline->mfY,pline->mfHdg,pline->mfLen);
                break;
            case 1:
                parc = &xvectorgeo2[j];
                pgb->AddGeometryArc(s,parc->mfX,parc->mfY,parc->mfHdgStart,parc->mfLen,1.0/parc->mR);
                break;
            case 2:
                pbez = &xvectorgeo2[j];
                std::cout<<"u0:"<<pbez->mfu[0]<<std::endl;
                pgb->AddGeometryParamPoly3(s,pbez->mfX,pbez->mfY,
                                           pbez->mfHdg,pbez->mfLen,pbez->mfu[0],
                        pbez->mfu[1],pbez->mfu[2],pbez->mfu[3],pbez->mfv[0],
                        pbez->mfv[1],pbez->mfv[2],pbez->mfv[3]);
                break;
            }
            s = s + xvectorgeo2[j].mfLen;
        }

        proad1->AddLaneSection(0);
        LaneSection * pLS1 = proad1->GetLaneSection(0);
        pLS1->SetS(0);
        pLS1->AddLane(0,0,"none",false);

        proad2->AddLaneSection(0);
        LaneSection * pLS2 = proad2->GetLaneSection(0);
        pLS2->SetS(0);
        pLS2->AddLane(0,0,"none",false);

        double b1,b2;
        double fwidth3;
        fwidth3 = fwidth1 + (fwidth2 - fwidth1)*(xroadlen1)/(xroadlen1+xroadlen2);
        b1 = (fwidth3 - fwidth1)/xroadlen1;
        b2 = (fwidth2 - fwidth3)/xroadlen2;

        pLS1->AddLane(-1,-1,"driving",false);
        pLane = pLS1->GetLastAddedLane();
        pLane->AddWidthRecord(0,fwidth1,b1,0,0);

        pLS2->AddLane(-1,-1,"driving",false);
        pLane = pLS2->GetLastAddedLane();
        pLane->AddWidthRecord(0,fwidth3,b2,0,0);



    }



}