#include "xodrdijkstra.h"

#include <qdebug.h>
#include <iostream>
#include <memory>

extern iv::Ivlog *givlog;
extern iv::Ivfault *gfault;

namespace iv {
   struct xodrtreeunit
   {
     double mlen;
     iv::xodrtreeunit * mparent;
     int nroad;

     iv::xodrtreeunit * mpchild;
     iv::xodrtreeunit * mpnextbro;
   };

}

namespace  iv {
struct vertexsame
{
    vertexsame(int a,int b) {ma=a; mb =b;}
    int ma;
    int mb;
};

}

namespace iv {
struct vertexsamearray
{
    std::vector<int> mvertexarray;
    vertexsamearray(int a,int b) {mvertexarray.push_back(a);mvertexarray.push_back(b);}
    bool isinarray(int x)
    {
        int i;
        for(i=0;i<mvertexarray.size();i++)
        {
            if(x == mvertexarray[i])return true;

        }
        return false;
    }

    void print()
    {
        char strout[10000];
        char strtem[30];
        int i;
        snprintf(strout,10000,"");
        for(i=0;i<mvertexarray.size();i++)
        {
            snprintf(strtem,30,"%d ",mvertexarray[i]);
            strncat(strout,strtem,10000);
        }
        qDebug(strout);
    }

    void sort()
    {
        int i;
        for(i=1;i<mvertexarray.size();i++)
        {
            if(mvertexarray[i]<mvertexarray[0])
            {
                int a = mvertexarray[0];
                mvertexarray[0] = mvertexarray[i];
                mvertexarray[i] = a;
            }
        }
    }
};

}



Road * xodrdijkstra::GetRoadByID(int nRoadID)
{
    OpenDrive * pxodr = mpxodr;
    int i;
    int nsize = pxodr->GetRoadCount();
    for(i=0;i<nsize;i++)
    {
        Road * pRoad = pxodr->GetRoad(i);
        if(atoi(pRoad->GetRoadId().data()) == nRoadID)
        {
            return pRoad;
        }
    }

    std::cout<<"xodrdijkstra::GetRoadByID "<<nRoadID<<"  Fail."<<std::endl;
    return 0;
}


