modularization/src/driver/driver_map_xodrload/globalplan.cpp

#include "globalplan.h"
#include "limits"
#include <iostream>

#include <Eigen/Dense>
#include <Eigen/Cholesky>
#include <Eigen/LU>
#include <Eigen/QR>
#include <Eigen/SVD>

#include <QDebug>
#include <QPointF>


using namespace  Eigen;

extern "C"
{
#include "dubins.h"
}


static void AddSignalMark(Road * pRoad, int nlane,std::vector<PlanPoint> & xvectorPP);
/**
 * @brief GetLineDis 获得点到直线Geometry的距离。
 * @param pline
 * @param x
 * @param y
 * @param nearx
 * @param neary
 * @param nearhead
 * @return
 */
static double GetLineDis(GeometryLine * pline,const double x,const double y,double & nearx,
                         double & neary,double & nearhead)
{
    double fRtn = 1000.0;

    double a1,a2,a3,a4,b1,b2;
    double ratio = pline->GetHdg();
    while(ratio >= 2.0* M_PI)ratio = ratio-2.0*M_PI;
    while(ratio<0)ratio = ratio+2.0*M_PI;

    double dis1,dis2,dis3;
    double x1,x2,x3,y1,y2,y3;
    x1 = pline->GetX();y1=pline->GetY();
    if((ratio == 0)||(ratio == M_PI))
    {
        a1 = 0;a4=0;
        a2 = 1;b1= pline->GetY();
        a3 = 1;b2= x;
    }
    else
    {
        if((ratio == 0.5*M_PI)||(ratio == 1.5*M_PI))
        {
            a2=0;a3=0;
            a1=1,b1=pline->GetX();
            a4 = 1;b2 = y;
        }
        else
        {
            a1 = tan(ratio) *(-1.0);
            a2 = 1;
            a3 = tan(ratio+M_PI/2.0)*(-1.0);
            a4 = 1;
            b1 = a1*pline->GetX() + a2 * pline->GetY();
            b2 = a3*x+a4*y;
        }
    }

    y2 = y1 + pline->GetLength() * sin(ratio);
    x2 = x1 + pline->GetLength() * cos(ratio);

    Eigen::Matrix2d A;
    A<<a1,a2,
            a3,a4;
    Eigen::Vector2d B(b1,b2);

    Eigen::Vector2d opoint  = A.lu().solve(B);

 //   std::cout<<opoint<<std::endl;

    x3 = opoint(0);
    y3 = opoint(1);

    dis1 = sqrt(pow(x1-x,2)+pow(y1-y,2));
    dis2 = sqrt(pow(x2-x,2)+pow(y2-y,2));
    dis3 = sqrt(pow(x3-x,2)+pow(y3-y,2));

//    std::cout<<" dis 1 is "<<dis1<<std::endl;
//    std::cout<<" dis 2 is "<<dis2<<std::endl;
//    std::cout<<" dis 3 is "<<dis3<<std::endl;

    if((dis1>pline->GetLength())||(dis2>pline->GetLength()))  //Outoff line
    {
 //       std::cout<<" out line"<<std::endl;
        if(dis1<dis2)
        {
            fRtn = dis1;
            nearx = x1;neary=y1;nearhead = pline->GetHdg();
        }
        else
        {
            fRtn = dis2;
            nearx = x2;neary=y2;nearhead = pline->GetHdg();
        }
    }
    else
    {
        fRtn = dis3;
        nearx = x3;neary=y3;nearhead = pline->GetHdg();
    }


//    std::cout<<"dis is "<<fRtn<<" x is "<<nearx<<" y is "<<neary<<" head is "<<nearhead<<std::endl;
    return fRtn;



}


static int getcirclecrosspoint(QPointF startPos, QPointF endPos, QPointF agvPos, double R,QPointF & point1,QPointF & point2 )
{
     double m=0;
     double k;
     double b;

      //计算分子
      m=startPos.x()-endPos.x();

      //求直线的方程
      if(0==m)
      {
          k=100000;
          b=startPos.y()-k*startPos.x();
      }
      else
      {
          k=(endPos.y()-startPos.y())/(endPos.x()-startPos.x());
          b=startPos.y()-k*startPos.x();
      }
//      qDebug()<<k<<b;

     double temp = fabs(k*agvPos.x()-agvPos.y()+b)/sqrt(k*k+b*b);
      //求直线与圆的交点 前提是圆需要与直线有交点
     if(fabs(k*agvPos.x()-agvPos.y()+b)/sqrt(k*k+b*b)<R)
      {
          point1.setX((2*agvPos.x()-2*k*(b-agvPos.y())+sqrt(pow((2*k*(b-agvPos.y())-2*agvPos.x()),2)-4*(k*k+1)*((b-agvPos.y())*(b-agvPos.y())+agvPos.x()*agvPos.x()-R*R)))/(2*k*k+2));
          point2.setX((2*agvPos.x()-2*k*(b-agvPos.y())-sqrt(pow((2*k*(b-agvPos.y())-2*agvPos.x()),2)-4*(k*k+1)*((b-agvPos.y())*(b-agvPos.y())+agvPos.x()*agvPos.x()-R*R)))/(2*k*k+2));
          point1.setY(k*point1.x()+b);
          point2.setY(k*point2.x()+b);
          return 0;
      }

     return -1;
}


/**
  * @brief CalcHdg
  * 计算点0到点1的航向
  * @param p0        Point 0
  * @param p1        Point 1
**/
static double CalcHdg(QPointF p0, QPointF p1)
{
    double x0,y0,x1,y1;
    x0 = p0.x();
    y0 = p0.y();
    x1 = p1.x();
    y1 = p1.y();
    if(x0 == x1)
    {
        if(y0 < y1)
        {
            return M_PI/2.0;
        }
        else
            return M_PI*3.0/2.0;
    }

    double ratio = (y1-y0)/(x1-x0);

    double hdg = atan(ratio);

    if(ratio > 0)
    {
        if(y1 > y0)
        {

        }
        else
        {
            hdg = hdg + M_PI;
        }
    }
    else
    {
        if(y1 > y0)
        {
            hdg = hdg + M_PI;
        }
        else
        {
            hdg = hdg + 2.0*M_PI;
        }
    }

    return hdg;
}



static bool pointinarc(GeometryArc * parc,QPointF poingarc,QPointF point1)
{
    double hdg = CalcHdg(poingarc,point1);
    if(parc->GetCurvature() >0)hdg = hdg + M_PI/2.0;
    else hdg = hdg - M_PI/2.0;
    if(hdg >= 2.0*M_PI)hdg = hdg - 2.0*M_PI;
    if(hdg < 0)hdg = hdg + 2.0*M_PI;

    double hdgrange = parc->GetLength()/(1.0/parc->GetCurvature());

    double hdgdiff = hdg - parc->GetHdg();

    if(hdgrange >= 0 )
    {
        if(hdgdiff < 0)hdgdiff = hdgdiff + M_PI*2.0;
    }
    else
    {
        if(hdgdiff > 0)hdgdiff = hdgdiff - M_PI*2.0;
    }

    if(fabs(hdgdiff ) < fabs(hdgrange))return true;
    return false;

}

static inline double calcpointdis(QPointF p1,QPointF p2)
{
    return sqrt(pow(p1.x()-p2.x(),2)+pow(p1.y()-p2.y(),2));
}

/**
  * @brief GetArcDis
  * 计算点到圆弧的最短距离，首先用点和圆弧中心点的直线与圆的交点，计算点到交点的距离，如果交点在圆弧上，则最短距离是交点之一，否则计算点到圆弧两个端点的距离。
  * @param parc        pointer to a arc geomery
  * @param x           current x
  * @param y           current y
  * @param nearx       near x
  * @param neary       near y
  * @param nearhead    nearhead
**/

static double GetArcDis(GeometryArc * parc,double x,double y,double & nearx,
                        double & neary,double & nearhead)
{

 //   double fRtn = 1000.0;
    if(parc->GetCurvature() == 0.0)return 1000.0;

    double R = fabs(1.0/parc->GetCurvature());

//    if(parc->GetX() == 4.8213842690322309e+01)
//    {
//        int a = 1;
//        a = 3;
//    }
//    if(parc->GetCurvature() == -0.0000110203)
//    {
//        int a = 1;
//        a = 3;
//    }

    //calculate arc center
    double x_center,y_center;
    if(parc->GetCurvature() > 0)
    {
        x_center = parc->GetX() + R * cos(parc->GetHdg() + M_PI/2.0);
        y_center = parc->GetY() + R * sin(parc->GetHdg()+ M_PI/2.0);
    }
    else
    {
        x_center = parc->GetX() + R * cos(parc->GetHdg() - M_PI/2.0);
        y_center = parc->GetY() + R * sin(parc->GetHdg() - M_PI/2.0);
    }

    double hdgltoa = CalcHdg(QPointF(x,y),QPointF(x_center,y_center));


    QPointF arcpoint;
    arcpoint.setX(x_center);arcpoint.setY(y_center);

    QPointF pointnow;
    pointnow.setX(x);pointnow.setY(y);
    QPointF point1,point2;
    point1.setX(x_center + (R * cos(hdgltoa)));
    point1.setY(y_center + (R * sin(hdgltoa)));
    point2.setX(x_center + (R * cos(hdgltoa + M_PI)));
    point2.setY(y_center + (R * sin(hdgltoa + M_PI)));

    //calculat dis
    bool bp1inarc,bp2inarc;
    bp1inarc =pointinarc(parc,arcpoint,point1);
    bp2inarc =pointinarc(parc,arcpoint,point2);
    double fdis[4];
    fdis[0] = calcpointdis(pointnow,point1);
    fdis[1] = calcpointdis(pointnow,point2);
    fdis[2] = calcpointdis(pointnow,QPointF(parc->GetX(),parc->GetY()));
    QPointF pointend;
    double hdgrange = parc->GetLength()*parc->GetCurvature();
    double hdgend = parc->GetHdg() + hdgrange;
    while(hdgend <0.0)hdgend = hdgend + 2.0 *M_PI;
    while(hdgend >= 2.0*M_PI) hdgend = hdgend -2.0*M_PI;

    if(parc->GetCurvature() >0)
    {
        pointend.setX(arcpoint.x() + R*cos(hdgend -M_PI/2.0 ));
        pointend.setY(arcpoint.y() + R*sin(hdgend -M_PI/2.0) );
    }
    else
    {
        pointend.setX(arcpoint.x() + R*cos(hdgend +M_PI/2.0 ));
        pointend.setY(arcpoint.y() + R*sin(hdgend +M_PI/2.0) );
    }

    fdis[3] = calcpointdis(pointnow,pointend);
    int indexmin = -1;
    double fdismin = 1000000.0;
    if(bp1inarc)
    {
        indexmin = 0;fdismin = fdis[0];
    }
    if(bp2inarc)
    {
        if(indexmin == -1)
        {
            indexmin = 1;fdismin = fdis[1];
        }
        else
        {
            if(fdis[1]<fdismin)
            {
                indexmin = 1;fdismin = fdis[1];
            }
        }
    }
    if(indexmin == -1)
    {
        indexmin = 2;fdismin = fdis[2];
    }
    else
    {
        if(fdis[2]<fdismin)
        {
            indexmin = 2;fdismin = fdis[2];
        }
    }
    if(fdis[3]<fdismin)
    {
        indexmin = 3;fdismin = fdis[3];
    }

    switch (indexmin) {
    case 0:
        nearx = point1.x();
        neary = point1.y();
        if(parc->GetCurvature()<0)
        {
            nearhead = CalcHdg(arcpoint,point1) - M_PI/2.0;
        }
        else
        {
            nearhead = CalcHdg(arcpoint,point1) + M_PI/2.0;
        }
        break;
    case 1:
        nearx = point2.x();
        neary = point2.y();
        if(parc->GetCurvature()<0)
        {
            nearhead = CalcHdg(arcpoint,point2) - M_PI/2.0;
        }
        else
        {
            nearhead = CalcHdg(arcpoint,point2) + M_PI/2.0;
        }
        break;
    case 2:
        nearx = parc->GetX();
        neary = parc->GetY();
        nearhead = parc->GetHdg();
        break;
    case 3:
        nearx = pointend.x();
        neary = pointend.y();
        nearhead = hdgend;
        break;
    default:
        std::cout<<"error in arcdis "<<std::endl;
        break;
    }

    while(nearhead>2.0*M_PI)nearhead = nearhead -2.0*M_PI;
    while(nearhead<0)nearhead = nearhead + 2.0*M_PI;
    return fdismin;
}


