modularization/src/tool/map_lanetoxodr/function/ndsdataproc.cpp

#include "ndsdataproc.h"

#include <QFile>
#include "gnss_coordinate_convert.h"
#include <QDateTime>

#include <QPointF>

#include "geofit.h"
#include "circlefitting.h"

#ifndef  INNDSPROC
#include "mainwindow.h"
#endif

#ifndef  INNDSPROC
extern MainWindow * gw;
#endif

NDSDataProc::NDSDataProc()
{

}


//-1 can't open line data file
//-2 can't open vehicle data file
//-3 no valid line data.
//-4 no valid vehicle data.
int NDSDataProc::ProcNDSData(std::string strlinepath,std::string strvehiclepath,OpenDrive * mpxodr,int nleftlanecount,int nrightlanecount)
{
    double fExtend = 100;
    mnProc = 0;
    mstrState = "Loading Data.";
    QFile xFileline;
    QFile xFilevehicle;
    xFileline.setFileName(strlinepath.data());
    xFilevehicle.setFileName(strvehiclepath.data());
    if(!xFileline.open(QIODevice::ReadOnly))
    {
        mnProc = 100;
        mstrState = " Can't Read Line File. ";
        return -1;
    }
    if(!xFilevehicle.open(QIODevice::ReadOnly))
    {
        xFileline.close();
        mnProc = 100;
        mstrState = " Can't Read Vehicle File. ";
        return -2;
    }

    double flaneavgwidthtotal = 0;
    double flaneavgwidth = 3.5;


    std::vector<iv::nds_line> xvectorline;
    std::vector<iv::nds_vehicle> xvectorvehicle;
    std::vector<iv::nds_vl> xvectorvl;

    QByteArray ba = xFileline.readAll();
    QList<QByteArray> baline =ba.split('\n');//x.split(QRegExp("\n ")) ;//ba.split('\n');
    int nline = baline.size();
    int i;
    for(i=1;i<nline;i++)
    {
        QString x(baline[i]);
        x = x.trimmed();
        //         QList<QByteArray> badata = baline[i].split('\t');
        QStringList badata = x.split(QRegExp("[,]"));

        if(badata.size()>=36)
        {
            iv::nds_line xline;
            xline.localtime = QString(badata[1]).toLongLong();
            if(badata[6] == "Normal")xline.feature = 0;
            if(badata[6] == "Left1")xline.feature = 1;
            if(badata[6] == "Right1")xline.feature = 2;
            xline.centerdeparture = badata[8].toDouble();
            xline.lanewidth = badata[9].toDouble();
            xline.laneheading = badata[10].toDouble();
            xline.lanecurv = badata[11].toDouble();
            xline.nlanetype = badata[23].toDouble();
            if(xline.feature >=0)xvectorline.push_back(xline);

            flaneavgwidthtotal = flaneavgwidthtotal + xline.lanewidth;
        }

    }

    if(nline>0)
    {
        flaneavgwidth = flaneavgwidthtotal/nline;
    }

    if(xvectorline.size() < 2)
    {
        xFileline.close();
        xFilevehicle.close();
        mnProc = 100;
        mstrState = " No Line Data. ";
        return -3;
    }

//    int64_t localtime;
//    int locationmode;
//    double lon;
//    double lat;
//    double height;
//    double heading;
    ba = xFilevehicle.readAll();
    baline =ba.split('\n');//x.split(QRegExp("\n ")) ;//ba.split('\n');
    nline = baline.size();
    for(i=1;i<nline;i++)
    {
        QString x(baline[i]);
        x = x.trimmed();
        //         QList<QByteArray> badata = baline[i].split('\t');
        QStringList badata = x.split(QRegExp("[,]"));

        if(badata.size()>=15)
        {
            iv::nds_vehicle xvehicle;
            xvehicle.localtime = badata[1].toLongLong();
            xvehicle.locationmode = badata[4].toInt();
            xvehicle.lon = badata[5].toDouble();
            xvehicle.lat = badata[6].toDouble();
            xvehicle.height = badata[7].toDouble();
            xvehicle.heading = badata[9].toDouble();
            if(xvehicle.heading<0)xvehicle.heading = xvehicle.heading + 360;
            if(xvehicle.locationmode > 0)xvectorvehicle.push_back(xvehicle);
        }

    }

    xFileline.close();
    xFilevehicle.close();

    if(xvectorvehicle.size()<2)
    {
        mnProc = 100;
        mstrState = " No Vehicle Data. ";
        return -4 ;
    }


    double flon0,flat0;

    if(mpxodr->GetRoadCount() == 0)
    {
        flon0 = xvectorvehicle[0].lon;
        flat0 = xvectorvehicle[0].lat;
        mpxodr->SetHeader(1,1,"adcmap",1.1,QDateTime::currentDateTime().toString("yyyy-MM-dd").toLatin1().data(),0,0,0,0,flat0,flon0,360);
    }
    double x0,y0;
    GaussProjCal(flon0,flat0,&x0,&y0);

    int j;
    int nlinecount = xvectorline.size();
    int nvehiclecount = xvectorvehicle.size();

    mnProc = 10;
    mstrState = " Calulation x y. ";
    for(i=0;i<nvehiclecount;i++)
    {
        iv::nds_vehicle * p;
        p = &xvectorvehicle[i];
        double x,y;
        GaussProjCal(p->lon,p->lat,&x,&y);
        p->frel_x = x - x0;
        p->frel_y = y - y0;
    }

    double fS = 0;
    for(i=1;i<xvectorvehicle.size();i++)
    {
        iv::nds_vehicle *p1,*p2;
        p1 = &xvectorvehicle[i-1];
        p2 = &xvectorvehicle[i];
        fS = fS + sqrt(pow(p2->frel_x - p1->frel_x,2)+pow(p2->frel_y - p1->frel_y,2));
    }

    std::cout<<" s: "<<fS<<std::endl;

    int nlinenow = 0;

    mnProc = 20;
    mstrState = " Get Vehicle And Line Relation. ";
    for(i=0;i<nvehiclecount;i++)
    {
        if(nlinenow >= nlinecount)
        {
            break;
        }

        iv::nds_vl xvl;
        xvl.mvehindex = i;

        while(xvectorvehicle[i].localtime > xvectorline[nlinenow].localtime)
        {
            nlinenow++;
        }

        while(xvectorvehicle[i].localtime == xvectorline[nlinenow].localtime)
        {
            xvl.mvectorlineindex.push_back(nlinenow);
//            std::cout<<" veh: "<<i<<" line: "<<nlinenow<<std::endl;
            nlinenow++;
        }
        xvectorvl.push_back(xvl);

    }

    //Erase no line info point.
//    std::cout<<" vl size "<<xvectorvl.size()<<std::endl;
    i= 0;
    while(i<(int)xvectorvl.size())
    {
        if(xvectorvl[i].mvectorlineindex.size() == 0)
        {
            xvectorvl.erase(xvectorvl.begin()+i);
            continue;
        }
        i++;
    }


    //Erase dis<0.5 point
    i=1;
    while(i<(int)xvectorvl.size())
    {
        iv::nds_vehicle v1 = xvectorvehicle[xvectorvl[i-1].mvehindex];
        iv::nds_vehicle v2 = xvectorvehicle[xvectorvl[i].mvehindex];
        double fdis = sqrt(pow(v1.frel_x - v2.frel_x,2) + pow(v1.frel_y - v2.frel_y,2));
        if(fdis <  0.5)
        {
            xvectorvl.erase(xvectorvl.begin()+i);
            continue;
        }
        i++;
    }
//    std::cout<<" vl size "<<xvectorvl.size()<<std::endl;

    double fdismin = 100000;
    double fdismax = 0;


    int nvlsize = xvectorvl.size();
    fS = 0;
    for(i=1;i<nvlsize;i++)
    {
        iv::nds_vehicle v1 = xvectorvehicle[xvectorvl[i-1].mvehindex];
        iv::nds_vehicle v2 = xvectorvehicle[xvectorvl[i].mvehindex];
        double fdis = sqrt(pow(v1.frel_x - v2.frel_x,2) + pow(v1.frel_y - v2.frel_y,2));
        xvectorvehicle[xvectorvl[i].mvehindex].fdistolast = fdis;
        fS = fS + fdis;
        if(fdis<fdismin)fdismin = fdis;
        if(fdis>fdismax)fdismax = fdis;
        if(fdis> 50)std::cout<<" index "<<xvectorvl[i].mvehindex<<" dis "<<fdis<<" time "<<xvectorvehicle[xvectorvl[i].mvehindex].localtime<<std::endl;
//        if(fdis<0.6)std::cout<<" index "<<xvectorvl[i].mvehindex<<" dis "<<fdis<<" time "<<xvectorvehicle[xvectorvl[i].mvehindex].localtime<<std::endl;
    }

    for(i=0;i<nvlsize;i++)
    {
        iv::nds_vehicle *pv1 = &xvectorvehicle[xvectorvl[i].mvehindex];
        double fhdg = (90-pv1->heading)*M_PI/360.0;
        if(fhdg<0)fhdg = fhdg + 2.0*M_PI;
        pv1->fhdg = fhdg;
        iv::nds_line * pline1 = &xvectorline[xvectorvl[i].mvectorlineindex[0]];
        pv1->flanewidth = pline1->lanewidth;
        iv::nds_line * pline2 = NULL;
        if(xvectorvl[i].mvectorlineindex.size()>=2)
        {
            pline2 = &xvectorline[xvectorvl[i].mvectorlineindex[1]];
        }
        pv1->fx = pv1->frel_x + (pline1->lanewidth/2.0 - pline1->centerdeparture)*cos(fhdg + M_PI/2.0);
        pv1->fy = pv1->frel_y + (pline1->lanewidth/2.0 - pline1->centerdeparture)*sin(fhdg + M_PI/2.0);
        if(pline1->feature == 1)
        {
            pv1->nleftlanetype = pline1->nlanetype;
        }
        if(pline1->feature == 2)
        {
            pv1->nrightlanetype = pline1->nlanetype;
        }
        if(pline2 != NULL)
        {
            if(pline2->feature == 1)
            {
                pv1->nleftlanetype = pline2->nlanetype;
            }
            if(pline2->feature == 2)
            {
                pv1->nrightlanetype = pline2->nlanetype;
            }
        }
    }


    mnProc = 30;
    mstrState = " Fitting Geo. ";

    std::cout<<" fdis "<<fS<<" min "<<fdismin<<" max "<<fdismax<<std::endl;

    double LINE_ERROR = 0.10;


    std::vector<geobase> xvectorgeo;

    bool bComplete = false;
//    int j;
    int ncurpos = 0;
    int nrange = xvectorvl.size();
//    double fXLast;
//    double fYLast;
//    bool bFirst = true;
    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] = xvectorvehicle[xvectorvl[j+ncurpos].mvehindex].fx;//mvectorrtkdata.at(j+ncurpos).mfrelx;
            y_veh[j] = xvectorvehicle[xvectorvl[j+ncurpos].mvehindex].fy;//mvectorrtkdata.at(j+ncurpos).mfrely;
        }

        if(nrange<1)
        {
            break;
        }

        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 = xvectorvl.size() - ncurpos;

        }
        else
        {
            if(nrange > 30)
                nrange = nrange/2;
            else
                nrange = nrange -1;
            if(nrange<2)nrange = 2;
        }
        if(ncurpos>=(int)(xvectorvl.size()-1))
        {
            if(ncurpos == (int)(xvectorvl.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] = xvectorvehicle[xvectorvl[ncurpos].mvehindex].fx;
                y_veh1[1] = 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;
    for(i=0;i<(int)xvectorgeo.size();i++)
    {
        geobase * pgeo = &xvectorgeo[i];
//        std::cout<<"geo len: "<<pgeo->mfLen<<" type"<<pgeo->mnType<<std::endl;
        double tems = 0;
        for(j=(pgeo->mnStartPoint+1);j<=pgeo->mnEndPoint;j++)
        {
            xvectorvehicle[xvectorvl[j].mvehindex].s = snow + tems + xvectorvehicle[xvectorvl[j].mvehindex].fdistolast;
            tems = tems + xvectorvehicle[xvectorvl[j].mvehindex].fdistolast;
        }
        snow = snow + pgeo->mfLen;
    }


    mnProc = 90;
    mstrState = " Fitting Width and Height. ";
    //Width
    std::vector<iv::widthabcd> xvectorwidthabcd;
    bComplete = false;
    ncurpos = 0;
    nrange = xvectorvl.size();
    double flanewidtherror = 0.1; //1cm

    if(nrange <2)
    {
        mstrState = " No Valid Data. Complete.";
        mnProc = 100;
        return 0;
    }
    while(bComplete == false)
    {
        double ele_coff[4];
        for(j=0;j<4;j++)ele_coff[j] = 0;
        if(nrange>0)ele_coff[0] = 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] = xvectorvehicle[xvectorvl[ncurpos+j].mvehindex].s - xvectorvehicle[xvectorvl[ncurpos].mvehindex].s;
            y_vehhg[j] =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 = 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 = xvectorvl.size() - ncurpos;
        }

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



    std::vector<iv::eleabcd> xvectoreleabcd;
    bComplete = false;
    ncurpos = 0;
    nrange = xvectorvl.size();
    double feleerror = 0.5; //1cm
    while(bComplete == false)
    {
        double ele_coff[4];
        for(j=0;j<4;j++)ele_coff[j] = 0;
        if(nrange>0)ele_coff[0] = xvectorvehicle[xvectorvl[ncurpos].mvehindex].height;
        int M = nrange;
        VectorXd x_vehhg(M);
        VectorXd y_vehhg(M);
        for(j=0;j<M;j++)
        {
            x_vehhg[j] = xvectorvehicle[xvectorvl[ncurpos+j].mvehindex].s - xvectorvehicle[xvectorvl[ncurpos].mvehindex].s;
            y_vehhg[j] =xvectorvehicle[xvectorvl[ncurpos+j].mvehindex].height;
        }
        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>feleerror)ferror = fvalue;
        }
        if(ferror>flanewidtherror)
        {
            if(nrange>10)nrange = nrange/2;
            else nrange = nrange -1;
        }
        else
        {
            iv::eleabcd xabcd;
            xabcd.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];
            xvectoreleabcd.push_back(xabcd);
            ncurpos = ncurpos + nrange-1;
            nrange = xvectorvl.size() - ncurpos;
        }

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

    std::cout<<" last s: "<<xvectorvehicle[xvectorvl[nvlsize-1].mvehindex].s<<std::endl;

    std::cout<<" geo size "<<xvectorgeo.size()<<std::endl;


    std::cout<<"complete "<<std::endl;