/**
  * @brief xodrdijkstra::xodrdijkstra
  * opendrive to directed graph ,so can use dijkstra
  * 1.将OpenDrive的Road转换为边，双行路转换为两条边，单行路转换为一条边,每条边包含两个顶点。
  * 2.寻找相同顶点，首先从Junction里寻找有连接关系的作为相同顶点插入到相同顶点表；然后从Road的Link关系里寻找相同顶点，第一种是Road不是Junction的，如果他的前面道路或后继道路是Road的，作为相同顶点关系插入到相同顶点表，第二种Road属于Junction的，如果他的后继道路是Road的，作为相同顶点插入到相同顶点表。
  * 3.根据相同顶点表，将相同顶点合并成相同顶点数组（比如（1，3） （2，3）合并为（1，3，2）），随后进行排序（比如(1,2,3)),然后对每条边的顶点重新设置顶点编号（比如3改为1），形成有向图。
  * @param pxodr      pointer to a OpenDrive Map
**/
xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
{
    mpxodr = pxodr;

    int i;
    double  nlenth = 0;
    int nroadsize = mpxodr->GetRoadCount();
    for(i=0;i<nroadsize;i++)
    {
        Road * px = mpxodr->GetRoad(i);
        nlenth = nlenth + px->GetRoadLength();

        if(px->GetLaneSectionCount()<1)
        {
            qDebug("no lanesection");
            continue;
        }

//        qDebug("lane count is %d left %d right %d" ,px->GetLaneSection(0)->GetLaneCount(),px->GetLaneSection(0)->GetLeftLaneCount(),
//               px->GetLaneSection(0)->GetRightLaneCount());

        int j;
        for(j=0;j<px->GetLaneSectionCount();j++)
        {
            if(px->GetLaneSection(j)->GetLaneCount()<2)
            {
                givlog->info("no lane");
                continue;
            }

            if(px->GetLaneSection(j)->GetLeftLaneCount()>0)
            {

                double fseclen = 0;
                if(j<(px->GetLaneSectionCount() -1))
                {

                    fseclen = px->GetLaneSection(j+1)->GetS() - px->GetLaneSection(j)->GetS();

                }
                else
                {
                    fseclen = px->GetRoadLength() - px->GetLaneSection(j)->GetS();
                }

                roadedge xroad(atoi(px->GetRoadId().data()),atoi(px->GetRoadJunction().data()),fseclen,1,j,px);
//                roadedge xroad(atoi(px->GetRoadId().data()),atoi(px->GetRoadJunction().data()),px->GetRoadLength(),1,j,px);


                //           xroad.mpx = px;
                mroadedge.push_back(xroad);
            }
            if(px->GetLaneSection(j)->GetRightLaneCount()>0)
            {
                double fseclen = 0;
                if(j<(px->GetLaneSectionCount() -1))
                {

                    fseclen = px->GetLaneSection(j+1)->GetS() - px->GetLaneSection(j)->GetS();

                }
                else
                {
                    fseclen = px->GetRoadLength() - px->GetLaneSection(j)->GetS();
                }
                roadedge xroad(atoi(px->GetRoadId().data()),atoi(px->GetRoadJunction().data()),fseclen,2,j,px);

//                roadedge xroad(atoi(px->GetRoadId().data()),atoi(px->GetRoadJunction().data()),px->GetRoadLength(),2,j,px);

                //           xroad.mpx = px;
                mroadedge.push_back(xroad);
            }
        }
//        qDebug("id = %s  juncton = %s ",px->GetRoadId().data(),px->GetRoadJunction().data());
    }

    givlog->debug("road %d ",mroadedge.size());

    std::vector<roadedge> * pxxx = &mroadedge;//10021


    if(mroadedge.size()<1)return;


    int nvertindex = 0;
    mroadedge[0].mvertexstart = 0;
    mroadedge[1].mvertexend = 1;

    //Init Vertex
    for(i=0;i<mroadedge.size();i++)
    {
        mroadedge[i].mbvertex = true;
        mroadedge[i].mvertexstart = 2*i;
        mroadedge[i].mvertexend = 2*i + 1;
    }


    //Find Same Vertex
    std::vector<iv::vertexsame> xvertexsame;

    std::vector<std::vector<int>> xcommonpoint;

    Junction * pj;
    for(i=0;i<pxodr->GetJunctionCount();i++)
    {
        pj = pxodr->GetJunction(i);
        int j;
//        if(atoi(pj->GetId().data()) == 810009)
//        {
//            int ggg = 11;
//        }
        for(j=0;j<pj->GetJunctionConnectionCount();j++)
        {
            JunctionConnection * pjc = pj->GetJunctionConnection(j);

            int road_from = atoi(pjc->GetIncomingRoad().data());
            int road_to = atoi(pjc->GetConnectingRoad().data());

            Road * pRoad_From = GetRoadByID(road_from);
            Road * pRoad_To = GetRoadByID(road_to);
            int k;
            for(k=0;k<pjc->GetJunctionLaneLinkCount();k++)
            {
                JunctionLaneLink * pjll = pjc->GetJunctionLaneLink(k);
                int lane_from = pjll->GetFrom();
                int lane_to = pjll->GetTo();
                lanenetunit lnu;

                int lr_from,lr_to;
                if(pjll->GetFrom()>0)lr_from = 1;
                else lr_from = 2;
                if(pjll->GetTo() > 0)lr_to = 1;
                else lr_to = 2;



                int fromedge;
                int toedge;

                fromedge = getroadedge(road_from,lr_from);
                toedge = getroadedge(road_to,lr_to);
//                qDebug("from %d %d to %d %d fromedge %d toedge %d %s",road_from, pjll->GetFrom(),
//                       road_to,pjll->GetTo(),fromedge,toedge,pjc->GetContactPoint().data());
                if((fromedge == -1)||(toedge == -1))
                {
                    givlog->debug("from %d %d to %d %d fromedge %d toedge %d %s",road_from, pjll->GetFrom(),
                           road_to,pjll->GetTo(),fromedge,toedge,pjc->GetContactPoint().data());
                    continue;
                }

                int a,b;


                if(strncmp(pjc->GetContactPoint().data(),"end",255) == 0)
                {
//                    if(road_from == 30011)
//                    {
//                        int xx = 1;
//                    }
//                    if(lr_from == 2)
                    if(lr_to == 1)
                    {
                        fromedge = getroadedge(road_from,lr_from,pRoad_From->GetLaneSectionCount() -1);
                        toedge = getroadedge(road_to,lr_to);
                        a = mroadedge[fromedge].mvertexend;
                        b = mroadedge[toedge].mvertexstart;
                        lnu.mpFromRoad = GetRoadByID(road_from);
                        lnu.mpToRoad = GetRoadByID(road_to);
                        lnu.mnFromSection = lnu.mpFromRoad->GetLaneSectionCount() - 1;
                        lnu.mnToSection = 0;
                        lnu.mnFromLane = lane_from;
                        lnu.mnToLane = lane_to;
                    }
                    else
                    {
                        fromedge = getroadedge(road_from,lr_from);
                        toedge = getroadedge(road_to,lr_to,pRoad_To->GetLaneSectionCount() -1);
                        a = mroadedge[fromedge].mvertexstart;
                        b = mroadedge[toedge].mvertexend;
                        lnu.mpFromRoad = GetRoadByID(road_to);
                        lnu.mpToRoad = GetRoadByID(road_from);
                        lnu.mnFromSection = 0;
                        lnu.mnToSection = lnu.mpToRoad->GetLaneSectionCount()-1;
                        lnu.mnFromLane = lane_to;
                        lnu.mnToLane = lane_from;
                    }

                }
                if(strncmp(pjc->GetContactPoint().data(),"start",255) == 0)
                {

 //                   if(lr_from == 1)
                    if(lr_to == 2)
                    {
                        fromedge = getroadedge(road_from,lr_from,pRoad_From->GetLaneSectionCount() -1);
                        toedge = getroadedge(road_to,lr_to);
                        a = mroadedge[fromedge].mvertexend;
                        b = mroadedge[toedge].mvertexstart;
                        lnu.mpFromRoad = GetRoadByID(road_from);
                        lnu.mpToRoad = GetRoadByID(road_to);
                        lnu.mnFromSection = lnu.mpFromRoad->GetLaneSectionCount() - 1;
                        lnu.mnToSection = 0;
                        lnu.mnFromLane = lane_from;
                        lnu.mnToLane = lane_to;
                    }
                    else
                    {
                        fromedge = getroadedge(road_from,lr_from);
                        toedge = getroadedge(road_to,lr_to,pRoad_To->GetLaneSectionCount() -1);
                        a = mroadedge[fromedge].mvertexstart;
                        b = mroadedge[toedge].mvertexend;
                        lnu.mpFromRoad = GetRoadByID(road_to);
                        lnu.mpToRoad = GetRoadByID(road_from);
                        lnu.mnFromSection = 0;
                        lnu.mnToSection = lnu.mpToRoad->GetLaneSectionCount()-1;
                        lnu.mnFromLane = lane_to;
                        lnu.mnToLane = lane_from;
                    }


                    mlanenet.push_back(lnu);
                iv::vertexsame vx(a,b);
                xvertexsame.push_back(vx);

            }
            }



        }
    }

    givlog->debug("After Junction vertex same is %d ",xvertexsame.size());

    for(i=0;i<nroadsize;i++)
    {
        Road * px = mpxodr->GetRoad(i);

        if(px->GetLaneSectionCount() == 1)
        {
            if(px->GetPredecessor() != 0)
            {
                if(strncmp(px->GetPredecessor()->GetElementType().data(),"road",255) == 0)
                {
                    int k;
                    int nroadpre = atoi(px->GetPredecessor()->GetElementId().data());
                    int nroadcur = atoi(px->GetRoadId().data());



                    for(k=0;k<px->GetLaneSection(0)->GetLaneCount();k++)
                    {
                        Lane * plane = px->GetLaneSection(0)->GetLane(k);

                        if(plane->GetId() == 0)continue;
                        if(plane->IsPredecessorSet())
                        {

                            int a,b;
                            int npre = plane->GetPredecessor();
                            int nid = plane->GetId();
                            lanenetunit lnu;
                            lnu.mnFromSection = 0;
                            lnu.mnToSection = 0;
                            if(strncmp(px->GetPredecessor()->GetContactPoint().data(),"end",255) == 0)
                            {
                                if(npre < 0)
                                {
                                    int nedgepre = getroadedge(nroadpre,2,GetRoadByID(nroadpre)->GetLaneSectionCount()-1);
                                    lnu.mnFromSection = GetRoadByID(nroadpre)->GetLaneSectionCount()-1;
                                    a = mroadedge[nedgepre].mvertexend;
                                    lnu.mpFromRoad = GetRoadByID(nroadpre);
                                    lnu.mpToRoad = GetRoadByID(nroadcur);
                                    lnu.mnFromLane = npre;
                                    lnu.mnToLane = nid;
                                }
                                else
                                {
                                    int nedgepre = getroadedge(nroadpre,1,GetRoadByID(nroadpre)->GetLaneSectionCount()-1);
                                    lnu.mnToSection = GetRoadByID(nroadpre)->GetLaneSectionCount()-1;
                                    a = mroadedge[nedgepre].mvertexstart;
                                    lnu.mpFromRoad = GetRoadByID(nroadcur);
                                    lnu.mpToRoad = GetRoadByID(nroadpre);
                                    lnu.mnFromLane = nid;
                                    lnu.mnToLane = npre;
                                }

                            }
                            if(strncmp(px->GetPredecessor()->GetContactPoint().data(),"start",255) == 0)
                            {
                                if(npre < 0)
                                {
                                    int nedgepre = getroadedge(nroadpre,2);
                                    a = mroadedge[nedgepre].mvertexstart;
                                    lnu.mpFromRoad = GetRoadByID(nroadcur);
                                    lnu.mpToRoad = GetRoadByID(nroadpre);
                                    lnu.mnFromLane = nid;
                                    lnu.mnToLane = npre;
                                }
                                else
                                {
                                    int nedgepre = getroadedge(nroadpre,1);
                                    a = mroadedge[nedgepre].mvertexend;
                                    lnu.mpFromRoad = GetRoadByID(nroadpre);
                                    lnu.mpToRoad = GetRoadByID(nroadcur);
                                    lnu.mnFromLane = npre;
                                    lnu.mnToLane = nid;
                                }
                            }
                            if(nid < 0)
                            {
                                int nedgecur = getroadedge(nroadcur,2);
                                b = mroadedge[nedgecur].mvertexstart;
                            }
                            else
                            {
                                int nedgecur = getroadedge(nroadcur,1);
                                b = mroadedge[nedgecur].mvertexend;
                            }
                            mlanenet.push_back(lnu);
                            iv::vertexsame vx(a,b);
                            xvertexsame.push_back(vx);

                        }
                        if(plane->IsSuccessorSet())
                        {

                        }
                    }
                }

            }


            if(px->GetSuccessor() != 0)
            {
                if(strncmp(px->GetSuccessor()->GetElementType().data(),"road",255) == 0)
                {
                    int k;
                    int nroadnext = atoi(px->GetSuccessor()->GetElementId().data());
                    int nroadcur = atoi(px->GetRoadId().data());

                    for(k=0;k<px->GetLaneSection(px->GetLaneSectionCount()-1)->GetLaneCount();k++)
                    {

//                        if(nroadcur == 30001)
//                        {
//                            int xg = 0;
//                            xg++;
//                        }
                        Lane * plane = px->GetLaneSection(px->GetLaneSectionCount()-1)->GetLane(k);
                        if(plane->GetId() == 0)continue;
                        if(plane->IsSuccessorSet())
                        {
                            int a,b;
                            int nnext = plane->GetSuccessor();
                            int nid = plane->GetId();
                            lanenetunit lnu;
                            lnu.mnFromSection = 0;
                            lnu.mnToSection = 0;
                            if(strncmp(px->GetSuccessor()->GetContactPoint().data(),"end",255) == 0)
                            {
                                if(nnext < 0)
                                {
                                    int nedgenext = getroadedge(nroadnext,2,GetRoadByID(nroadnext)->GetLaneSectionCount()-1);
                                    b = mroadedge[nedgenext].mvertexend;
                                    lnu.mnFromSection = GetRoadByID(nroadnext)->GetLaneSectionCount()-1;
                                    lnu.mpFromRoad = GetRoadByID(nroadnext);
                                    lnu.mpToRoad = GetRoadByID(nroadcur);
                                    lnu.mnFromLane = nnext;
                                    lnu.mnToLane = nid;

                                }
                                else
                                {
                                    int nedgenext = getroadedge(nroadnext,1,GetRoadByID(nroadnext)->GetLaneSectionCount()-1);
                                    b = mroadedge[nedgenext].mvertexstart;
                                    lnu.mpFromRoad = GetRoadByID(nroadcur);
                                    lnu.mnToSection = GetRoadByID(nroadnext)->GetLaneSectionCount()-1;
                                    lnu.mpToRoad = GetRoadByID(nroadnext);
                                    lnu.mnFromLane = nid;
                                    lnu.mnToLane = nnext;
                                }

                            }
                            if(strncmp(px->GetSuccessor()->GetContactPoint().data(),"start",255) == 0)
                            {
                                if(nnext < 0)
                                {
                                    int nedgenext = getroadedge(nroadnext,2);
                                    b = mroadedge[nedgenext].mvertexstart;

                                    lnu.mpFromRoad = GetRoadByID(nroadcur);
                                    lnu.mpToRoad = GetRoadByID(nroadnext);
                                    lnu.mnFromLane = nid;
                                    lnu.mnToLane = nnext;
                                }
                                else
                                {
                                    int nedgenext = getroadedge(nroadnext,1);
                                    b = mroadedge[nedgenext].mvertexend;

                                    lnu.mpFromRoad = GetRoadByID(nroadnext);
                                    lnu.mpToRoad = GetRoadByID(nroadcur);
                                    lnu.mnFromLane = nnext;
                                    lnu.mnToLane = nid;
                                }
                            }
                            if(nid < 0)
                            {
                                int nedgecur = getroadedge(nroadcur,2);
                                a = mroadedge[nedgecur].mvertexend;
                            }
                            else
                            {
                                int nedgecur = getroadedge(nroadcur,1);
                                a = mroadedge[nedgecur].mvertexstart;
                            }
                            iv::vertexsame vx(a,b);
                            xvertexsame.push_back(vx);
                            mlanenet.push_back(lnu);

                        }

                    }
                }

            }

        }
        else
        {
            if(px->GetPredecessor() != 0)
            {
                if(strncmp(px->GetPredecessor()->GetElementType().data(),"road",255) == 0)
                {
                    int k;
                    int nroadpre = atoi(px->GetPredecessor()->GetElementId().data());
                    int nroadcur = atoi(px->GetRoadId().data());

                    Road * pRoad_Pre = GetRoadByID(nroadpre);

                    if(pRoad_Pre == 0)
                    {
                        qDebug("Pre Road Missing.");
                        continue;
                    }


//                    if(nroadcur == 30012)
//                    {
//                        int ag;
//                        ag= 1;
//                    }

                    for(k=0;k<px->GetLaneSection(0)->GetLaneCount();k++)
                    {
                        Lane * plane = px->GetLaneSection(0)->GetLane(k);

                        if(plane->GetId() == 0)continue;
                        if(plane->IsPredecessorSet())
                        {
                            int a,b;
                            int npre = plane->GetPredecessor();
                            int nid = plane->GetId();
                            lanenetunit lnu;

                            if(strncmp(px->GetPredecessor()->GetContactPoint().data(),"end",255) == 0)
                            {
                                if(npre < 0)
                                {
                                    int nedgepre = getroadedge(nroadpre,2,pRoad_Pre->GetLaneSectionCount()-1);
                                    a = mroadedge[nedgepre].mvertexend;
                                    lnu.mpFromRoad = GetRoadByID(nroadpre);
                                    lnu.mpToRoad = GetRoadByID(nroadcur);
                                    lnu.mnFromSection = lnu.mpFromRoad->GetLaneSectionCount() - 1;
                                    lnu.mnToSection = 0;
                                    lnu.mnFromLane = npre;
                                    lnu.mnToLane = nid;
                                }
                                else
                                {
                                    int nedgepre = getroadedge(nroadpre,1,pRoad_Pre->GetLaneSectionCount()-1);
                                    a = mroadedge[nedgepre].mvertexstart;
                                    lnu.mpFromRoad = GetRoadByID(nroadcur);
                                    lnu.mpToRoad = GetRoadByID(nroadpre);
                                    lnu.mnFromSection = 0;
                                    lnu.mnToSection = lnu.mpToRoad->GetLaneSectionCount() -1;
                                    lnu.mnFromLane = nid;
                                    lnu.mnToLane = npre;
                                }

                            }
                            if(strncmp(px->GetPredecessor()->GetContactPoint().data(),"start",255) == 0)
                            {
                                if(npre < 0)
                                {
                                    int nedgepre = getroadedge(nroadpre,2);
                                    a = mroadedge[nedgepre].mvertexstart;
                                    lnu.mpFromRoad = GetRoadByID(nroadcur);
                                    lnu.mpToRoad = GetRoadByID(nroadpre);
                                    lnu.mnFromSection = 0;
                                    lnu.mnToSection = lnu.mpToRoad->GetLaneSectionCount() -1;
                                    lnu.mnFromLane = nid;
                                    lnu.mnToLane = npre;
                                }
                                else
                                {
                                    int nedgepre = getroadedge(nroadpre,1);
                                    a = mroadedge[nedgepre].mvertexend;
                                    lnu.mpFromRoad = GetRoadByID(nroadpre);
                                    lnu.mpToRoad = GetRoadByID(nroadcur);
                                    lnu.mnFromSection = lnu.mpFromRoad->GetLaneSectionCount() - 1;
                                    lnu.mnToSection = 0;
                                    lnu.mnFromLane = npre;
                                    lnu.mnToLane = nid;
                                }
                            }
                            if(nid < 0)
                            {
                                int nedgecur = getroadedge(nroadcur,2);
                                b = mroadedge[nedgecur].mvertexstart;
                            }
                            else
                            {
                                int nedgecur = getroadedge(nroadcur,1);
                                b = mroadedge[nedgecur].mvertexend;
                            }
                            iv::vertexsame vx(a,b);
                            xvertexsame.push_back(vx);
                            mlanenet.push_back(lnu);

                        }
                        if(plane->IsSuccessorSet())
                        {

                        }
                    }

                }

            }

            int j;
            for(j=1;j<px->GetLaneSectionCount();j++)
            {
                int k;
                int nroadpre = atoi(px->GetRoadId().data());
                int nroadcur = atoi(px->GetRoadId().data());

                for(k=0;k<px->GetLaneSection(j)->GetLaneCount();k++)
                {
                    Lane * plane = px->GetLaneSection(j)->GetLane(k);

                    if(plane->GetId() == 0)continue;
                    if(plane->IsPredecessorSet())
                    {
                        int a,b;
                        int npre = plane->GetPredecessor();
                        int nid = plane->GetId();
                        lanenetunit lnu;
                        lnu.mpFromRoad = GetRoadByID(nroadcur);
                        lnu.mpToRoad = lnu.mpFromRoad;
                        if(npre < 0)
                        {
                            int nedgepre = getroadedge(nroadpre,2,j-1);
                            a = mroadedge[nedgepre].mvertexend;
                            lnu.mnFromSection = j-1;
                            lnu.mnToSection = j;
                            lnu.mnFromLane = npre;
                            lnu.mnToLane = nid;
                        }
                        else
                        {
                            int nedgepre = getroadedge(nroadpre,1,j-1);
                            a = mroadedge[nedgepre].mvertexstart;
                            lnu.mnFromSection = j;
                            lnu.mnToSection = j-1;
                            lnu.mnFromLane = nid;
                            lnu.mnToLane = npre;
                        }
                        if(nid < 0)
                        {
                            int nedgecur = getroadedge(nroadcur,2,j);
                            b = mroadedge[nedgecur].mvertexstart;
                        }
                        else
                        {
                            int nedgecur = getroadedge(nroadcur,1,j);
                            b = mroadedge[nedgecur].mvertexend;
                        }


                        mlanenet.push_back(lnu);
                        iv::vertexsame vx(a,b);
                        xvertexsame.push_back(vx);

                    }
                    if(plane->IsSuccessorSet())
                    {

                    }
                }

            }

            if(px->GetSuccessor() != 0)
            {
                if(strncmp(px->GetSuccessor()->GetElementType().data(),"road",255) == 0)
                {
                    int k;
                    int nroadnext = atoi(px->GetSuccessor()->GetElementId().data());
                    int nroadcur = atoi(px->GetRoadId().data());

                    if(nroadcur == 116)
                    {
                        int xxxxx = 1;
                    }

                    for(k=0;k<px->GetLaneSection(px->GetLaneSectionCount()-1)->GetLaneCount();k++)
                    {
                        Lane * plane = px->GetLaneSection(px->GetLaneSectionCount()-1)->GetLane(k);
                        if(plane->GetId() == 0)continue;
                        if(plane->IsSuccessorSet())
                        {
                            int a,b;
                            int nnext = plane->GetSuccessor();
                            int nid = plane->GetId();
                            lanenetunit lnu;
                            if(strncmp(px->GetSuccessor()->GetContactPoint().data(),"end",255) == 0)
                            {
                                if(nnext < 0)
                                {
                                    int nedgenext = getroadedge(nroadnext,2,GetRoadByID(nroadnext)->GetLaneSectionCount()-1);
                                    b = mroadedge[nedgenext].mvertexend;

                                    lnu.mpFromRoad = GetRoadByID(nroadnext);
                                    lnu.mpToRoad = GetRoadByID(nroadcur);
                                    lnu.mnFromSection = GetRoadByID(nroadnext)->GetLaneSectionCount()-1;
                                    lnu.mnToSection = lnu.mpToRoad->GetLaneSectionCount() - 1;
                                    lnu.mnFromLane = nnext;
                                    lnu.mnToLane = nid;
                                }
                                else
                                {
                                    int nedgenext = getroadedge(nroadnext,1,GetRoadByID(nroadnext)->GetLaneSectionCount()-1);
                                    b = mroadedge[nedgenext].mvertexstart;

                                    lnu.mpFromRoad = GetRoadByID(nroadcur);
                                    lnu.mpToRoad = GetRoadByID(nroadnext);
                                    lnu.mnFromSection = lnu.mpFromRoad->GetLaneSectionCount() -1;
                                    lnu.mnToSection = GetRoadByID(nroadnext)->GetLaneSectionCount()-1;
                                    lnu.mnFromLane = nid;
                                    lnu.mnToLane = nnext;
                                }

                            }
                            if(strncmp(px->GetSuccessor()->GetContactPoint().data(),"start",255) == 0)
                            {
                                if(nnext < 0)
                                {
                                    int nedgenext = getroadedge(nroadnext,2);
                                    b = mroadedge[nedgenext].mvertexstart;

                                    lnu.mpFromRoad = GetRoadByID(nroadcur);
                                    lnu.mpToRoad = GetRoadByID(nroadnext);
                                    lnu.mnFromSection = lnu.mpFromRoad->GetLaneSectionCount() -1;
                                    lnu.mnToSection = 0;
                                    lnu.mnFromLane = nid;
                                    lnu.mnToLane = nnext;
                                }
                                else
                                {
                                    int nedgenext = getroadedge(nroadnext,1);
                                    b = mroadedge[nedgenext].mvertexend;

                                    lnu.mpFromRoad = GetRoadByID(nroadnext);
                                    lnu.mpToRoad = GetRoadByID(nroadcur);
                                    lnu.mnFromSection = 0;
                                    lnu.mnToSection = lnu.mpToRoad->GetLaneSectionCount() - 1;
                                    lnu.mnFromLane = nnext;
                                    lnu.mnToLane = nid;
                                }
                            }
                            if(nid < 0)
                            {
                                int nedgecur = getroadedge(nroadcur,2,px->GetLaneSectionCount()-1);
                                a = mroadedge[nedgecur].mvertexend;
                            }
                            else
                            {
                                int nedgecur = getroadedge(nroadcur,1,px->GetLaneSectionCount()-1);
                                a = mroadedge[nedgecur].mvertexstart;
                            }
                            iv::vertexsame vx(a,b);
                            xvertexsame.push_back(vx);
                            mlanenet.push_back(lnu);

                        }

                    }
                }

            }

        }



    }

    givlog->debug("vertex same is %d ",xvertexsame.size());

    for(i=0;i<xvertexsame.size();i++)
    {
        givlog->debug("%d: %d %d",i,xvertexsame[i].ma,xvertexsame[i].mb);
    }


    //Merge Same Vertex

    std::vector<iv::vertexsamearray> xvertexsamearray;

    for(i=0;i<xvertexsame.size();i++)
    {
        int a,b;
        a = xvertexsame[i].ma;
        b = xvertexsame[i].mb;

        int j;
        int napos = -1;
        for(j=0;j<xvertexsamearray.size();j++)
        {
            if(xvertexsamearray[j].isinarray(a))
            {
                napos = j;
                break;
            }
        }

        if(napos >= 0)
        {
            if(!xvertexsamearray[napos].isinarray(b))
            {
                xvertexsamearray[napos].mvertexarray.push_back(b);
            }
        }
        else
        {
            int nbpos = -1;
            for(j=0;j<xvertexsamearray.size();j++)
            {
                if(xvertexsamearray[j].isinarray(b))
                {
                    nbpos = j;
                    break;
                }
            }

            if(nbpos >= 0)
            {
               xvertexsamearray[nbpos].mvertexarray.push_back(a);
            }
            else
            {
                iv::vertexsamearray va(a,b);
                xvertexsamearray.push_back(va);
            }
        }
    }

    for(i=0;i<xvertexsamearray.size();i++)
    {
        xvertexsamearray[i].sort();
 //       xvertexsamearray[i].print();
    }

    for(i=0;i<mroadedge.size();i++)
    {
        int j;
        for(j=0;j<xvertexsamearray.size();j++)
        {
            if(xvertexsamearray[j].isinarray(mroadedge[i].mvertexstart))
            {
                mroadedge[i].mvertexstart = xvertexsamearray[j].mvertexarray[0];
            }
            if(xvertexsamearray[j].isinarray(mroadedge[i].mvertexend))
            {
                mroadedge[i].mvertexend = xvertexsamearray[j].mvertexarray[0];
            }
        }
    }

    for(i=0;i<mroadedge.size();i++)
    {
        givlog->debug("%d %d %d: %d %d",mroadedge[i].mroadid,mroadedge[i].mnleftright,mroadedge[i].mnsectionid,  mroadedge[i].mvertexstart,mroadedge[i].mvertexend);
    }



    int * xset = new int[mroadedge.size()*2];
    std::shared_ptr<int> pxset;pxset.reset(xset);
    for(i=0;i<(mroadedge.size()*2);i++)
    {
        xset[i] = -1;
    }

    int nnow = -1;
    std::vector<int > xvalidvertex;
    for(i=0;i<(mroadedge.size()*2);i++)
    {
        bool bfind = false;
        int j;
        int ntempnow = 0;
        for(j=0;j<mroadedge.size();j++)
        {
            if((mroadedge[j].mvertexstart - nnow)>0)
            {
                if(bfind == false)
                {
                    ntempnow = mroadedge[j].mvertexstart;
                    bfind = true;
                }
                else
                {
                    if(mroadedge[j].mvertexstart<ntempnow)ntempnow = mroadedge[j].mvertexstart;
                }
            }
            if((mroadedge[j].mvertexend - nnow)>0)
            {
                if(bfind == false)
                {
                    ntempnow = mroadedge[j].mvertexend;
                    bfind = true;
                }
                else
                {
                    if(mroadedge[j].mvertexend<ntempnow)ntempnow = mroadedge[j].mvertexend;
                }
            }
        }
        if(bfind == false)break;
        else
        {
            nnow = ntempnow;
            xvalidvertex.push_back(nnow);
        }

    }

    for(i=0;i<mroadedge.size();i++)
    {
        int j;
        for(j=0;j<xvalidvertex.size();j++)
        {
            if(mroadedge[i].mvertexstart == xvalidvertex[j])
            {
                mroadedge[i].mvertexstart = j;
                break;
            }
        }
        if(j == xvalidvertex.size())qDebug("not found vertex");
        for(j=0;j<xvalidvertex.size();j++)
        {
            if(mroadedge[i].mvertexend == xvalidvertex[j])
            {
                mroadedge[i].mvertexend = j;
                break;
            }
        }
        if(j == xvalidvertex.size())qDebug("not found vertex");

    }

    for(i=0;i<mroadedge.size();i++)
    {
        givlog->debug("%d %d %d: %d %d",mroadedge[i].mroadid,mroadedge[i].mnleftright,mroadedge[i].mnsectionid, mroadedge[i].mvertexstart,mroadedge[i].mvertexend);
    }


    int nvertexnum = xvalidvertex.size(); //vertex num
    mvertexnum = nvertexnum;

    mvectorvertexedge.clear();

    int nmaxvertex = 0;
    for(i=0;i<nvertexnum;i++)
    {
        if(xvalidvertex[i]>nmaxvertex)nmaxvertex = xvalidvertex[i];
    }

//    for(i=0;i<mroadedge.size();i++)
//    {
//        if(mroadedge[i].mvertexstart>nmaxvertex)nmaxvertex = mroadedge[i].mvertexstart;
//    }

    for(i=0;i<=nmaxvertex;i++)
    {
        vertexedge x;
        mvectorvertexedge.push_back(x);
    }

    for(i=0;i<mroadedge.size();i++)
    {
        mvectorvertexedge[mroadedge[i].mvertexstart].mvectorroadedge.push_back(&mroadedge[i]);
    }

    std::cout<<" max is "<<nmaxvertex<<" num is "<<nvertexnum<<std::endl;
    nmaxvertex = nmaxvertex + 0;




}