static double GetSpiralDis(GeometrySpiral * pspiral,double xnow,double ynow,double & nearx,
                        double & neary,double & nearhead)
{

    double x,y,hdg;
    double s = 0.0;
    double fdismin = 100000.0;
    double s0 = pspiral->GetS();

    while(s<pspiral->GetLength())
    {
        pspiral->GetCoords(s0+s,x,y,hdg);
        s = s+0.1;

        double fdis = calcpointdis(QPointF(x,y),QPointF(xnow,ynow));
        if(fdis<fdismin)
        {
            fdismin = fdis;
            nearhead = hdg;
            nearx = x;
            neary = y;
        }
    }

    return fdismin;
}

/**
 * @brief GetParamPoly3Dis 获得点到贝塞尔曲线的距离。
 * @param parc
 * @param xnow
 * @param ynow
 * @param nearx
 * @param neary
 * @param nearhead
 * @return
 */
static double GetParamPoly3Dis(GeometryParamPoly3 * parc,double xnow,double ynow,double & nearx,
                        double & neary,double & nearhead)
{

    double s = 0.1;
    double fdismin = 100000.0;

    double xold,yold;
    xold = parc->GetX();
    yold = parc->GetY();

    double fdis = calcpointdis(QPointF(parc->GetX(),parc->GetY()),QPointF(xnow,ynow));
    if(fdis<fdismin)
    {
        fdismin = fdis;
        nearhead = parc->GetHdg();
        nearx = parc->GetX();
        neary = parc->GetY();
    }

    double s_start = parc->GetS();

    while(s < parc->GetLength())
    {

        double x, y,hdg;//xtem,ytem;
        parc->GetCoords(s_start+s,x,y,hdg);
        if(fdis<fdismin)
        {
            fdismin = fdis;
            nearhead = hdg;
            nearx = x;
            neary = y;
        }

//        xold = x;
//        yold = y;
        s = s+ 0.1;
    }

    return fdismin;

}


static double GetPoly3Dis(GeometryPoly3 * ppoly,double xnow,double ynow,double & nearx,
                        double & neary,double & nearhead)
{
    double x,y,hdg;
//    double s = 0.0;
    double fdismin = 100000.0;
 //   double s0 = ppoly->GetS();

    x = ppoly->GetX();
    y = ppoly->GetY();
    double A,B,C,D;
    A = ppoly->GetA();
    B = ppoly->GetB();
    C = ppoly->GetC();
    D = ppoly->GetD();
    const double steplim = 0.3;
    double du = steplim;
    double u = 0;
    double v = 0;
    double oldx,oldy;
    oldx = x;
    oldy = y;
    double xstart,ystart;
    xstart = x;
    ystart = y;


    double hdgstart = ppoly->GetHdg();
    double flen = 0;
    u = u+du;
    while(flen < ppoly->GetLength())
    {
        double fdis = 0;
        v = A + B*u + C*u*u + D*u*u*u;
        x = xstart + u*cos(hdgstart) - v*sin(hdgstart);
        y = ystart + u*sin(hdgstart) + v*cos(hdgstart);
        fdis = sqrt(pow(x- oldx,2)+pow(y-oldy,2));

        if(fdis>(steplim*2.0))du = du/2.0;
        flen = flen + fdis;
        u = u + du;
        hdg = CalcHdg(QPointF(oldx,oldy),QPointF(x,y));

        double fdisnow = calcpointdis(QPointF(x,y),QPointF(xnow,ynow));
        if(fdisnow<fdismin)
        {
            fdismin = fdisnow;
            nearhead = hdg;
            nearx = x;
            neary = y;
        }

        oldx = x;
        oldy = y;
    }

    return fdismin;
}


/**
  * @brief GetNearPoint
  * 计算点到圆弧的最短距离，首先用点和圆弧中心点的直线与圆的交点，计算点到交点的距离，如果交点在圆弧上，则最短距离是交点之一，否则计算点到圆弧两个端点的距离。
  * @param x           current x
  * @param y           current y
  * @param pxodr       OpenDrive
  * @param pObjRoad    Road
  * @param pgeo        Geometry of road
  * @param nearx       near x
  * @param neary       near y
  * @param nearhead    nearhead
  * @param nearthresh  near threshhold
  * @retval            if == 0 successfull  <0 fail.
**/
int GetNearPoint(const double x,const double y,OpenDrive * pxodr,Road ** pObjRoad,GeometryBlock ** pgeo, double & s,double & nearx,
                 double & neary,double & nearhead,const double nearthresh,double &froads)
{

    double dismin = std::numeric_limits<double>::infinity();
    s = dismin;
    int i;
    double frels;
    for(i=0;i<pxodr->GetRoadCount();i++)
    {
        int j;
        Road * proad = pxodr->GetRoad(i);
        double nx,ny,nh;

        for(j=0;j<proad->GetGeometryBlockCount();j++)
        {
            GeometryBlock * pgb = proad->GetGeometryBlock(j);
            double dis;
            RoadGeometry * pg;
            pg = pgb->GetGeometryAt(0);

            switch (pg->GetGeomType()) {
            case 0:   //line
                dis = xodrfunc::GetLineDis((GeometryLine *) pg,x,y,nx,ny,nh,frels);
 //               dis = GetLineDis((GeometryLine *) pg,x,y,nx,ny,nh);
                break;
            case 1:
                dis = xodrfunc::GetSpiralDis((GeometrySpiral *)pg,x,y,nx,ny,nh,frels);
                break;
            case 2:  //arc
                dis = xodrfunc::GetArcDis((GeometryArc *)pg,x,y,nx,ny,nh,frels);
                break;

            case 3:
                dis = xodrfunc::GetPoly3Dis((GeometryPoly3 *)pg,x,y,nx,ny,nh,frels);
                break;
            case 4:
                dis = xodrfunc::GetParamPoly3Dis((GeometryParamPoly3 *)pg,x,y,nx,ny,nh,frels);
                break;
            default:
                dis = 100000.0;
                break;
            }

            if(dis < dismin)
            {
                dismin = dis;
                nearx = nx;
                neary = ny;
                nearhead = nh;
                s = dis;
                froads = frels;
                *pObjRoad = proad;
                *pgeo = pgb;
            }


//            if(pgb->CheckIfLine())
//            {
//                GeometryLine * pline = (GeometryLine *)pgb->GetGeometryAt(0);

//                double dis = GetLineDis(pline,x,y,nx,ny,nh);
//                if(dis < dismin)
//                {
//                    dismin = dis;
//                    nearx = nx;
//                    neary = ny;
//                    nearhead = nh;
//                    s = dis;
//                    *pObjRoad = proad;
//                    *pgeo = pgb;
//                }
//            }
//            else
//            {
//                GeometryLine * pline1 = (GeometryLine *)pgb->GetGeometryAt(0);
//                double dis = GetLineDis(pline1,x,y,nx,ny,nh);
//                if(dis < dismin)
//                {
//                    dismin = dis;
//                    nearx = nx;
//                    neary = ny;
//                    nearhead = nh;
//                    s = dis;
//                    *pObjRoad = proad;
//                    *pgeo = pgb;
//                }
//                GeometryArc * parc = (GeometryArc *)pgb->GetGeometryAt(1);
//                dis = GetArcDis(parc,x,y,nx,ny,nh);
//                if(dis < dismin)
//                {
//                    dismin = dis;
//                    nearx = nx;
//                    neary = ny;
//                    nearhead = nh;
//                    s = dis;
//                    *pObjRoad = proad;
//                    *pgeo = pgb;
//                }
//                pline1 = (GeometryLine *)pgb->GetGeometryAt(2);
//                dis = GetLineDis(pline1,x,y,nx,ny,nh);
//                if(dis < dismin)
//                {
//                    dismin = dis;
//                    nearx = nx;
//                    neary = ny;
//                    nearhead = nh;
//                    s = dis;
//                    *pObjRoad = proad;
//                    *pgeo = pgb;
//                }
//            }

        }
    }
    if(s > nearthresh)return -1;
    return 0;
}