#ifndef INNDSPROC
    mpxodr->AddRoad("",xvectorvehicle[xvectorvl[nvlsize-1].mvehindex].s, QString::number(gw->CreateRoadID()).toStdString(),"-1");
#else
    mpxodr->AddRoad("",xvectorvehicle[xvectorvl[nvlsize-1].mvehindex].s, QString::number(1).toStdString(),"-1");
#endif
    Road * p = mpxodr->GetRoad(mpxodr->GetRoadCount() - 1);


  //  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<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]);
            break;
        }
        s = s + xvectorgeo[j].mfLen;
    }

    p->SetRoadLength(s);

    p->AddLaneSection(0);

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

    pLS->AddLane(0,0,"none",false);

    int noldtype = 0;
    int ntype = 0;
    Lane * pCenterLane = pLS->GetLastCenterLane();
    if(nleftlanecount == 0)
    {
        if(nvlsize > 0)
        {
            ntype = xvectorvehicle[xvectorvl[0].mvehindex].nleftlanetype;
            pCenterLane->AddRoadMarkRecord(xvectorvehicle[xvectorvl[0].mvehindex].s,roadmarktypetostr(ntype),"standard","white",0.15,"none");
            noldtype = ntype;
        }
        for(i=1;i<(int)(nvlsize-1);i++)
        {
            int ntype = xvectorvehicle[xvectorvl[i].mvehindex].nleftlanetype;
            if(ntype!= noldtype )
            {
                pCenterLane->AddRoadMarkRecord(xvectorvehicle[xvectorvl[i].mvehindex].s,roadmarktypetostr(ntype),"standard","white",0.15,"none");
                noldtype = ntype;
            }
        }
    }
    else
    {
        pCenterLane->AddRoadMarkRecord(0,"solid","standard","yellow",0.15,"none");
        p->AddLaneOffset(0,nleftlanecount*flaneavgwidth,0,0,0);
        int k;
        for(k=0;k<nleftlanecount;k++)
        {
            pLS->AddLane(-1,(-1)*(pLS->GetRightLaneCount()+1),"driving",false);
            Lane * pLane = pLS->GetLastRightLane();
            pLane->AddWidthRecord(0,flaneavgwidth,0,0,0);
            if(k != (nleftlanecount-1))
            {
                pLane->AddRoadMarkRecord(0,"broken","standard","standard",0.15,"none");
            }
            else
            {
                if(nvlsize > 0)
                {
                    ntype = xvectorvehicle[xvectorvl[0].mvehindex].nleftlanetype;
                    pLane->AddRoadMarkRecord(xvectorvehicle[xvectorvl[0].mvehindex].s,roadmarktypetostr(ntype),"standard","white",0.15,"none");
                    noldtype = ntype;
                }
                for(i=1;i<(int)(nvlsize-1);i++)
                {
                    int ntype = xvectorvehicle[xvectorvl[i].mvehindex].nleftlanetype;
                    if(ntype!= noldtype )
                    {
                        pLane->AddRoadMarkRecord(xvectorvehicle[xvectorvl[i].mvehindex].s,roadmarktypetostr(ntype),"standard","white",0.15,"none");
                        noldtype = ntype;
                    }
                }
            }
        }
    }


    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);
    }

    for(j=0;j<(int)xvectoreleabcd.size();j++)
    {
        iv::eleabcd * pwa = &xvectoreleabcd[j];
        p->AddElevation(pwa->s,pwa->A,pwa->B,pwa->C,pwa->D);
    }



    noldtype = 0;
    ntype = 0;
    if(nvlsize > 0)
    {
        ntype = xvectorvehicle[xvectorvl[0].mvehindex].nrightlanetype;
        pLane->AddRoadMarkRecord(xvectorvehicle[xvectorvl[0].mvehindex].s,roadmarktypetostr(ntype),"standard","white",0.15,"none");
        noldtype = ntype;
    }
    for(i=1;i<(int)(nvlsize-1);i++)
    {
        int ntype = xvectorvehicle[xvectorvl[i].mvehindex].nrightlanetype;
        if(ntype!= noldtype )
        {
            pLane->AddRoadMarkRecord(xvectorvehicle[xvectorvl[i].mvehindex].s,roadmarktypetostr(ntype),"standard","white",0.15,"none");
            noldtype = ntype;
        }
    }

    for(i=0;i<nrightlanecount;i++)
    {
        pLS->AddLane(-1,(-1)*(pLS->GetRightLaneCount()+1),"driving",false);
        Lane * pLane = pLS->GetLastRightLane();
        pLane->AddWidthRecord(0,flaneavgwidth,0,0,0);
        if(i != (nrightlanecount-1))
        {
            pLane->AddRoadMarkRecord(0,"broken","standard","standard",0.15,"none");
        }
        else
        {
            pLane->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"none");
        }
    }

    if(fExtend>0.1)
    {
        double endx,endy,endhdg;
        p->GetGeometryCoords(p->GetRoadLength()-0.001,endx,endy,endhdg);
        p->AddGeometryBlock();
        GeometryBlock * pgeob = p->GetLastAddedGeometryBlock();
        pgeob->AddGeometryLine(p->GetRoadLength(),endx,endy,endhdg,fExtend);
        p->SetRoadLength(p->GetRoadLength()+ fExtend);
    }


    mnProc = 100;
    mstrState = " Complete. ";
    return 0;
}

std::string NDSDataProc::roadmarktypetostr(int ntype)
{
    switch (ntype) {
    case 1:
        return "none";
        break;
    case 2:
        return "broken";
        break;
    case 3:
        return "solid";
        break;
    default:
        return "none";
        break;
    }
}