int xodrdijkstra::getroadedge(int nroad,int leftright,int nsection )
{
    int nrtn = -1;

    int i;

//    for(i=0;i<mroadedge.size();i++)
//    {
//        if((mroadedge[i].mroadid == nroad) &&(mroadedge[i].mnleftright == leftright) )
//        {
//            qDebug("equal, road %d edge %d ",nroad,i);
//        }
//    }

    for(i=0;i<mroadedge.size();i++)
    {
        if((mroadedge[i].mroadid == nroad) &&(mroadedge[i].mnleftright == leftright) &&(nsection == mroadedge[i].mnsectionid))
        {
            nrtn = i;
            return nrtn;
        }
    }

    return nrtn;

}


inline double xodrdijkstra::getedgedis(int vs, int vd)
{
    int i;

    std::vector<roadedge > * proadedge = &mroadedge;
    double dis = std::numeric_limits<double>::max();

    for(i=0;i<mvectorvertexedge[vs].mvectorroadedge.size();i++)
    {
        if((vs == mvectorvertexedge[vs].mvectorroadedge[i]->mvertexstart)&&(vd == mvectorvertexedge[vs].mvectorroadedge[i]->mvertexend))
        {
            dis = mroadedge[i].mlen;
            return dis;
        }
    }
    return dis;

    for(i=0;i<mroadedge.size();i++)
    {
        if((vs == mroadedge[i].mvertexstart)&&(vd == mroadedge[i].mvertexend))
        {
            dis = mroadedge[i].mlen;
            return dis;
        }
    }
    return dis;
}