int GetNearPointWithHead(const double x,const double y,const double hdg,OpenDrive * pxodr,std::vector<iv::nearroad> & xvectornear,
                         const double nearthresh,bool bhdgvalid = true)
{
    std::cout<<" near thresh "<<nearthresh<<std::endl;
    int i;
    double frels;
    int nminmode = 6;
    for(i=0;i<pxodr->GetRoadCount();i++)
    {
        int j;
        Road * proad = pxodr->GetRoad(i);
        double nx,ny,nh;

        bool bchange = false;
        double localdismin = std::numeric_limits<double>::infinity();

        double localnearx = 0;
        double localneary = 0;
        double localnearhead = 0;
        double locals = 0;
        double localfrels = 0;
        GeometryBlock * pgeolocal;
        localdismin = std::numeric_limits<double>::infinity();

        for(j=0;j<proad->GetGeometryBlockCount();j++)
        {

            GeometryBlock * pgb = proad->GetGeometryBlock(j);
            double dis;
            RoadGeometry * pg;
            pg = pgb->GetGeometryAt(0);

            switch (pg->GetGeomType()) {
            case 0:   //line
                dis = xodrfunc::GetLineDis((GeometryLine *) pg,x,y,nx,ny,nh,frels);
                break;
            case 1:
                dis = xodrfunc::GetSpiralDis((GeometrySpiral *)pg,x,y,nx,ny,nh,frels);
                break;
            case 2:  //arc
                dis = xodrfunc::GetArcDis((GeometryArc *)pg,x,y,nx,ny,nh,frels);
                break;

            case 3:
                dis = xodrfunc::GetPoly3Dis((GeometryPoly3 *)pg,x,y,nx,ny,nh,frels);
                break;
            case 4:
                dis = xodrfunc::GetParamPoly3Dis((GeometryParamPoly3 *)pg,x,y,nx,ny,nh,frels);
                break;
            default:
                dis = 100000.0;
                break;
            }


            if(dis<localdismin)
            {
                localdismin = dis;
                localnearx = nx;
                localneary = ny;
                localnearhead = nh;
                locals = dis;
                localfrels = frels;
                pgeolocal = pgb;
                bchange = true;
            }


        }

        std::cout<<" local dismin "<<localdismin<<std::endl;
        if(localdismin < nearthresh)
        {
            iv::nearroad xnear;
            xnear.pObjRoad = proad;
            xnear.pgeob = pgeolocal;
            xnear.nearx = localnearx;
            xnear.neary = localneary;
            xnear.fdis = localdismin;
            xnear.nearhead = localnearhead;
            xnear.frels = localfrels;
            if((xnear.fdis>0)&&(xnear.frels>0.00001)&&(xnear.frels<(proad->GetRoadLength()-0.00001)))
            {
                double fcalhdg = xodrfunc::CalcHdg(QPointF(xnear.nearx,xnear.neary),QPointF(x,y));
                double fhdgdiff = fcalhdg - xnear.nearhead;
                while(fhdgdiff<0)fhdgdiff = fhdgdiff + 2.0*M_PI;
                while(fhdgdiff>= 2.0*M_PI)fhdgdiff = fhdgdiff - 2.0*M_PI;
                if(fhdgdiff<M_PI)
                {
                    double fdisroad = proad->GetRoadLeftWidth(xnear.frels);
                    if(fdisroad>xnear.fdis)
                    {
                        xnear.fdis = 0;
                    }
                    else
                    {
                        xnear.fdis = xnear.fdis - fdisroad;
                    }
                    xnear.lr = 1;
                }
                else
                {
                    double fdisroad = proad->GetRoadRightWidth(xnear.frels);
                    if(fdisroad>xnear.fdis)
                    {
                        xnear.fdis = 0;
                    }
                    else
                    {
                        xnear.fdis = xnear.fdis - fdisroad;
                    }
                    xnear.lr = 2;
                }


            }
            else
            {
                if(bhdgvalid == false)
                {
                    if(xnear.pObjRoad->GetLaneSectionCount()>0)
                    {
                        LaneSection * pLS = xnear.pObjRoad->GetLaneSection(0);
                        if(pLS->GetRightLaneCount()>0)
                        {
                            xnear.lr = 2;
                        }
                        else
                        {
                            xnear.lr = 1;
                        }
                    }
                    else
                    {
                        xnear.lr = 2;
                    }
                }
            }
            LaneSection * pLS = xnear.pObjRoad->GetLaneSection(0);
            if(pLS != NULL)
            {
            if(xnear.fdis < 0.00001)
            {
                if(bhdgvalid == false)
                {
                    xnear.nmode = 0;
                }
                else
                {
                    double headdiff = hdg - xnear.nearhead;
                    while(headdiff < 2.0*M_PI) headdiff = headdiff + 2.0*M_PI;
                    while(headdiff >= 2.0*M_PI)headdiff = headdiff - 2.0*M_PI;
                    if(((headdiff <M_PI/2.0)||(headdiff > M_PI*3.0/2.0))&&(pLS->GetRightLaneCount()>0))
                    {
                       xnear.nmode = 0;
                    }
                    else
                    {
                        xnear.nmode = 1;
                    }
                }

            }
            else
            {
                if(xnear.fdis<5)
                {
                    if(bhdgvalid == false)
                    {
                        xnear.nmode = 2;
                    }
                    else
                    {
                        double headdiff = hdg - xnear.nearhead;
                        while(headdiff < 2.0*M_PI) headdiff = headdiff + 2.0*M_PI;
                        while(headdiff >= 2.0*M_PI)headdiff = headdiff - 2.0*M_PI;
                        if(((headdiff <M_PI/2.0)||(headdiff > M_PI*3.0/2.0))&&(pLS->GetRightLaneCount()>0))
                        {
                            xnear.nmode = 2;
                        }
                        else
                        {
                            xnear.nmode = 3;
                        }
                    }

                }
                else
                {
                    if(bhdgvalid == false)
                    {
                        xnear.nmode = 4;
                    }
                    else
                    {
                        double headdiff = hdg - xnear.nearhead;
                        while(headdiff < 2.0*M_PI) headdiff = headdiff + 2.0*M_PI;
                        while(headdiff >= 2.0*M_PI)headdiff = headdiff - 2.0*M_PI;
                        if(((headdiff <M_PI/2.0)||(headdiff > M_PI*3.0/2.0))&&(pLS->GetRightLaneCount()>0))
                        {
                            xnear.nmode = 4;
                        }
                        else
                        {
                            xnear.nmode = 5;
                        }
                    }
                }
            }
            if(xnear.nmode < nminmode)nminmode = xnear.nmode;

            if(xnear.pObjRoad->GetLaneSectionCount()>0)
            {
                LaneSection * pLS = xnear.pObjRoad->GetLaneSection(0);
                if((pLS->GetRightLaneCount()>0)&& ((pLS->GetLeftLaneCount()==0)) && (xnear.lr == 1))
                {
                    xnear.lr = 2;
                }
                if((pLS->GetRightLaneCount() == 0)&& ((pLS->GetLeftLaneCount()>0)) && (xnear.lr == 2))
                {
                    xnear.lr = 1;
                }
            }

            xvectornear.push_back(xnear);
            }
        }

    }

    if(xvectornear.size() == 0)
    {
        std::cout<<" no near. "<<std::endl;
        return -1;
    }


//    std::cout<<" near size: "<<xvectornear.size()<<std::endl;
    if(xvectornear.size() > 1)
    {
        int i;
        for(i=0;i<(int)xvectornear.size();i++)
        {
            if(xvectornear[i].nmode > nminmode)
            {
                xvectornear.erase(xvectornear.begin() + i);
                i = i-1;
            }
        }
    }
//    std::cout<<" after size: "<<xvectornear.size()<<std::endl;
    if((xvectornear.size()>1)&&(nminmode>=4))  //if dis > 5 select small dis
    {
        int i;
        while(xvectornear.size()>1)
        {
            if(xvectornear[1].fdis < xvectornear[0].fdis)
            {
                xvectornear.erase(xvectornear.begin());
            }
            else
            {
                xvectornear.erase(xvectornear.begin()+1);
            }
        }
    }
    return 0;

}



/**
  * @brief SelectRoadLeftRight
  * 选择左侧道路或右侧道路
  * 1.选择角度差小于90度的道路一侧，即同侧道路
  * 2.单行路，选择存在的那一侧道路。
  * @param pRoad       道路
  * @param head1       车的航向或目标点的航向
  * @param head_road   目标点的航向
  * @retval            1 left   2 right
**/
static int SelectRoadLeftRight(Road * pRoad,const double head1,const double  head_road)
{
    int nrtn = 2;


    double headdiff = head1 - head_road;
    while(headdiff < 2.0*M_PI) headdiff = headdiff + 2.0*M_PI;
    while(headdiff >= 2.0*M_PI)headdiff = headdiff - 2.0*M_PI;

    bool bSel = false;
    if((headdiff >=M_PI/2.0)&&(headdiff <= M_PI*3.0/2.0))  //if diff is
    {
        if(pRoad->GetLaneSection(0)->GetLeftLaneCount()>0)
        {

            nrtn = 1;
            bSel = true;
        }
    }
    else
    {
        if(pRoad->GetLaneSection(0)->GetRightLaneCount()>0)
        {
            nrtn = 2;
            bSel = true;
        }
    }

    if(bSel == false)
    {
        if(pRoad->GetLaneSection(0)->GetLeftLaneCount() >0)
        {
            nrtn = 1;
        }
        else
        {
            nrtn = 2;
        }
    }

    return nrtn;

}



//static double getlinereldis(GeometryLine * pline,double x,double y)
//{
//    double x0,y0;
//    x0 = pline->GetX();
//    y0 = pline->GetY();
//    double dis = sqrt(pow(x-x0,2) + pow(y-y0,2));
//    if(dis > pline->GetS())
//    {
//        std::cout<<"getlinereldis exceed s S is "<<pline->GetS()<<" dis is "<<dis<<std::endl;
//        return dis;
//    }
//    return dis;
//}

//static double getarcreldis(GeometryArc * parc,double x,double y)
//{
//    double x0,y0;
//    x0 = parc->GetX();
//    y0 = parc->GetY();
//    double dis = sqrt(pow(x-x0,2) + pow(y-y0,2));
//    double R = fabs(1.0/parc->GetCurvature());
//    double ang = 2.0* asin(dis/(2.0*R));
//    double frtn = ang * R;
//    return frtn;
//}

//static double getparampoly3reldis(GeometryParamPoly3 *parc, double xnow, double ynow)
//{
//    double s = 0.1;

//    double xold,yold;
//    xold = parc->GetX();
//    yold = parc->GetY();


//    while(s < parc->GetLength())
//    {

//        double x, y,xtem,ytem;
//        xtem = parc->GetuA() + parc->GetuB() * s + parc->GetuC() * s*s + parc->GetuD() * s*s*s;
//        ytem = parc->GetvA() + parc->GetvB() * s + parc->GetvC() * s*s + parc->GetvD() * s*s*s;
//        x = xtem*cos(parc->GetHdg()) - ytem * sin(parc->GetHdg()) + parc->GetX();
//        y = xtem*sin(parc->GetHdg()) + ytem * cos(parc->GetHdg()) + parc->GetY();

//        if(sqrt(pow(xnow - x,2) + pow(ynow -y,2))<0.3)
//        {
//            break;
//        }

//        xold = x;
//        yold = y;
//        s = s+ 0.1;
//    }

//    return s;
//}

//static double getreldis(RoadGeometry * prg,double x,double y)
//{
//    int ngeotype = prg->GetGeomType();
//    double frtn = 0;
//    switch (ngeotype) {
//    case 0:
//        frtn = getlinereldis((GeometryLine *)prg,x,y);
//        break;
//    case 1:
//        break;
//    case 2:
//        frtn = getarcreldis((GeometryArc *)prg,x,y);
//        break;
//    case 3:
//        break;
//    case 4:
//        frtn = getparampoly3reldis((GeometryParamPoly3 *)prg,x,y);
//        break;
//    default:
//        break;
//    }

//    return frtn;
//}




//static double getposinroad(Road * pRoad,GeometryBlock * pgeob,double x,double y)
//{
//    RoadGeometry* prg = pgeob->GetGeometryAt(0);
//    double s1 = prg->GetS();
//    double srtn = s1;

//    return s1 + getreldis(prg,x,y);
//}


static std::vector<PlanPoint> getlinepoint(GeometryLine * pline,const double fspace = 0.1)
{
    std::vector<PlanPoint> xvectorPP;
    int i;
    double s = pline->GetLength();
    int nsize = s/fspace;
    if(nsize ==0)nsize = 1;

    for(i=0;i<nsize;i++)
    {
        double x,y;
        x = pline->GetX() + i *fspace * cos(pline->GetHdg());
        y = pline->GetY() + i *fspace * sin(pline->GetHdg());
        PlanPoint pp;
        pp.x = x;
        pp.y = y;
        pp.dis = i*fspace;
        pp.hdg = pline->GetHdg();
        pp.mS = pline->GetS() + i*fspace;
        xvectorPP.push_back(pp);
    }
    return xvectorPP;
}

static std::vector<PlanPoint> getarcpoint(GeometryArc * parc,const double fspace = 0.1)
{
    std::vector<PlanPoint> xvectorPP;

    //   double fRtn = 1000.0;
    if(parc->GetCurvature() == 0.0)return xvectorPP;

    double R = fabs(1.0/parc->GetCurvature());

    //calculate arc center
    double x_center = parc->GetX() + (1.0/parc->GetCurvature()) * cos(parc->GetHdg() + M_PI/2.0);
    double y_center = parc->GetY() + (1.0/parc->GetCurvature()) * sin(parc->GetHdg()+ M_PI/2.0);

    double arcdiff = fspace/R;
    int nsize = parc->GetLength()/fspace;
    if(nsize == 0)nsize = 1;
    int i;
    for(i=0;i<nsize;i++)
    {
        double x,y;
        PlanPoint pp;
        if(parc->GetCurvature() > 0)
        {
            x = x_center + R * cos(parc->GetHdg() + i*arcdiff - M_PI/2.0);
            y = y_center + R * sin(parc->GetHdg() + i*arcdiff - M_PI/2.0);
            pp.hdg = parc->GetHdg() + i*arcdiff;
        }
        else
        {
            x = x_center + R * cos(parc->GetHdg() -i*arcdiff + M_PI/2.0);
            y = y_center + R * sin(parc->GetHdg() -i*arcdiff + M_PI/2.0);
            pp.hdg = parc->GetHdg() - i*arcdiff;
        }

        pp.x = x;
        pp.y = y;
        pp.dis = i*fspace;
        pp.mS = parc->GetS() + i*fspace;
        xvectorPP.push_back(pp);
    }
    return xvectorPP;
}


static std::vector<PlanPoint> getspiralpoint(GeometrySpiral * pspiral,const double fspace = 0.1)
{

    double x,y,hdg;
    double s = 0.0;
    double s0 = pspiral->GetS();

    std::vector<PlanPoint> xvectorPP;
    PlanPoint pp;

    while(s<pspiral->GetLength())
    {
        pspiral->GetCoords(s0+s,x,y,hdg);

        pp.x = x;
        pp.y = y;
        pp.hdg = hdg;
        pp.dis = s;
        pp.mS = pspiral->GetS() + s;
        xvectorPP.push_back(pp);

        s = s+fspace;

    }
    return xvectorPP;
}

static std::vector<PlanPoint> getpoly3dpoint(GeometryPoly3 * ppoly,const double fspace = 0.1)
{
    double x,y;
    x = ppoly->GetX();
    y = ppoly->GetY();
    double A,B,C,D;
    A = ppoly->GetA();
    B = ppoly->GetB();
    C = ppoly->GetC();
    D = ppoly->GetD();
    const double steplim = fspace;
    double du = steplim;
    double u = 0;
    double v = 0;
    double oldx,oldy;
    oldx = x;
    oldy = y;
    double xstart,ystart;
    xstart = x;
    ystart = y;
    double hdgstart = ppoly->GetHdg();
    double flen = 0;
    std::vector<PlanPoint> xvectorPP;
    PlanPoint pp;
    pp.x = xstart;
    pp.y = ystart;
    pp.hdg = hdgstart;
    pp.dis = 0;
    pp.mS = ppoly->GetS();
    xvectorPP.push_back(pp);
    u = du;
    while(flen < ppoly->GetLength())
    {
        double fdis = 0;
        v = A + B*u + C*u*u + D*u*u*u;
        x = xstart + u*cos(hdgstart) - v*sin(hdgstart);
        y = ystart + u*sin(hdgstart) + v*cos(hdgstart);
        fdis = sqrt(pow(x- oldx,2)+pow(y-oldy,2));
        double hdg = CalcHdg(QPointF(oldx,oldy),QPointF(x,y));
        oldx = x;
        oldy = y;
        if(fdis>(steplim*2.0))du = du/2.0;
        flen = flen + fdis;
        u = u + du;


        pp.x = x;
        pp.y = y;
        pp.hdg = hdg;

 //       s = s+ dt;
        pp.dis = flen;;
        pp.mS = ppoly->GetS() + flen;
        xvectorPP.push_back(pp);
    }

    return xvectorPP;

}

static std::vector<PlanPoint> getparampoly3dpoint(GeometryParamPoly3 * parc,const double fspace = 0.1)
{
    double s = 0.1;
    std::vector<PlanPoint> xvectorPP;
    PlanPoint pp;

    double xold,yold;
    xold = parc->GetX();
    yold = parc->GetY();
    double hdg0 = parc->GetHdg();
    pp.x = xold;
    pp.y = yold;
    pp.hdg = hdg0;
    pp.dis = 0;
//    xvectorPP.push_back(pp);

    double dt = 1.0;
    double tcount = parc->GetLength()/0.1;
    if(tcount > 0)
    {
        dt = 1.0/tcount;
    }
    double t;
    s = dt;
    s = 0;

    double ua,ub,uc,ud,va,vb,vc,vd;
    ua = parc->GetuA();ub= parc->GetuB();uc= parc->GetuC();ud = parc->GetuD();
    va = parc->GetvA();vb= parc->GetvB();vc= parc->GetvC();vd = parc->GetvD();

    double s_start = parc->GetS();
    while(s < parc->GetLength())
    {
        double x, y,hdg;
        parc->GetCoords(s_start+s,x,y,hdg);
        pp.x = x;
        pp.y = y;
        pp.hdg = hdg;
        s = s+ fspace;
        pp.dis = pp.dis + fspace;;
        pp.mS = s_start + s;
        xvectorPP.push_back(pp);
    }
    return xvectorPP;
}


std::vector<PlanPoint> GetPoint(pathsection xpath,const double fspace = 0.1)
{
    Road * pRoad = xpath.mpRoad;
    //s_start  s_end for select now section data.
    double s_start,s_end;
    LaneSection * pLS = pRoad->GetLaneSection(xpath.msectionid);
    s_start = pLS->GetS();
    if(xpath.msectionid == (pRoad->GetLaneSectionCount()-1))s_end = pRoad->GetRoadLength();
    else
    {
        s_end = pRoad->GetLaneSection(xpath.msectionid+1)->GetS();
    }

//    if(xpath.mroadid == 10190)
//    {
//        int a = 1;
//        a++;
//    }

    std::vector<PlanPoint> xvectorPPS;
    double s = 0;


    int i;
    for(i=0;i<pRoad->GetGeometryBlockCount();i++)
    {
        GeometryBlock * pgb =  pRoad->GetGeometryBlock(i);
        RoadGeometry * prg = pgb->GetGeometryAt(0);
        std::vector<PlanPoint> xvectorPP;
        if(i<(pRoad->GetGeometryBlockCount() -0))
        {
            if(prg->GetS()>s_end)
            {
                continue;
            }
            if((prg->GetS() + prg->GetLength())<s_start)
            {
                continue;
            }
        }


        double s1 = prg->GetLength();

        switch (prg->GetGeomType()) {
        case 0:
            xvectorPP = getlinepoint((GeometryLine *)prg,fspace);

            break;
        case 1:
            xvectorPP = getspiralpoint((GeometrySpiral *)prg,fspace);
            break;
        case 2:
            xvectorPP = getarcpoint((GeometryArc *)prg,fspace);
            break;
        case 3:

            xvectorPP = getpoly3dpoint((GeometryPoly3 *)prg,fspace);
            break;
        case 4:
            xvectorPP = getparampoly3dpoint((GeometryParamPoly3 *)prg,fspace);
            break;
        default:
            break;
        }
        int j;
        if(prg->GetS()<s_start)
        {
            s1 = prg->GetLength() -(s_start - prg->GetS());
        }
        if((prg->GetS() + prg->GetLength())>s_end)
        {
            s1 = s_end - prg->GetS();
        }
        for(j=0;j<xvectorPP.size();j++)
        {
            PlanPoint pp = xvectorPP.at(j);

//            double foffset = pRoad->GetLaneOffsetValue(pp.mS);

//            if(fabs(foffset)>0.001)
//            {
//                pp.x = pp.x + foffset * cos(pp.hdg + M_PI/2.0);
//                pp.y = pp.y + foffset * sin(pp.hdg + M_PI/2.0);
//            }

            pp.mfCurvature = pRoad->GetRoadCurvature(pp.mS);



            if(((pp.dis+prg->GetS()) >= s_start) &&((pp.dis+prg->GetS()) <= s_end))
            {
                if(s_start > prg->GetS())
                {
                    pp.dis = s + pp.dis -(s_start - prg->GetS());
                }
                else
                {
                    pp.dis = s+ pp.dis;
                }
                pp.nRoadID = atoi(pRoad->GetRoadId().data());
                xvectorPPS.push_back(pp);
            }
//            if((prg->GetS()>s_start)&&((prg->GetS() + prg->GetLength())<s_end))
//            {
//                pp.dis = s + pp.dis;
//                pp.nRoadID = atoi(pRoad->GetRoadId().data());
//                xvectorPPS.push_back(pp);
//            }
//            else
//            {
//                if(prg->GetS()<s_start)
//                {

//                }
//                else
//                {
//                    if(pp.dis<(s_end -prg->GetS()))
//                    {
//                        pp.dis = s + pp.dis;
//                        pp.nRoadID = atoi(pRoad->GetRoadId().data());
//                        xvectorPPS.push_back(pp);
//                    }
//                }
//            }
        }
        s = s+ s1;

    }
    return xvectorPPS;
}

std::vector<PlanPoint> GetPoint(Road * pRoad)
{
    std::vector<PlanPoint> xvectorPPS;
    double s = 0;

    int i;
    for(i=0;i<pRoad->GetGeometryBlockCount();i++)
    {
        GeometryBlock * pgb =  pRoad->GetGeometryBlock(i);
        RoadGeometry * prg = pgb->GetGeometryAt(0);
        std::vector<PlanPoint> xvectorPP;
        double s1 = prg->GetLength();
        switch (prg->GetGeomType()) {
        case 0:
            xvectorPP = getlinepoint((GeometryLine *)prg);
            break;
        case 1:
            xvectorPP = getspiralpoint((GeometrySpiral *)prg);
            break;
        case 2:
            xvectorPP = getarcpoint((GeometryArc *)prg);

            break;
        case 3:
            xvectorPP = getpoly3dpoint((GeometryPoly3 *)prg);
            break;
        case 4:
            xvectorPP = getparampoly3dpoint((GeometryParamPoly3 *)prg);
            break;
        default:
            break;
        }
        int j;
        for(j=0;j<xvectorPP.size();j++)
        {
            PlanPoint pp = xvectorPP.at(j);
            pp.dis = s + pp.dis;
            pp.nRoadID = atoi(pRoad->GetRoadId().data());
            xvectorPPS.push_back(pp);
        }
        s = s+ s1;

    }

    unsigned int j;
    for(j=0;j<xvectorPPS.size();j++)
    {
        xvectorPPS.at(j).mfCurvature = pRoad->GetRoadCurvature(xvectorPPS.at(j).mS);
    }
    return xvectorPPS;

}

int indexinroadpoint(std::vector<PlanPoint> xvectorPP,double x,double y)
{
    int nrtn = 0;
    int i;
    int dismin = 1000;
    for(i=0;i<xvectorPP.size();i++)
    {
        double dis = sqrt(pow(xvectorPP.at(i).x - x,2) + pow(xvectorPP.at(i).y -y,2));
        if(dis <dismin)
        {
            dismin = dis;
            nrtn = i;
        }
    }

    if(dismin > 10.0)
    {
        std::cout<<"indexinroadpoint near point is error. dis is "<<dismin<<std::endl;
    }
    return nrtn;
}


double getwidth(Road * pRoad, int nlane)
{
    double frtn = 0;

    int i;
    int nlanecount = pRoad->GetLaneSection(0)->GetLaneCount();
    for(i=0;i<nlanecount;i++)
    {
        if(nlane == pRoad->GetLaneSection(0)->GetLane(i)->GetId())
        {
            LaneWidth* pLW = pRoad->GetLaneSection(0)->GetLane(i)->GetLaneWidth(0);
            if(pLW != 0)
            {
            frtn = pRoad->GetLaneSection(0)->GetLane(i)->GetLaneWidth(0)->GetA();
            break;
            }
        }
    }
    return frtn;
}

double getoff(Road * pRoad,int nlane)
{
    double off = getwidth(pRoad,nlane)/2.0;
    if(nlane < 0)off = off * (-1.0);
 //   int nlanecount = pRoad->GetLaneSection(0)->GetLaneCount();
    int i;
    if(nlane > 0)
        off = off + getwidth(pRoad,0)/2.0;
    else
        off = off - getwidth(pRoad,0)/2.0;
    if(nlane > 0)
    {
        for(i=1;i<nlane;i++)
        {
            off = off + getwidth(pRoad,i);
        }
    }
    else
    {
        for(i=-1;i>nlane;i--)
        {
            off = off - getwidth(pRoad,i);
        }
    }
//    return 0;
    return off;

}


namespace  iv {

struct lanewidthabcd
{
    int nlane;
    double A;
    double B;
    double C;
    double D;
};
}

double getwidth(Road * pRoad, int nlane,std::vector<iv::lanewidthabcd> xvectorlwa,double s)
{
    double frtn = 0;


    int i;
    int nlanecount = xvectorlwa.size();
    for(i=0;i<nlanecount;i++)
    {
        if(nlane == xvectorlwa.at(i).nlane)
        {
            iv::lanewidthabcd * plwa = &xvectorlwa.at(i);
            frtn = plwa->A + plwa->B*s + plwa->C *s*s + plwa->D *s*s*s;
            break;
        }
    }
    return frtn;
}