int xodrdijkstra::GetRoadSectionIndexByS(Road *pRoad, const double s)
{
    int nrtn = -1;
    if(s<0)return nrtn;
    if(s>pRoad->GetRoadLength())return nrtn;
    int i;
    int nseccount = pRoad->GetLaneSectionCount();
    if(nseccount== 1)return 0;
    for(i=0;i<(nseccount-1);i++)
    {
        if(s<(pRoad->GetLaneSection(i+1)->GetS()))
        {
            break;
        }
    }
    return i;
}

double xodrdijkstra::getpathlength(std::vector<int> xvectorpath)
{
    int i;
    int nsize = xvectorpath.size();
    double flen = 0;
    for(i=0;i<nsize;i++)
    {
        flen = flen + mroadedge[xvectorpath[i]].mlen;
    }
    return flen;
}

std::vector<int> xodrdijkstra::getpath(int srcroadid, int nsrclr, int dstroadid, int ndstlr,const double s_src ,const double s_obj )
{

    std::vector<int> rtnpath;
    int nvertexnum = mvertexnum;
    int i;

    int nsecsrc =0;
    int nsecdst =0;
    nsecsrc = GetRoadSectionIndexByS(GetRoadByID(srcroadid),s_src);
    nsecdst = GetRoadSectionIndexByS(GetRoadByID(dstroadid),s_obj);
    if((nsecsrc<0)||(nsecdst<0))
    {
        qDebug("getpath section error.");
        return rtnpath;
    }
    int srcedge = getroadedge(srcroadid,nsrclr,nsecsrc);
    int dstedge = getroadedge(dstroadid,ndstlr,nsecdst);

//    roadedge * px1 = &mroadedge[srcedge];
//    roadedge * px2 = &mroadedge[dstedge];

    if((srcedge == -1)||(dstedge == -1))
    {
        qDebug("srcedge = %d dstedge = %d ",srcedge,dstedge);
        return rtnpath;
    }

    int * flag = new int[nvertexnum];
    int * prev = new int[nvertexnum];
    double * dist = new double[nvertexnum];
    std::shared_ptr<int> pflag; pflag.reset(flag);
    std::shared_ptr<int> pprev; pprev.reset(prev);
    std::shared_ptr<double> pdist; pdist.reset(dist);
    double min;

    int vs = mroadedge[srcedge].mvertexend;
//    int dstvertex = mroadedge[dstedge].mvertexend;
    int dstvertex = mroadedge[dstedge].mvertexstart;
    if(srcedge == dstedge)dstvertex = mroadedge[dstedge].mvertexstart;
    givlog->debug("src edge is %d vertex is %d",srcedge,vs);
    givlog->debug("dst vertex is %d ",dstvertex);

    // 初始化
    for (i = 0; i < nvertexnum; i++)
    {
        flag[i] = 0;              // 顶点i的最短路径还没获取到。
        prev[i] = vs;              // 顶点i的前驱顶点为0。
        dist[i] = getedgedis(vs,i);// 顶点i的最短路径为"顶点vs"到"顶点i"的权。
    }

    // 对"顶点vs"自身进行初始化
    flag[vs] = 1;
    dist[vs] = 0;

    // 遍历G.vexnum-1次；每次找出一个顶点的最短路径。
    for (i = 1; i < nvertexnum; i++)
    {
 //       qDebug("i = %d",i);
        int k,j;
        // 寻找当前最小的路径；
        // 即，在未获取最短路径的顶点中，找到离vs最近的顶点(k)。
        k = -1;
        min = std::numeric_limits<double>::max();;
        for (j = 0; j < nvertexnum; j++)
        {
            if (flag[j]==0 && dist[j]<min)
            {
                min = dist[j];
                k = j;
            }
        }

        if(min < 0)
        {
            qDebug("hi");
        }

        if(k == -1)
        {
            givlog->debug("i = %d not found k",i);
            break;
        }
        // 标记"顶点k"为已经获取到最短路径
        flag[k] = 1;

        // 修正当前最短路径和前驱顶点
        // 即，当已经"顶点k的最短路径"之后，更新"未获取最短路径的顶点的最短路径和前驱顶点"。
        for (j = 0; j < nvertexnum; j++)
        {
            double tmp;
            tmp = getedgedis(k,j);
            if(tmp == std::numeric_limits<double>::max())
            {

            }
            else
            {
                tmp = min + tmp;
            }
//            tmp = (G.matrix[k][j]==INF ? INF : (min + G.matrix[k][j])); // 防止溢出
            if (flag[j] == 0 && (tmp  < dist[j]) )
            {
                dist[j] = tmp;
                prev[j] = k;
            }
        }
    }

//    for (i = 0; i < nvertexnum; i++)
//        qDebug("  shortest(%d, %d)=%f\n", vs, i, dist[i]);

    for (i = 0; i < nvertexnum; i++)
        givlog->debug("  %d =%d\n", i, prev[i]);


    if(flag[dstvertex] == 1)
    {
        int nstart;
        int nend;
        nend = dstvertex;
        nstart = prev[nend];
        while(nend != vs)
        {
            nstart = prev[nend];
            int nedge = getroadedgefromvertex(nstart,nend);
            if(nedge<0)
            {
                givlog->error("prev path error.");
                break;
            }
            rtnpath.push_back(nedge);
            nend = nstart;
        }
    }

    std::vector<int> rtnpath2;

    rtnpath2.push_back(srcedge);


    for(i=0;i<rtnpath.size();i++)
    {
        givlog->debug("%d %d %d %d",i,mroadedge[rtnpath[rtnpath.size()-1-i]].mroadid,mroadedge[rtnpath[rtnpath.size()-1-i]].mnleftright,
                mroadedge[rtnpath[rtnpath.size()-1-i]].mnsectionid);
        rtnpath2.push_back(rtnpath[rtnpath.size()-1-i]);
    }

/*    if(srcedge == dstedge)*/rtnpath2.push_back(dstedge);

    givlog->debug("rt = %d ",rtnpath.size());
    //gfault->SetFaultState(0,0,"ok");

    return rtnpath2;


}