std::vector<iv::lanewidthabcd> GetLaneWidthABCD(Road * pRoad)
{
    std::vector<iv::lanewidthabcd> xvectorlwa;
    if(pRoad->GetLaneSectionCount()<1)return xvectorlwa;
    int i;


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

//    if(pRoad->GetLaneSectionCount() > 1)
//    {
//        for(i=0;i<(pRoad->GetLaneSectionCount()-1);i++)
//        {
//            if(s>pRoad->GetLaneSection(i+1)->GetS())
//            {
//                pLS = pRoad->GetLaneSection(i+1);
//            }
//            else
//            {
//                break;
//            }
//        }
//    }

    int nlanecount = pLS->GetLaneCount();

    for(i=0;i<nlanecount;i++)
    {
        iv::lanewidthabcd xlwa;


        LaneWidth* pLW = pLS->GetLane(i)->GetLaneWidth(0);
        xlwa.nlane = pLS->GetLane(i)->GetId();
        if(pLW != 0)
        {

            xlwa.A = pLW->GetA();
            xlwa.B = pLW->GetB();
            xlwa.C = pLW->GetC();
            xlwa.D = pLW->GetD();

        }
        else
        {
            xlwa.A = 0;
            xlwa.B = 0;
            xlwa.C = 0;
            xlwa.D = 0;
        }

 //       if(strncmp(pLS->GetLane(i)->GetType().data(),"driving",256) == 0)
        xvectorlwa.push_back(xlwa);   //Calculate Road Width, not driving need add in.

    }
    return xvectorlwa;

}


inline double getoff(int nlane,double s,std::vector<iv::lanewidthabcd> xvectorLWA,std::vector<int> xvectorIndex)
{
    int i;
    double off = 0;
    double a,b,c,d;
    if(xvectorIndex.size() == 0)return off;

    for(i=0;i<(xvectorIndex.size()-1);i++)
    {

        a = xvectorLWA[xvectorIndex[i]].A;
        b = xvectorLWA[xvectorIndex[i]].B;
        c = xvectorLWA[xvectorIndex[i]].C;
        d = xvectorLWA[xvectorIndex[i]].D;
        if(xvectorLWA[xvectorIndex[i]].nlane != 0)
        {
            off = off + a + b*s + c*s*s + d*s*s*s;
        }
        else
        {
            off = off + (a + b*s + c*s*s + d*s*s*s)/2.0;
        }
    }
    i = xvectorIndex.size()-1;
    a = xvectorLWA[xvectorIndex[i]].A;
    b = xvectorLWA[xvectorIndex[i]].B;
    c = xvectorLWA[xvectorIndex[i]].C;
    d = xvectorLWA[xvectorIndex[i]].D;
    off = off + (a + b*s + c*s*s + d*s*s*s)/2.0;
    if(nlane < 0) off = off*(-1.0);
//    std::cout<<"s is "<<s<<std::endl;
//    std::cout<<" off is "<<off<<std::endl;
    return off;

}

double GetRoadWidth(std::vector<iv::lanewidthabcd> xvectorLWA,int nlane,double s)
{
    double fwidth = 0;
    int i;
    double a,b,c,d;

    int nsize = xvectorLWA.size();
    for(i=0;i<nsize;i++)
    {
        if(nlane*(xvectorLWA[i].nlane)>0)
        {
            a = xvectorLWA[i].A;
            b = xvectorLWA[i].B;
            c = xvectorLWA[i].C;
            d = xvectorLWA[i].D;
            fwidth = fwidth + a + b*s +c*s*s + d*s*s*s;
        }
    }
    return fwidth;

}

int GetLaneOriTotal(Road * pRoad, int nlane,double s,int & nori, int & ntotal,double & fSpeed,bool & bNoavoid)
{
    if(pRoad->GetLaneSectionCount() < 1)return -1;

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

    int i;

    if(pRoad->GetLaneSectionCount() > 1)
    {
        for(i=0;i<(pRoad->GetLaneSectionCount()-1);i++)
        {
            if(s>pRoad->GetLaneSection(i+1)->GetS())
            {
                pLS = pRoad->GetLaneSection(i+1);
            }
            else
            {
                break;
            }
        }
    }

    nori = 0;
    ntotal = 0;
    fSpeed = -1; //if -1 no speedlimit for map

    pRoad->GetRoadSpeedMax(s,fSpeed);   //Get Road Speed Limit.

    pRoad->GetRoadNoavoid(s,bNoavoid);

    int nlanecount = pLS->GetLaneCount();
    for(i=0;i<nlanecount;i++)
    {
        int nlaneid = pLS->GetLane(i)->GetId();

        if(nlaneid == nlane)
        {
            Lane * pLane = pLS->GetLane(i);
            if(pLane->GetLaneSpeedCount() > 0)
            {
                LaneSpeed * pLSpeed = pLane->GetLaneSpeed(0);
                int j;
                int nspeedcount = pLane->GetLaneSpeedCount();
                for(j=0;j<(nspeedcount -1);j++)
                {
                    if((s-pLS->GetS())>=pLane->GetLaneSpeed(j+1)->GetS())
                    {
                        pLSpeed = pLane->GetLaneSpeed(j+1);
                    }
                    else
                    {
                        break;
                    }
                }
                if(pLSpeed->GetS()<=(s-pLS->GetS()))fSpeed = pLSpeed->GetMax();

            }
        }
        if(nlane<0)
        {

            if(nlaneid < 0)
            {
                if(strncmp(pLS->GetLane(i)->GetType().data(),"driving",256) == 0)
                {
                    ntotal++;
                    if(nlaneid<nlane)nori++;
                }
            }


        }
        else
        {
            if(nlaneid > 0)
            {
                if(strncmp(pLS->GetLane(i)->GetType().data(),"driving",256) == 0)
                {
                    ntotal++;
                    if(nlaneid > nlane)nori++;
                }
            }
        }
    }

    return 0;
}

std::vector<int> GetLWIndex(std::vector<iv::lanewidthabcd> xvectorLWA,int nlane)
{
    std::vector<int> xvectorindex;
    int i;
    if(nlane >= 0)
    {
    for(i=0;i<=nlane;i++)
    {
       int j;
       int nsize = xvectorLWA.size();
       for(j=0;j<nsize;j++)
       {
           if(i == xvectorLWA.at(j).nlane )
           {
               xvectorindex.push_back(j);
               break;
           }
       }
    }
    }
    else
    {
        for(i=0;i>=nlane;i--)
        {
           int j;
           int nsize = xvectorLWA.size();
           for(j=0;j<nsize;j++)
           {
               if(i == xvectorLWA.at(j).nlane )
               {
                   xvectorindex.push_back(j);
                   break;
               }
           }
        }
    }
    return xvectorindex;
}

void CalcBorringRoad(pathsection xps,std::vector<PlanPoint> & xvectorPP)
{
    if(xps.mpRoad->GetRoadBorrowCount() == 0)
    {
        return;
    }

    Road * pRoad = xps.mpRoad;
    unsigned int nborrowsize = pRoad->GetRoadBorrowCount();
    unsigned int i;
    unsigned int nPPCount = xvectorPP.size();
    for(i=0;i<nborrowsize;i++)
    {
        RoadBorrow * pborrow = pRoad->GetRoadBorrow(i);
        if(pborrow == NULL)
        {
            std::cout<<"warning:can't get borrow"<<std::endl;
            continue;
        }
        if((pborrow->GetMode() == "All")||((pborrow->GetMode()=="R2L")&&(xps.mainsel<0))||((pborrow->GetMode()=="L2R")&&(xps.mainsel>0)))
        {
            unsigned int j;
            double soffset = pborrow->GetS();
            double borrowlen = pborrow->GetLength();
            for(j=0;j<xvectorPP.size();j++)
            {
                if((xvectorPP[j].mS>=soffset)&&(xvectorPP[j].mS<=(soffset + borrowlen)))
                {
                    xvectorPP[j].mbBoringRoad = true;
                }
            }
        }
    }
}

void CalcInLaneAvoid(pathsection xps,std::vector<PlanPoint> & xvectorPP,const double fvehiclewidth,
                     const int nchang1,const int nchang2,const int nchangpoint)
{
    if(xvectorPP.size()<2)return;
    bool bInlaneavoid = false;
    int i;
    if((xps.mbStartToMainChange == false) && (xps.mbMainToEndChange == false))
    {
        if(xps.mpRoad->GetRoadLength()<30)
        {
            double hdgdiff = xvectorPP.at(xvectorPP.size()-1).hdg - xvectorPP.at(0).hdg;
            if(hdgdiff<0)hdgdiff = hdgdiff + 2.0*M_PI;
            if(hdgdiff>(M_PI/3.0)&&(hdgdiff<(5.0*M_PI/3.0)))
                bInlaneavoid = false;
            else
                bInlaneavoid = true;
        }
        else
        {
            bInlaneavoid = true;
        }
    }
    else
    {
        if(xps.mpRoad->GetRoadLength()>100)bInlaneavoid = true;
    }

    if(bInlaneavoid == false)
    {
        int nvpsize = xvectorPP.size();
        for(i=0;i<nvpsize;i++)
        {
            xvectorPP.at(i).bInlaneAvoid = false;
            xvectorPP.at(i).mx_left = xvectorPP.at(i).x;
            xvectorPP.at(i).my_left = xvectorPP.at(i).y;
            xvectorPP.at(i).mx_right = xvectorPP.at(i).x;
            xvectorPP.at(i).my_right = xvectorPP.at(i).y;
        }
    }
    else
    {
      int nvpsize = xvectorPP.size();
      for(i=0;i<nvpsize;i++)xvectorPP.at(i).bInlaneAvoid = true;
      if((xps.mbStartToMainChange == true)||(xps.mbMainToEndChange == true))
      {
          if(xps.mbStartToMainChange == true)
          {
              for(i=(nchang1 - nchangpoint/2);i<(nchang1 + nchangpoint/2);i++)
              {
                  if((i>=0)&&(i<nvpsize))
                    xvectorPP.at(i).bInlaneAvoid = false;
              }

          }
          if(xps.mbMainToEndChange)
          {
              for(i=(nchang2 - nchangpoint/2);i<(nchang2 + nchangpoint/2);i++)
              {
                  if((i>=0)&&(i<nvpsize))
                    xvectorPP.at(i).bInlaneAvoid = false;
              }
          }
      }

      for(i=0;i<nvpsize;i++)
      {
          if(xvectorPP.at(i).bInlaneAvoid)
          {
              double foff = xvectorPP.at(i).mfDisToLaneLeft -0.3-fvehiclewidth*0.5; //30cm + 0.5vehcilewidth
              if(foff < 0.3)
              {
                  xvectorPP.at(i).bInlaneAvoid = false;
                  xvectorPP.at(i).mx_left = xvectorPP.at(i).x;
                  xvectorPP.at(i).my_left = xvectorPP.at(i).y;
                  xvectorPP.at(i).mx_right = xvectorPP.at(i).x;
                  xvectorPP.at(i).my_right = xvectorPP.at(i).y;
              }
              else
              {
                  xvectorPP.at(i).mx_left = xvectorPP.at(i).x + foff * cos(xvectorPP.at(i).hdg + M_PI/2.0);
                  xvectorPP.at(i).my_left = xvectorPP.at(i).y + foff * sin(xvectorPP.at(i).hdg + M_PI/2.0);
                  xvectorPP.at(i).mx_right = xvectorPP.at(i).x + foff * cos(xvectorPP.at(i).hdg - M_PI/2.0);
                  xvectorPP.at(i).my_right = xvectorPP.at(i).y + foff * sin(xvectorPP.at(i).hdg - M_PI/2.0);
              }
          }
      }
    }

}