int xodrdijkstra::getroadedgefromvertex(int nstart, int nend)
{
    int i;
    int nrtn = -1;
    for(i=0;i<mroadedge.size();i++)
    {
        if((nstart == mroadedge[i].mvertexstart)&&(nend == mroadedge[i].mvertexend))
        {
            nrtn = i;
            break;
        }
    }
    return nrtn;
}


Junction * xodrdijkstra::GetJunctionByID(string junctionname)
{
    int i;
    for(i=0;i<mpxodr->GetJunctionCount();i++)
    {
        if(strncmp(mpxodr->GetJunction(i)->GetId().data(),junctionname.data(),255) == 0)
        {
            return mpxodr->GetJunction(i);
        }
    }

    std::cout<<"xodrdijkstra::GetJunctionByID can't find"<<std::endl;
    return 0;
}


/**
  * @brief xodrdijkstra::CalcLaneChange
  * Decide Lane Chage,if need.
  * @param  p1               pre path
  * @param  p2               next path
**/
void xodrdijkstra::CalcLaneChange(pathsection *p1, pathsection *p2)
{
//    int njunctionpos = 1;  //if 1 p1 is junction  if 2 p2 is junction, in junction from it's start have several road.
    int nlr1,nlr2;
    nlr1 = mroadedge[p1->mnroadedgeid].mnleftright;
    nlr2 = mroadedge[p2->mnroadedgeid].mnleftright;

    std::vector<lanenetunit> xln = GetLaneNet(p1->mpRoad,p1->msectionid,p2->mpRoad,p2->msectionid);


    int i;
    int nsel1 = p1->mainsel;
    int nsel2 = p2->mainsel;
    if(IsInLaneNet(xln,p1->mpRoad,p1->msectionid,p1->mainsel,p2->mpRoad,p2->msectionid,p2->mainsel))
    {

    }
    else
    {
        bool bhave = false;
        for(i=0;i<xln.size();i++)
        {
            if(xln.at(i).mnToLane == nsel2)         //Have To Next Lane
            {
                nsel1 = xln.at(i).mnFromLane;
                bhave = true;
                break;
            }
        }
        if(bhave == false)
        {
            bool bhave2 = false;
            for(i=0;i<xln.size();i++)
            {
                if(xln.at(i).mnFromLane == nsel1)       //Have from Lane
                {
                    nsel2 = xln.at(i).mnToLane;
                    bhave2= true;
                    break;
                }
            }
            if(bhave2 == false)
            {
                int nseldiff = 100;
                for(i=0;i<xln.size();i++)
                {
                    if(abs(xln.at(i).mnFromLane - p1->mainsel)<nseldiff) //Find Change Lane Less Lane
                    {
                        nsel1 = xln.at(i).mnFromLane;
                        nsel2 = xln.at(i).mnToLane;
                        nseldiff = abs(xln.at(i).mnFromLane - p1->mainsel);
                    }
                }
            }
        }
    }

    p1->mnEndLaneSel = nsel1;
    p2->mnStartLaneSel = nsel2;

}