double getoff(Road * pRoad,int nlane,const double s)
{
    int i;
    int nLSCount = pRoad->GetLaneSectionCount();
    double s_section = 0;

    double foff = 0;

    for(i=0;i<nLSCount;i++)
    {

        LaneSection * pLS = pRoad->GetLaneSection(i);
        if(i<(nLSCount -1))
        {
            if(pRoad->GetLaneSection(i+1)->GetS()<s)
            {
                continue;
            }
        }
        s_section = pLS->GetS();
        int nlanecount = pLS->GetLaneCount();
        int j;
        for(j=0;j<nlanecount;j++)
        {
            Lane * pLane = pLS->GetLane(j);

            int k;
            double s_lane = s-s_section;


            if(pLane->GetId() != 0)
            {

                for(k=0;k<pLane->GetLaneWidthCount();k++)
                {
                    if(k<(pLane->GetLaneWidthCount()-1))
                    {
                        if((pLane->GetLaneWidth(k+1)->GetS()+s_section)<s)
                        {
                            continue;
                        }
                    }
                    s_lane = pLane->GetLaneWidth(k)->GetS();
                    break;
                }
                LaneWidth * pLW  = pLane->GetLaneWidth(k);
                if(pLW == 0)
                {
                    std::cout<<"not find LaneWidth"<<std::endl;
                    break;
                }

                double fds = s - s_lane - s_section;
                double fwidth= pLW->GetA() + pLW->GetB() * fds
                        +pLW->GetC() * pow(fds,2) + pLW->GetD() * pow(fds,3);

//                if((pRoad->GetRoadId() == "211210") &&(nlane == -1) &&(pLane->GetId() == -1))
//                {
//                    qDebug("fs is %f width is %f",fds,fwidth);
//                }
                if(nlane == pLane->GetId())
                {
                    foff = foff + fwidth/2.0;
                }
                if((nlane*(pLane->GetId())>0)&&(abs(nlane)>abs(pLane->GetId())))
                {
                    foff = foff + fwidth;
                }

            }

        }


        break;

    }

    if(pRoad->GetLaneOffsetCount()>0)
    {

        int nLOCount = pRoad->GetLaneOffsetCount();
        int isel = -1;
        for(i=0;i<(nLOCount-1);i++)
        {
            if(pRoad->GetLaneOffset(i+1)->GetS()>s)
            {
                isel = i;
                break;
            }
        }
        if(isel < 0)isel = nLOCount-1;
        LaneOffset * pLO = pRoad->GetLaneOffset(isel);
        double s_off = s - pLO->GetS();
        double off1 = pLO->Geta() + pLO->Getb()*s_off + pLO->Getc() * s_off * s_off
                +pLO->Getd() * s_off * s_off * s_off;
        foff = foff - off1;
    }

    if(nlane<0)foff = foff*(-1);
    return foff;
}


std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP,const double fvehiclewidth)
{
    std::vector<PlanPoint> xvectorPP;

    std::vector<iv::lanewidthabcd> xvectorLWA = GetLaneWidthABCD(xps.mpRoad);
    std::vector<int> xvectorindex1,xvectorindex2,xvectorindex3;
    int nchange1,nchange2;
    int nlane1,nlane2,nlane3;
    if(xps.mnStartLaneSel == xps.mnEndLaneSel)
    {
        xps.mainsel = xps.mnStartLaneSel;
        xps.mbStartToMainChange = false;
        xps.mbMainToEndChange = false;
    }
    else
    {
        if(xps.mpRoad->GetRoadLength() < 100)
        {
            xps.mainsel = xps.mnEndLaneSel;
            xps.mbStartToMainChange = true;
            xps.mbMainToEndChange = false;
        }
    }

    double off1,off2,off3;
    if(xps.mnStartLaneSel < 0)
    {
    off1 = getoff(xps.mpRoad,xps.mnStartLaneSel);
    off2 = getoff(xps.mpRoad,xps.mainsel);
    off3 = getoff(xps.mpRoad,xps.mnEndLaneSel);
    xvectorindex1 = GetLWIndex(xvectorLWA,xps.mnStartLaneSel);
    xvectorindex2 = GetLWIndex(xvectorLWA,xps.mainsel);
    xvectorindex3 = GetLWIndex(xvectorLWA,xps.mnEndLaneSel);
    nlane1 = xps.mnStartLaneSel;
    nlane2 = xps.mainsel;
    nlane3 = xps.mnEndLaneSel;
    }
    else
    {
        off3 = getoff(xps.mpRoad,xps.mnStartLaneSel);
        off2 = getoff(xps.mpRoad,xps.mainsel);
        off1 = getoff(xps.mpRoad,xps.mnEndLaneSel);
        xvectorindex3 = GetLWIndex(xvectorLWA,xps.mnStartLaneSel);
        xvectorindex2 = GetLWIndex(xvectorLWA,xps.mainsel);
        xvectorindex1 = GetLWIndex(xvectorLWA,xps.mnEndLaneSel);
        nlane3 = xps.mnStartLaneSel;
        nlane2 = xps.mainsel;
        nlane1 = xps.mnEndLaneSel;
    }

    int nchangepoint = 300;
    if((nchangepoint * 3) > xvPP.size())
    {
        nchangepoint = xvPP.size()/3;
    }
    int nsize = xvPP.size();

    nchange1 = nsize/3;
    if(nchange1>500)nchange1 = 500;
    nchange2 = nsize*2/3;
    if( (nsize-nchange2)>500)nchange2 = nsize-500;
//    if(nsize < 1000)
//    {
//        std::cout<<"GetLanePoint Error. road id is "<<xps.mpRoad->GetRoadId()<<std::endl;
//        return xvectorPP;
//    }
    double x,y;
    int i;
    if(xps.mainsel < 0)
    {

        for(i=0;i<nsize;i++)
        {
            PlanPoint pp = xvPP.at(i);
    //            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);

            off1 = getoff(xps.mpRoad,nlane2,pp.mS);
            pp.x = pp.x + off1 *cos(pp.hdg + M_PI/2.0);
            pp.y = pp.y + off1 *sin(pp.hdg + M_PI/2.0);

            pp.nlrchange = 1;

            if(xps.mainsel != xps.secondsel)
            {
                off1 = getoff(xps.mpRoad,xps.secondsel,pp.mS);
                pp.mfSecx = xvPP.at(i).x + off1 *cos(pp.hdg + M_PI/2.0);
                pp.mfSecy = xvPP.at(i).y + off1 *sin(pp.hdg + M_PI/2.0);

                if(xps.mainsel >xps.secondsel)
                {
                    pp.nlrchange = 1;
                }
                else
                {
                    pp.nlrchange = 2;
                }
            }
            else
            {
                pp.mfSecx = pp.x ;
                pp.mfSecy = pp.y ;
            }

            pp.mWidth = getwidth(xps.mpRoad,xps.mainsel,xvectorLWA,pp.mS);
            pp.mfDisToLaneLeft = pp.mWidth/2.0;
            pp.lanmp = 0;
            pp.mfDisToRoadLeft = off1*(-1);
            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,xps.mainsel,pp.mS);
            GetLaneOriTotal(xps.mpRoad,xps.mainsel,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);

            xvectorPP.push_back(pp);
        }


    }
    else
    {

        for(i=0;i<nsize;i++)
        {
            PlanPoint pp = xvPP.at(i);
    //            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);


            off1 = getoff(xps.mpRoad,nlane2,pp.mS);
            pp.x = pp.x + off1 *cos(pp.hdg + M_PI/2.0);
            pp.y = pp.y + off1 *sin(pp.hdg + M_PI/2.0);

            pp.nlrchange = 1;

            if(xps.mainsel != xps.secondsel)
            {
                off1 = getoff(xps.mpRoad,xps.secondsel,pp.mS);
                pp.mfSecx = xvPP.at(i).x + off1 *cos(pp.hdg + M_PI/2.0);
                pp.mfSecy = xvPP.at(i).y + off1 *sin(pp.hdg + M_PI/2.0);

                if(xps.mainsel >xps.secondsel)
                {
                    pp.nlrchange = 2;
                }
                else
                {
                    pp.nlrchange = 1;
                }
            }
            else
            {
                pp.mfSecx = pp.x ;
                pp.mfSecy = pp.y ;
            }

            pp.mWidth = getwidth(xps.mpRoad,xps.mainsel,xvectorLWA,pp.mS);
            pp.mfDisToLaneLeft = pp.mWidth/2.0;
            pp.lanmp = 0;
            pp.mfDisToRoadLeft = off1;
            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,xps.mainsel,pp.mS);
            GetLaneOriTotal(xps.mpRoad,xps.mainsel,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);

            pp.hdg = pp.hdg + M_PI;
            xvectorPP.push_back(pp);
        }

//        for(i=0;i<(nchange1- nchangepoint/2);i++)
//        {
//            PlanPoint pp = xvPP.at(i);
//            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
//            pp.x = pp.x + off1 *cos(pp.hdg + M_PI/2.0);
//            pp.y = pp.y + off1 *sin(pp.hdg + M_PI/2.0);
//            pp.hdg = pp.hdg + M_PI;

//            pp.mWidth = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
//            pp.mfDisToLaneLeft = pp.mWidth/2.0;
//            pp.lanmp = 0;
//            pp.mfDisToRoadLeft = off1;
//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane1,pp.mS);
//            GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);


//            xvectorPP.push_back(pp);

//        }
//        for(i=(nchange1 - nchangepoint/2);i<(nchange1+nchangepoint/2);i++)
//        {

//            PlanPoint pp = xvPP.at(i);
//            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
//            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
//            double offx = off1 + (off2 - off1) *(i-nchange1 + nchangepoint/2)/nchangepoint;
//            pp.x = pp.x + offx *cos(pp.hdg + M_PI/2.0);
//            pp.y = pp.y + offx *sin(pp.hdg + M_PI/2.0);
//            pp.hdg = pp.hdg + M_PI;

//            double flanewidth1 = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);

//            pp.mfDisToRoadLeft = offx;
//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane1,pp.mS);
//            if(nlane1 == nlane2)
//            {
//                pp.mWidth = flanewidth1;
//                pp.mfDisToLaneLeft = pp.mWidth/2.0;
//                pp.lanmp = 0;
//                GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
//            }
//            else
//            {
//                if(nlane1 < nlane2)
//                {
//                    pp.lanmp = -1;
//                }
//                else
//                {
//                    pp.lanmp = 1;
//                }

//                if(i<nchange1)
//                {
//                    pp.mWidth = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
//                    double fmove = pp.mWidth * (i-(nchange1 - nchangepoint/2))/nchangepoint;
//                    if(nlane1<nlane2)pp.mfDisToLaneLeft = pp.mWidth/2.0  - fmove;
//                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 + fmove;
//                    GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
//                }
//                else
//                {
//                    pp.mWidth = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
//                    double fmove = pp.mWidth * (nchange1+nchangepoint/2 -i)/nchangepoint;
//                    if(nlane1<nlane2)pp.mfDisToLaneLeft = pp.mWidth/2.0  + fmove;
//                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 - fmove;
//                    GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
//                }

//            }

//            xvectorPP.push_back(pp);

//        }
//        for(i=(nchange1 + nchangepoint/2);i<(nchange2-nchangepoint/2);i++)
//        {
//            PlanPoint pp = xvPP.at(i);
//            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
//            pp.x = pp.x + off2 *cos(pp.hdg + M_PI/2.0);
//            pp.y = pp.y + off2 *sin(pp.hdg + M_PI/2.0);
//            pp.hdg = pp.hdg + M_PI;

//            pp.mWidth = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
//            pp.mfDisToLaneLeft = pp.mWidth/2.0;
//            pp.lanmp = 0;
//            pp.mfDisToRoadLeft = off2;
//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane2,pp.mS);
//            GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);


//            xvectorPP.push_back(pp);

//        }
//        for(i=(nchange2 - nchangepoint/2);i<(nchange2+nchangepoint/2);i++)
//        {

//            PlanPoint pp = xvPP.at(i);
//            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
//            off3 = getoff(nlane3,pp.mS,xvectorLWA,xvectorindex3);
//            double offx = off2 + (off3 - off2) *(i-nchange2 + nchangepoint/2)/nchangepoint;
//            pp.x = pp.x + offx *cos(pp.hdg + M_PI/2.0);
//            pp.y = pp.y + offx *sin(pp.hdg + M_PI/2.0);
//            pp.hdg = pp.hdg + M_PI;

//            double flanewidth1 = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);