/**
  * @brief getgpspoint
  * get gps trace
  * @param srcroadidr        source road id.
  * @param  nsrclr           source road left or right
  * @param  dstroadid        destination road id
  * @param  ndstlr           destination road left or right
  * @param  xvectorpath      grapth path vector,this path is calculated by getpath
**/

std::vector<pathsection> xodrdijkstra::getgpspoint(int srcroadid, int nsrclr, int dstroadid, int ndstlr, std::vector<int> xvectorpath,int nSel)
{

//    int nSel = 1;

    std::vector<int> nvectorlane1;  //Road Start Select Lane
    std::vector<int> nvectorlane2;  //Road Center Select Lane;
    std::vector<int> nvectorlane3;  //Road End Select Lane

    std::vector<pathsection> xpathsection;

    int nlanecount = mroadedge[getroadedge(srcroadid,nsrclr)].mpx->GetLaneSectionCount();
    std::cout<<" section count is "<<nlanecount<<std::endl;;

    //First Get Every Path's lane section for calculate point.
    int i;
    int nsize = xvectorpath.size();

    for(i=0;i<nsize;i++)
    {
         Road * pRoad = mroadedge[xvectorpath[i]].mpx;
         qDebug("road %d is %s",i,pRoad->GetRoadId().data());
    }

    Road * pLastRoad = mroadedge[xvectorpath[nsize-1]].mpx;



    for(i=0;i<nsize;i++)
    {
        pathsection xps;
        Road * pRoad = mroadedge[xvectorpath[i]].mpx;
        int nsuggest = nSel;
        if(mroadedge[xvectorpath[i]].mnleftright == 2)nsuggest  = nsuggest *(-1);
        int nlane = xodrfunc::GetDrivingLane(pRoad,mroadedge[xvectorpath[i]].mnsectionid,nsuggest);
        xps.mainsel = nlane;
        xps.mnStartLaneSel = nlane;
        xps.mnEndLaneSel = nlane;
        xps.mainselindex = 0;//mainselindex;
        xps.mroadid = mroadedge[xvectorpath[i]].mroadid;
        xps.msectionid = mroadedge[xvectorpath[i]].mnsectionid;
        xps.mnroadedgeid = xvectorpath[i];
        xps.mpRoad = pRoad;
        xpathsection.push_back(xps);
    }

    for(i=(nsize-1);i>0;i--)
    {
        Road * pRoad = mroadedge[xvectorpath[i]].mpx;
        LaneSection * pLS = pRoad->GetLaneSection(mroadedge[xvectorpath[i]].mnsectionid);
        Lane * pLane = xodrfunc::GetLaneByID(pLS,xpathsection[i].mainsel);

        Road * pRoad2 = mroadedge[xvectorpath[i-1]].mpx;
        LaneSection * pLS2 = pRoad2->GetLaneSection(mroadedge[xvectorpath[i-1]].mnsectionid);
        Lane * pLane2 = xodrfunc::GetLaneByID(pLS2,xpathsection[i-1].mainsel);

        int nlr = mroadedge[xvectorpath[i]].mnleftright;

        bool bNeedChangeLane = true;
        if(nlr == 2)
        {
            if(pLane2->IsSuccessorSet())
            {
                if(pLane2->GetSuccessor() != xpathsection[i].mainsel)
                {
                    bool bfindgood = false;
                    int ngoodlane = 0;
                    int j;
                    for(j=0;j<pLS2->GetLaneCount();j++)
                    {
                        if(pLS2->GetLane(j)->IsSuccessorSet())
                        {
                            if(pLS2->GetLane(j)->GetSuccessor() == xpathsection[i].mainsel)
                            {
                                ngoodlane = pLS2->GetLane(j)->GetId();
                                bfindgood = true;
                                break;
                            }
                        }
                    }
                    if(bfindgood)
                    {
                        xpathsection[i-1].mainsel = ngoodlane;
                        xpathsection[i-1].mnStartLaneSel = ngoodlane;
                        xpathsection[i-1].mnEndLaneSel = ngoodlane;
                        bNeedChangeLane = false;
                    }
                }
                else
                {
                    bNeedChangeLane = false;
                }
            }
            else
            {
                if(pLane->IsPredecessorSet())
                {
                    if(pLane->GetPredecessor() != xpathsection[i-1].mainsel)
                    {
                        xpathsection[i-1].mainsel = pLane->GetPredecessor();
                        xpathsection[i-1].mnStartLaneSel = pLane->GetPredecessor();
                        xpathsection[i-1].mnEndLaneSel = pLane->GetPredecessor();
                        bNeedChangeLane = false;
                    }

                }
            }
        }
        else
        {
            if(pLane2->IsPredecessorSet())
            {
                if(pLane2->GetPredecessor() != xpathsection[i].mainsel)
                {
                    bool bfindgood = false;
                    int ngoodlane = 0;
                    int j;
                    for(j=0;j<pLS2->GetLaneCount();j++)
                    {
                        if(pLS2->GetLane(j)->IsPredecessorSet())
                        {
                            if(pLS2->GetLane(j)->GetPredecessor() == xpathsection[i].mainsel)
                            {
                                ngoodlane = pLS2->GetLane(j)->GetId();
                                bfindgood = true;
                                break;
                            }
                        }
                    }
                    if(bfindgood)
                    {
                        xpathsection[i-1].mainsel = ngoodlane;
                        xpathsection[i-1].mnStartLaneSel = ngoodlane;
                        xpathsection[i-1].mnEndLaneSel = ngoodlane;
                        bNeedChangeLane = false;
                    }
                }
                else
                {
                    bNeedChangeLane = false;
                }
            }
            else
            {
                if(pLane->IsSuccessorSet())
                {
                    if(pLane->GetSuccessor() != xpathsection[i-1].mainsel)
                    {
                        xpathsection[i-1].mainsel = pLane->GetSuccessor();
                        xpathsection[i-1].mnStartLaneSel = pLane->GetSuccessor();
                        xpathsection[i-1].mnEndLaneSel = pLane->GetSuccessor();
                        bNeedChangeLane = false;
                    }

                }
            }

        }

        if(bNeedChangeLane == true)
        {
            std::cout<<" Road "<<pRoad->GetRoadId()<<" need change lane "<<std::endl;
            if(pRoad == pRoad2)
            {
                std::cout<<" Lane Change in road inter. "<<std::endl;
            }
            else
            {
                if(nlr == 2)
                {
                    if(pLane2->IsSuccessorSet())
                    {
                        xpathsection[i].secondsel = pLane2->GetSuccessor();
                    }
                    else
                    {
                        int k;
                        int xlanecount = pLS->GetLaneCount();
                        for(k=0;k<xlanecount;k++)
                        {
                            if(pLS->GetLane(k)->IsPredecessorSet())
                            {
                                if(pLS->GetLane(k)->GetPredecessor() == pLane2->GetSuccessor())
                                {
                                    xpathsection[i].secondsel = k;
                                    break;
                                }
                            }
                        }
                    }
                }
                else
                {
                    if(pLane2->IsPredecessorSet())
                    {
                        xpathsection[i].secondsel = pLane2->GetPredecessor();
                    }
                    else
                    {
                        int k;
                        int xlanecount = pLS->GetLaneCount();
                        for(k=0;k<xlanecount;k++)
                        {
                            if(pLS->GetLane(k)->IsSuccessorSet())
                            {
                                if(pLS->GetLane(k)->GetSuccessor() == pLane2->GetPredecessor())
                                {
                                    xpathsection[i].secondsel = k;
                                    break;
                                }
                            }
                        }
                    }

                }

            }

            int k;
            for(k=(nsize-1);k>i;k--)
            {
                if(xpathsection[k].mpRoad == xpathsection[i].mpRoad)
                {
                    xpathsection[k].secondsel = xpathsection[i].secondsel;
                }
            }

        }
        else
        {
            xpathsection[i].secondsel = xpathsection[i].mainsel;
            xpathsection[i-1].secondsel = xpathsection[i-1].mainsel;
        }

    }

    for(i=0;i<xpathsection.size();i++)
    {
        std::cout<<"path "<<i<<" road:"<<xpathsection[i].mroadid<<" section "<<xpathsection[i].msectionid
                <<"  lane  "<<xpathsection[i].mainsel<<std::endl;
    }

    return xpathsection;


//    for(i=(nsize-1);i>=0;i--)
//    {
//        Road * pRoad = mroadedge[xvectorpathi]
//    }


    for(i=0;i<nsize;i++)
    {
        Road * pRoad = mroadedge[xvectorpath[i]].mpx;
        int nlr = mroadedge[xvectorpath[i]].mnleftright;
  //      int nsectioncount = pRoad->GetLaneSectionCount();
        int j;
        j = mroadedge[xvectorpath[i]].mnsectionid;
            LaneSection * pLaneSection =  pRoad->GetLaneSection(j);
            int nlane = nSel;
            if(nlane <= 0)nlane = 10;
            int mainselindex = 0;
            if(nlr == 2)nlane = nlane*(-1);
            int nlanecount = pLaneSection->GetLaneCount();
            int k;
            bool bFind = false;
            while(bFind == false)
            {
                for(k=0;k<nlanecount;k++)
                {
                    Lane * pLane = pLaneSection->GetLane(k);
                    if((nlane == pLane->GetId())&&(strncmp(pLane->GetType().data(),"driving",255) == 0))
                    {
                        bFind = true;
                        mainselindex = k;
                        break;
                    }
                }
                if(bFind)continue;
                if(nlr == 1)nlane--;
                else nlane++;
                if(nlane == 0)  //Find A can use lane;
                {
 //                   std::cout<<" Fail. can't find lane"<<std::endl;

                    for(k=0;k<nlanecount;k++)
                    {
                        Lane * pLane = pLaneSection->GetLane(k);
                        if(strncmp(pLane->GetType().data(),"driving",255) == 0)
                        {
                            if(nlr == 1)
                            {
                                if(pLane->GetId()> 0)
                                {
                                    nlane = pLane->GetId();
                                    mainselindex = k;
                                    bFind = true;
                                    break;
                                }

                            }
                            else
                            {
                                if(pLane->GetId()<0)
                                {
                                    nlane = pLane->GetId();
                                    mainselindex = k;
                                    bFind = true;
                                    break;
                                }
                            }
                        }
                    }

                    if(nlane == 0)
                    {
                        std::cout<<" Fail. can't find lane"<<std::endl;
                    }
                    break;
                }
            }


//            std::cout<<" select lane is "<<nlane<<std::endl;
            pathsection xps;
            xps.mainsel = nlane;
            xps.mnStartLaneSel = nlane;
            xps.mnEndLaneSel = nlane;
            xps.mainselindex = mainselindex;
//            if((i>0)&&(pRoad->GetRoadLength()<300))   //Use Connection,not use default.
//            {
//                int nlast = xpathsection[i-1].mainselindex;

//                for(k=0;k<nlanecount;k++)
//                {
//                    Lane * pLane = pLaneSection->GetLane(k);
//                    if(pLane->IsPredecessorSet())
//                    {
//                        if(nlast == pLane->GetPredecessor())
//                        {
//                            xps.mainsel = pLane->GetId();
//                            break;
//                        }
//                    }
//                }
//            }
            xps.mroadid = mroadedge[xvectorpath[i]].mroadid;
            xps.msectionid = j;
            xps.mnroadedgeid = xvectorpath[i];
            xps.mpRoad = pRoad;
            xpathsection.push_back(xps);

    }


    int nps = xpathsection.size();
    for(i=0;i<nps;i++)
    {
        std::cout<<xpathsection[i].mroadid<<" "<<xpathsection[i].msectionid<<" "<<xpathsection[i].mainsel<<" "
                <<xpathsection[i].mainselindex<<std::endl;
    }

    for(i=0;i<(nps-1);i++)
    {
        CalcLaneChange(&xpathsection[i],&xpathsection[i+1]);
    }

    for(i=0;i<nps;i++)
    {
        if((xpathsection[i].mnStartLaneSel == xpathsection[i].mnEndLaneSel) &&(xpathsection[i].mainsel != xpathsection[i].mnStartLaneSel))
        {
            xpathsection[i].mainsel = xpathsection[i].mnStartLaneSel;
        }

        if(xpathsection[i].mnStartLaneSel == xpathsection[i].mainsel)
        {
            xpathsection[i].mbStartToMainChange = false;
        }
        else
        {
            xpathsection[i].mbStartToMainChange = true;
        }
        if(xpathsection[i].mnEndLaneSel == xpathsection[i].mainsel)
        {
            xpathsection[i].mbMainToEndChange = false;
        }
        else
        {
            xpathsection[i].mbMainToEndChange = true;
        }
    }

//    CalcLaneChange(&xpathsection[10], &xpathsection[11]);
//    for(i=1;i<nsize;i++)
//    {
//        pathsection * p1 = &xpathsection[i-1];
//        pathsection * p2 = &xpathsection[i];
//        if(xpathsection[i-1].mpRoad != xpathsection[i].mpRoad)
//        {
//            CalcLaneChange(&xpathsection[i-1], &xpathsection[i]);
//        }
//        else
//        {
//            if(p1->mainsel != p2->mainsel)
//            {
//                p1->mbMainToEndChange = true;
//                p1->mnEndLaneSel = p2->mainsel;
//                if(abs(p1->mainsel) > abs(p2->mainsel))
//                {
//                    p1->mnMainToEndLeftRight = 1;
//                }
//                else
//                {
//                    p1->mnMainToEndLeftRight = 2;
//                }
//            }
//        }
//    }

    for(i=0;i<nsize;i++)
    {
        std::cout<<xpathsection[i].mroadid<<" "<<xpathsection[i].mbStartToMainChange<<"  "
                <<xpathsection[i].mbMainToEndChange<<std::endl;
        if(xpathsection[i].mbStartToMainChange == false)
        {
            xpathsection[i].mnStartLaneSel = xpathsection[i].mainsel;
        }
        if(xpathsection[i].mbMainToEndChange == false)
        {
            xpathsection[i].mnEndLaneSel = xpathsection[i].mainsel;
        }
    }
    return xpathsection;
}

std::vector<lanenetunit> xodrdijkstra::GetLaneNet(Road *p1, int nsec1, Road *p2, int nsec2)
{
    int i;
    std::vector<lanenetunit> xln;
    int nsize = mlanenet.size();
    for(i=0;i<nsize;i++)
    {
        if((p1== mlanenet.at(i).mpFromRoad) &&(p2 == mlanenet.at(i).mpToRoad))
        {

                if((nsec1 == mlanenet.at(i).mnFromSection)&&(nsec2 == mlanenet.at(i).mnToSection))
                {
                    xln.push_back(mlanenet.at(i));
                }
        }
    }
    return xln;
}

bool xodrdijkstra::IsInLaneNet(std::vector<lanenetunit> xln, Road *p1, int nsec1, int nlane1, Road *p2, int nsec2, int nlane2)
{
    int i;
    int nsize = xln.size();
    for(i=0;i<nsize;i++)
    {
        if((p1== xln.at(i).mpFromRoad) &&(p2 == xln.at(i).mpToRoad))
        {

                if((nsec1 == xln.at(i).mnFromSection)&&(nsec2 == xln.at(i).mnToSection))
                {
                    if((nlane1 == xln.at(i).mnFromLane)&&(nlane2 == xln.at(i).mnToLane))
                    {
                        return true;
                    }
                }
        }
    }
    return false;
}