//            pp.mfDisToRoadLeft = offx;
//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane2,pp.mS);
//            if(nlane2 == nlane3)
//            {
//                pp.mWidth = flanewidth1;
//                pp.mfDisToLaneLeft = pp.mWidth/2.0;
//                pp.lanmp = 0;
//                GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
//            }
//            else
//            {
//                if(nlane2 < nlane3)
//                {
//                    pp.lanmp = -1;
//                }
//                else
//                {
//                    pp.lanmp = 1;
//                }

//                if(i<nchange2)
//                {
//                    pp.mWidth = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
//                    double fmove = pp.mWidth * (i-(nchange2 - nchangepoint/2))/nchangepoint;
//                    if(nlane2<nlane3)pp.mfDisToLaneLeft = pp.mWidth/2.0  - fmove;
//                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 + fmove;
//                    GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
//                }
//                else
//                {
//                    pp.mWidth = getwidth(xps.mpRoad,nlane3,xvectorLWA,pp.mS);
//                    double fmove = pp.mWidth * (nchange2+nchangepoint/2 -i)/nchangepoint;
//                    if(nlane2<nlane3)pp.mfDisToLaneLeft = pp.mWidth/2.0  + fmove;
//                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 - fmove;
//                    GetLaneOriTotal(xps.mpRoad,nlane3,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
//                }

//            }

//            xvectorPP.push_back(pp);

//        }
//        for(i=(nchange2+ nchangepoint/2);i<nsize;i++)
//        {
//            PlanPoint pp = xvPP.at(i);
//            off3 = getoff(nlane3,pp.mS,xvectorLWA,xvectorindex3);
//            pp.x = pp.x + off3 *cos(pp.hdg + M_PI/2.0);
//            pp.y = pp.y + off3 *sin(pp.hdg + M_PI/2.0);
//            pp.hdg = pp.hdg + M_PI;

//            pp.mWidth = getwidth(xps.mpRoad,nlane3,xvectorLWA,pp.mS);
//            pp.mfDisToLaneLeft = pp.mWidth/2.0;
//            pp.lanmp = 0;
//            pp.mfDisToRoadLeft = off3;
//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane3,pp.mS);
//            GetLaneOriTotal(xps.mpRoad,nlane3,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);


//            xvectorPP.push_back(pp);

//        }

    }



    CalcInLaneAvoid(xps,xvectorPP,fvehiclewidth,nchange1,nchange2,nchangepoint);

    if(xps.mpRoad->GetRoadBorrowCount()>0)
    {
        CalcBorringRoad(xps,xvectorPP);
    }


    if(xps.mnStartLaneSel > 0)
    {
        std::vector<PlanPoint> xvvPP;
        int nvsize = xvectorPP.size();
        for(i=0;i<nvsize;i++)
        {
            xvvPP.push_back(xvectorPP.at(nvsize-1-i));
        }
        AddSignalMark(xps.mpRoad,xps.mainsel,xvvPP);
        return xvvPP;
    }

//    pRoad->GetLaneSection(xps.msectionid)

    AddSignalMark(xps.mpRoad,xps.mainsel,xvectorPP);
    return xvectorPP;
}


static void AddSignalMark(Road * pRoad, int nlane,std::vector<PlanPoint> & xvectorPP)
{
    if(pRoad->GetSignalCount() == 0)return;
    int nsigcount = pRoad->GetSignalCount();
    int i;
    for(i=0;i<nsigcount;i++)
    {
        Signal * pSig = pRoad->GetSignal(i);
        int nfromlane = -100;
        int ntolane = 100;
        signal_laneValidity * pSig_laneValidity = pSig->GetlaneValidity();
        if(pSig_laneValidity != 0)
        {
            nfromlane = pSig_laneValidity->GetfromLane();
            ntolane = pSig_laneValidity->GettoLane();
        }
        if((nlane < 0)&&(nfromlane >= 0))
        {
            continue;
        }
        if((nlane > 0)&&(ntolane<=0))
        {
            continue;
        }

        double s = pSig->Gets();
        double fpos = s/pRoad->GetRoadLength();
        if(nlane > 0)fpos = 1.0 -fpos;
        int npos = fpos *xvectorPP.size();
        if(npos <0)npos = 0;
        if(npos>=xvectorPP.size())npos = xvectorPP.size()-1;
        while(xvectorPP.at(npos).nSignal != -1)
        {
            if(npos > 0)npos--;
            else break;
        }
        while(xvectorPP.at(npos).nSignal != -1)
        {
            if(npos < (xvectorPP.size()-1))npos++;
            else break;
        }
        xvectorPP.at(npos).nSignal = atoi(pSig->Gettype().data());


    }
}

static std::vector<PlanPoint> GetPlanPoint(std::vector<pathsection> xpathsection,Road * pRoad,GeometryBlock * pgeob,
                                          Road * pRoad_obj,GeometryBlock * pgeob_obj,
                                           const double x_now,const double y_now,const double head,
                                           double nearx,double neary, double nearhead,
                                           double nearx_obj,double  neary_obj,const double fvehiclewidth,const double flen = 1000)
{

  std::vector<PlanPoint> xvectorPPs;

  double fspace = 0.1;

  if(flen<2000)fspace = 0.1;
  else
  {
      if(flen<5000)fspace = 0.3;
      else
      {
          if(flen<10000)fspace = 0.5;
          else
              fspace = 1.0;
      }
  }

  int i;


//  std::vector<PlanPoint> xvectorPP = GetPoint( xpathsection[0].mpRoad);
  std::vector<PlanPoint> xvectorPP = GetPoint( xpathsection[0],fspace);


  int indexstart = indexinroadpoint(xvectorPP,nearx,neary);

  std::vector<PlanPoint> xvPP = GetLanePoint(xpathsection[0],xvectorPP,fvehiclewidth);

  if(xpathsection[0].mainsel < 0)
  {
  for(i=indexstart;i<xvPP.size();i++)
  {
      xvectorPPs.push_back(xvPP.at(i));
  }
  }
  else
  {

      for(i=(xvPP.size() -indexstart);i<xvPP.size();i++)
      {
          PlanPoint pp;
          pp = xvPP.at(i);
//          pp.hdg = pp.hdg +M_PI;
          xvectorPPs.push_back(pp);
      }
  }

  int npathlast = xpathsection.size() - 1;
//  while(npathlast >= 0)
//  {
//    if(npathlast == 0)break;
//    if(xpathsection[npathlast].mpRoad != xpathsection[npathlast - 1].mpRoad)break;
//  }
  for(i=1;i<(npathlast);i++)
  {
//      if(xpathsection[i].mpRoad == xpathsection[i-1].mpRoad)
//      {
//          if(xpathsection[i].mainsel * xpathsection[i-1].mainsel >0)
//          {
//            continue;
//          }
//      }
//      if(xpathsection[i].mpRoad == xpathsection[npathlast].mpRoad)
//      {
//          if(xpathsection[i].mainsel * xpathsection[npathlast].mainsel > 0)
//          {
//            break;
//          }
//      }
 //     xvectorPP = GetPoint( xpathsection[i].mpRoad);
      xvectorPP = GetPoint( xpathsection[i],fspace);
      xvPP = GetLanePoint(xpathsection[i],xvectorPP,fvehiclewidth);
//      std::cout<<" road id: "<<xpathsection[i].mroadid<<" section: "
//              <<xpathsection[i].msectionid<<" size is "<<xvectorPP.size()<<std::endl;
//      std::cout<<" road id is "<<xpathsection[i].mpRoad->GetRoadId().data()<<" size is "<<xvPP.size()<<std::endl;
      int j;
      for(j=0;j<xvPP.size();j++)
      {

          PlanPoint pp;
          pp = xvPP.at(j);
//          pp.hdg = pp.hdg +M_PI;
          xvectorPPs.push_back(pp);
      }
  }

  xvectorPP = GetPoint(xpathsection[npathlast],fspace);


//  xvectorPP = GetPoint(xpathsection[npathlast].mpRoad);
  int indexend = indexinroadpoint(xvectorPP,nearx_obj,neary_obj);
  xvPP = GetLanePoint(xpathsection[npathlast],xvectorPP,fvehiclewidth);
  int nlastsize;
  if(xpathsection[npathlast].mainsel<0)
  {
      nlastsize = indexend;
  }
  else
  {
      if(indexend>0) nlastsize = xvPP.size() - indexend;
      else nlastsize = xvPP.size();
  }
  for(i=0;i<nlastsize;i++)
  {
      xvectorPPs.push_back(xvPP.at(i));

  }

  //Find StartPoint
//  if

//  int a = 1;




  return xvectorPPs;
}

std::vector<PlanPoint> gTempPP;
int getPoint(double q[3], double x, void* user_data) {
//    printf("%f, %f, %f, %f\n", q[0], q[1], q[2], x);
    PlanPoint pp;
    pp.x = q[0];
    pp.y = q[1];
    pp.hdg = q[2];
    pp.dis = x;
    pp.speed = *((double *)user_data);
    gTempPP.push_back(pp);
//    std::cout<<pp.x<<" "<<" "<<pp.y<<" "<<pp.hdg<<std::endl;
    return 0;
}

/**
 * @brief getlanesel
 * @param nSel
 * @param pLaneSection
 * @param nlr
 * @return
 */
int getlanesel(int nSel,LaneSection * pLaneSection,int nlr)
{
    int nlane = nSel;
    int mainselindex = 0;
    if(nlr == 2)nlane = nlane*(-1);
    int nlanecount = pLaneSection->GetLaneCount();

    int i;
    int nTrueSel = nSel;
    if(nTrueSel <= 1)nTrueSel= 1;
    int nCurSel = 1;
    if(nlr ==  2)nCurSel = nCurSel *(-1);
    bool bOK = false;
    int nxsel = 1;
    int nlasttid = 0;
    bool bfindlast = false;
    while(bOK == false)
    {
        bool bHaveDriving = false;
        int ncc = 0;
        for(i=0;i<nlanecount;i++)
        {
            Lane * pLane = pLaneSection->GetLane(i);
            if(strncmp(pLane->GetType().data(),"driving",255) == 0 )
            {
                if((nlr == 1)&&(pLane->GetId()>0))
                {
                    ncc++;
                }
                if((nlr == 2)&&(pLane->GetId()<0))
                {
                    ncc++;
                }
                if(ncc == nxsel)
                {
                    bHaveDriving = true;
                    bfindlast = true;
                    nlasttid = pLane->GetId();
                    break;
                }
            }
        }
        if(bHaveDriving == true)
        {
            if(nxsel == nTrueSel)
            {
                return nlasttid;
            }
            else
            {
                nxsel++;
            }
        }
        else
        {
            if(bfindlast)
            {
                return nlasttid;
            }
            else
            {
                std::cout<<"can't find lane."<<std::endl;
                return 0;
            }
            //Use Last
        }

    }

    return 0;

    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)
        {
            std::cout<<" Fail. can't find lane"<<std::endl;
            break;
        }
    }
    return nlane;
}



void checktrace(std::vector<PlanPoint> & xPlan)
{
    int i;
    int nsize = xPlan.size();
    for(i=1;i<nsize;i++)
    {
        double dis = sqrt(pow(xPlan.at(i).x - xPlan.at(i-1).x,2) + pow(xPlan.at(i).y - xPlan.at(i-1).y,2));
        if(dis > 0.3)
        {
            double hdg =  CalcHdg(QPointF(xPlan.at(i-1).x,xPlan.at(i-1).y),QPointF(xPlan.at(i).x,xPlan.at(i).y));

            int ncount = dis/0.1;
            int j;
            for(j=1;j<ncount;j++)
            {
                double x, y;

                PlanPoint pp;
                pp.x = xPlan.at(i-1).x + j*0.1 *cos(hdg);
                pp.y = xPlan.at(i-1).y + j*0.1 *sin(hdg);
                pp.hdg = hdg;
                xPlan.insert(xPlan.begin()+i+j-1,pp);

            }
            qDebug("dis is %f",dis);
        }
    }
}



void ChangeLane(std::vector<PlanPoint> & xvectorPP)
{
    int i = 0;
    int nsize = xvectorPP.size();
    for(i=0;i<nsize;i++)
    {
        if((xvectorPP[i].mfSecx == xvectorPP[i].x)&&(xvectorPP[i].mfSecy == xvectorPP[i].y))
        {

        }
        else
        {
            int k;
            for(k=i;k<(nsize-1);k++)
            {
                if((xvectorPP[k].mfSecx == xvectorPP[k].x)&&(xvectorPP[k].mfSecy == xvectorPP[k].y))
                {
                    break;
                }
            }
            int nnum = k-i;
            int nchangepoint = 300;
            double froadlen = sqrt(pow(xvectorPP[k].x - xvectorPP[i].x,2)
                                   +pow(xvectorPP[k].y - xvectorPP[i].y,2));
            const double fMAXCHANGE = 100;
            if(froadlen<fMAXCHANGE)
            {
                nchangepoint = nnum;
            }
            else
            {
                nchangepoint = (fMAXCHANGE/froadlen) * nnum;
            }

            qDebug(" road %d len is %f changepoint is %d nnum is %d",
                   xvectorPP[k-1].nRoadID,froadlen,nchangepoint,nnum);

            int nstart = i + nnum/2 -nchangepoint/2;
            int nend = i+nnum/2 + nchangepoint/2;
            if(nnum<300)
            {
                nstart = i;
                nend = k;
            }
            int j;
            for(j=i;j<nstart;j++)
            {
                xvectorPP[j].x = xvectorPP[j].mfSecx;
                xvectorPP[j].y = xvectorPP[j].mfSecy;
            }
            nnum = nend - nstart;
            for(j=nstart;j<nend;j++)
            {
                double fdis = sqrt(pow(xvectorPP[j].x - xvectorPP[j].mfSecx,2)
                                   +pow(xvectorPP[j].y - xvectorPP[j].mfSecy,2));
                double foff = fdis *(j-nstart)/nnum;
                if(xvectorPP[j].nlrchange == 1)
                {
//                    std::cout<<"foff is "<<foff<<std::endl;
                    xvectorPP[j].x = xvectorPP[j].mfSecx + foff *cos(xvectorPP[j].hdg + M_PI/2.0);
                    xvectorPP[j].y = xvectorPP[j].mfSecy + foff *sin(xvectorPP[j].hdg + M_PI/2.0);
                    xvectorPP[j].mfDisToRoadLeft = xvectorPP[j].mfDisToRoadLeft + fdis - foff;
                }
                else
                {
                    xvectorPP[j].x = xvectorPP[j].mfSecx + foff *cos(xvectorPP[j].hdg - M_PI/2.0);
                    xvectorPP[j].y = xvectorPP[j].mfSecy + foff *sin(xvectorPP[j].hdg - M_PI/2.0);
                    xvectorPP[j].mfDisToRoadLeft = xvectorPP[j].mfDisToRoadLeft  - fdis +foff;
                }
            }
            i =k;

        }
    }
}

#include <QFile>

/**
 * @brief MakePlan         全局路径规划
 * @param pxd              有向图
 * @param pxodr            OpenDrive地图
 * @param x_now            当前x坐标
 * @param y_now            当前y坐标
 * @param head             当前航向
 * @param x_obj            目标点x坐标
 * @param y_obj            目标点y坐标
 * @param obj_dis          目标点到路径的距离
 * @param srcnearthresh    当前点离最近路径的阈值，如果不在这个范围，寻找失败
 * @param dstnearthresh    目标点离最近路径的阈值，如果不在这个范围，寻找失败
 * @param nlanesel         车道偏好，1为内车道，数值越大越偏外，如果数值不满足，会选择最靠外的。
 * @param xPlan            返回的轨迹点
 * @return                 0 成功  <0 失败
 */
int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const double y_now,const double head,
             const double x_obj,const double y_obj,const double & obj_dis,
             const double srcnearthresh,const double dstnearthresh,
             const int nlanesel,std::vector<PlanPoint> & xPlan,const double fvehiclewidth )
{
    double  s;double nearx;
    double  neary;double  nearhead;
    Road * pRoad;GeometryBlock * pgeob;
    Road * pRoad_obj;GeometryBlock * pgeob_obj;
    double  s_obj;double nearx_obj;
    double  neary_obj;double  nearhead_obj;
    int lr_end = 2;
    int lr_start = 2;
    double fs1,fs2;
//    int nfind = GetNearPoint(x_now,y_now,pxodr,&pRoad,&pgeob,s,nearx,neary,nearhead,srcnearthresh);

    std::vector<iv::nearroad> xvectornearstart;
    std::vector<iv::nearroad> xvectornearend;

    GetNearPointWithHead(x_now,y_now,head,pxodr,xvectornearstart,srcnearthresh,true);
    GetNearPointWithHead(x_obj,y_obj,0,pxodr,xvectornearend,dstnearthresh,false);

    if(xvectornearstart.size()<1)
    {
        std::cout<<" plan fail. can't find start point."<<std::endl;
        return -1;
    }
    if(xvectornearend.size() < 1)
    {
        std::cout<<" plan fail. can't find end point."<<std::endl;
        return -2;
    }

    std::cout<<" start size: "<<xvectornearstart.size()<<std::endl;
    std::cout<<" end size: "<<xvectornearend.size()<<std::endl;

    std::vector<std::vector<PlanPoint>> xvectorplans;
    std::vector<double> xvectorroutedis;

    int i;
    int j;
    for(i=0;i<(int)xvectornearstart.size();i++)
    {
        for(j=0;j<(int)xvectornearend.size();j++)
        {
            iv::nearroad * pnearstart = &xvectornearstart[i];
            iv::nearroad * pnearend = &xvectornearend[j];
            pRoad = pnearstart->pObjRoad;
            pgeob = pnearstart->pgeob;
            pRoad_obj = pnearend->pObjRoad;
            pgeob_obj = pnearend->pgeob;
            lr_start = pnearstart->lr;
            lr_end = pnearend->lr;

            std::cout<<" lr start: "<<lr_start<<" lr end "<<lr_end<<std::endl;

            nearx = pnearstart->nearx;
            neary = pnearstart->neary;

            nearx_obj = pnearend->nearx;
            neary_obj = pnearend->neary;

            bool bNeedDikstra = true;
            if((atoi(pRoad->GetRoadId().data()) == atoi(pRoad_obj->GetRoadId().data()))&&(lr_start == lr_end) &&(pRoad->GetLaneSectionCount() == 1))
            {
                std::vector<PlanPoint> xvPP = GetPoint(pRoad);
                int nindexstart = indexinroadpoint(xvPP,nearx,neary);
                int nindexend =  indexinroadpoint(xvPP,nearx_obj,neary_obj);
                int nlane = getlanesel(nlanesel,pRoad->GetLaneSection(0),lr_start);
                AddSignalMark(pRoad,nlane,xvPP);
                if((nindexend >nindexstart)&&(lr_start == 2))
                {
                    bNeedDikstra = false;
                }
                if((nindexend <nindexstart)&&(lr_start == 1))
                {
                    bNeedDikstra = false;
                }
                if(bNeedDikstra == false)
                {

                    std::vector<iv::lanewidthabcd> xvectorLWA = GetLaneWidthABCD(pRoad);
                    std::vector<int> xvectorindex1;

                    xvectorindex1 = GetLWIndex(xvectorLWA,nlane);

                    double foff = getoff(pRoad,nlane);

                    foff = foff + 0.0;
                    std::vector<PlanPoint> xvectorPP;
                    int i = nindexstart;
                    while(i!=nindexend)
                    {

                        if(lr_start == 2)
                        {
                            PlanPoint pp = xvPP.at(i);
                            foff = getoff(nlane,pp.mS,xvectorLWA,xvectorindex1);
                            pp.x = pp.x + foff *cos(pp.hdg + M_PI/2.0);
                            pp.y = pp.y + foff *sin(pp.hdg + M_PI/2.0);
                            pp.mWidth = getwidth(pRoad,nlane,xvectorLWA,pp.mS);
                            pp.mfDisToLaneLeft = pp.mWidth/2.0;
                            pp.lanmp = 0;
                            pp.mfDisToRoadLeft = foff *(-1.0);
                            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane,pp.mS);
                            pp.mLaneCur = abs(nlane);
                            pp.mVectorLaneWidth = pRoad->GetDrivingLaneWidthVector(pp.mS,lr_start);
                            pp.mLaneCount = pRoad->GetRightDrivingLaneCount(pp.mS);
                            GetLaneOriTotal(pRoad,nlane,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);

                            xvectorPP.push_back(pp);
                            i++;
                        }
                        else
                        {
                            PlanPoint pp = xvPP.at(i);
                            foff = getoff(nlane,pp.mS,xvectorLWA,xvectorindex1);
                            pp.x = pp.x + foff *cos(pp.hdg + M_PI/2.0);
                            pp.y = pp.y + foff *sin(pp.hdg + M_PI/2.0);
                            pp.hdg = pp.hdg + M_PI;

                            pp.mWidth = getwidth(pRoad,nlane,xvectorLWA,pp.mS);
                            pp.mfDisToLaneLeft = pp.mWidth/2.0;
                            pp.lanmp = 0;
                            pp.mfDisToRoadLeft = foff;
                            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane,pp.mS);
                            pp.mLaneCur = abs(nlane);
                            pp.mVectorLaneWidth = pRoad->GetDrivingLaneWidthVector(pp.mS,lr_start);
                            pp.mLaneCount = pRoad->GetLeftDrivingLaneCount(pp.mS);
                            GetLaneOriTotal(pRoad,nlane,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);


                            xvectorPP.push_back(pp);
                            i--;
                        }
                    }

                    for(i=0;i<(int)xvectorPP.size();i++)
                    {
                        xvectorPP[i].mfCurvature = pRoad->GetRoadCurvature(xvectorPP[i].mS);
                    }

                    pathsection xps;
                    xps.mbStartToMainChange = false;
                    xps.mbMainToEndChange = false;
         //           CalcInLaneAvoid(xps,xvectorPP,fvehiclewidth,0,0,0);
                    xPlan = xvectorPP;

                    xvectorplans.push_back(xvectorPP);
                    xvectorroutedis.push_back(xvectorPP.size() * 0.1);

                }
            }

            if(bNeedDikstra)
            {
                bool bSuc = false;
            std::vector<int> xpath = pxd->getpath(atoi(pRoad->GetRoadId().data()),lr_start,atoi(pRoad_obj->GetRoadId().data()),lr_end,bSuc,fs1,fs2);
            if(xpath.size()<1)
            {
                continue;
            }
            if(bSuc == false)
            {
                continue;
            }
            double flen = pxd->getpathlength(xpath);
            std::vector<pathsection> xpathsection = pxd->getgpspoint(atoi(pRoad->GetRoadId().data()),lr_start,atoi(pRoad_obj->GetRoadId().data()),lr_end,xpath,nlanesel);

            std::vector<PlanPoint> xvectorPP = GetPlanPoint(xpathsection,pRoad,pgeob,pRoad_obj,pgeob_obj,x_now,y_now,
                         head,nearx,neary,nearhead,nearx_obj,neary_obj,fvehiclewidth,flen);

            ChangeLane(xvectorPP);
            xvectorplans.push_back(xvectorPP);
            xvectorroutedis.push_back(flen);

            }

        }
    }

    if(xvectorplans.size() > 0)
    {
        if(xvectorplans.size() == 1)
        {
            xPlan = xvectorplans[0];
        }
        else
        {
            int nsel = 0;
            double fdismin = xvectorroutedis[0];
            for(i=1;i<(int)xvectorroutedis.size();i++)
            {
                if(xvectorroutedis[i]<fdismin)
                {
                    nsel = i;
                }
            }
            xPlan = xvectorplans[nsel];
        }
        return 0;
    }
    std::cout<<" plan fail. can't find path."<<std::endl;
    return -1000;

}