modularization/src/tool/map_lanetoxodr/mainwindow.cpp

#include "mainwindow.h"
#include "ui_mainwindow.h"

#include <iomanip>//不要忘记包含此头文件
#include <QScrollBar>

#include "gpsimu.pb.h"

#include "gnss_coordinate_convert.h"

#define VIEW_WIDTH 10000
#define VIEW_HEIGHT 10000

double glon0 = 117.0866293;
double glat0 = 39.1364713;
//double glon0 = 117;
//double glat0 = 39;
double ghdg0 = 360;


MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);

    mnfac = 1;

//    int aaa[10000000];

    mnMoveX = VIEW_WIDTH/2;
    mnMoveY = VIEW_HEIGHT/2;

    mnDefmnfac = mnfac;
    mnDefMoveX = mnMoveX;
    mnDefMoveY = mnMoveY;


    myview = new MyView(this);
    myview->setObjectName(QStringLiteral("graphicsView"));
    myview->setGeometry(QRect(30, 30, 600, 600));

    connect(myview,SIGNAL(dbclickxy(double,double)),this,SLOT(onClickXY(double,double)));

    image = new QImage(VIEW_WIDTH, VIEW_HEIGHT, QImage::Format_RGB32);//画布的初始化大小设为300*300，使用32位颜色
    myview->setCacheMode(myview->CacheBackground);

    painter = new QPainter(image);

    painter->end();

    scene = new QGraphicsScene;


 //    painter->begin(image);

    QTabWidget * p = new QTabWidget(ui->centralWidget);
    p->setGeometry(30,30,300,300);

    mnFontHeight =  ui->centralWidget->fontMetrics().height();

    CreateTab1View(p);

    mTabMain = p;

    QTimer * timer = new QTimer();
    connect(timer,SIGNAL(timeout()),this,SLOT(onTimer()));
    timer->start(1000);

    setWindowTitle("Create Map From Lane Info");

}

MainWindow::~MainWindow()
{
    delete ui;
}

void MainWindow::resizeEvent(QResizeEvent *event)
{
    qDebug("resize");
    QSize sizemain = ui->centralWidget->size();
    qDebug("size x = %d y=%d",sizemain.width(),sizemain.height());

    AdjustWPos(sizemain);

}

void MainWindow::AdjustWPos(QSize sizemain)
{
    myview->setGeometry(0,30,sizemain.width()-mnFontHeight * 22 - 30,sizemain.height());
    mTabMain->setGeometry(sizemain.width()-mnFontHeight * 22,30,mnFontHeight * 22,sizemain.height()-50);
 //   mgplidar->setGeometry(sizemain.width()-280,30,260,200);
}


void MainWindow::ExecPainter()
{



    QTime x;
    x.start();
    //    qDebug("painter.");

   painter->begin(image);
        qDebug("time is %d",x.elapsed());
        image->fill(QColor(255, 255, 255));//对画布进行填充

    //    std::vector<iv::GPSData> navigation_data = brain->navigation_data;
        painter->setRenderHint(QPainter::Antialiasing, true);//设置反锯齿模式，好看一点

        painter->translate(mnMoveX,mnMoveY);


        painter->setPen(Qt::black);

        painter->drawLine(VIEW_WIDTH/(-2),0,VIEW_WIDTH/2,0);
        painter->drawLine(0,VIEW_HEIGHT/(-2),0,VIEW_HEIGHT/2);

        int i;


       //         int nfac = 5;;


        painter->setPen(Qt::blue);
                int nfac = mnfac;

        if(mbClick)
        {
        painter->setPen(Qt::red);
        painter->drawEllipse(QPoint(mClickX ,mClickY),mnMarkSize,mnMarkSize);

        painter->setPen(Qt::black);
        }

    if(mbSetObj)
    {
        painter->setPen(Qt::green);
        painter->drawRect(mfObjX*mnfac-mnMarkSize,mfObjY*mnfac*(-1)-mnMarkSize,mnMarkSize*2,mnMarkSize*2);

        painter->setPen(Qt::black);
    }

    painter->setPen(Qt::green);
    double x0,y0;
    GaussProjCal(glon0,glat0,&x0,&y0);

    painter->setPen(Qt::blue);
//            int nfac = mnfac;
    int selid = mpCBRoad->currentText().toInt();
    for(i=0;i<mxodr.GetRoadCount();i++)
    {
  //     continue;
        int j;
        Road * pRoad = mxodr.GetRoad(i);
        painter->setPen(Qt::blue);
        if(selid == atoi(pRoad->GetRoadId().data()))
        {
            painter->setPen(Qt::red);
        }

        if(mxodr.GetRoad(i)->GetGeometryBlockCount()>0)
        {
            GeometryBlock * pgeob = pRoad->GetGeometryBlock(0);
            double x,y;
            RoadGeometry * pg;
            pg = pgeob->GetGeometryAt(0);
            x = pg->GetX();
            y = pg->GetY();

            double endx,endy,endhdg;
            GetEndPoint(pRoad,endx,endy,endhdg);
            x = (x+endx)/2;
            y = (y+endy)/2;
            painter->drawText(x*mnfac,y*mnfac*(-1),mxodr.GetRoad(i)->GetRoadId().data());
        }
        for(j=0;j<mxodr.GetRoad(i)->GetGeometryBlockCount();j++)
        {
            GeometryBlock * pgeob = pRoad->GetGeometryBlock(j);
            double x,y;
            double x_center,y_center;
            double R;
            RoadGeometry * pg;
            GeometryArc * parc;
            GeometryParamPoly3 * ppp3;
            GeometrySpiral *pSpiral;
            double rel_x,rel_y,rel_hdg;
            pg = pgeob->GetGeometryAt(0);

            x = pg->GetX();
            y = pg->GetY();


            if(j== 0)
            {
                if(selid == atoi(pRoad->GetRoadId().data()))
                {
                    painter->setPen(Qt::green);
                    painter->drawEllipse(x*mnfac-5,y*mnfac*(-1)-5,10,10);
                    painter->setPen(Qt::red);
                }
            }

            switch (pg->GetGeomType()) {
            case 0:
                painter->drawLine(QPoint(x*mnfac,y*mnfac*(-1)),
                                  QPoint((x + pg->GetLength() * cos(pg->GetHdg()))*mnfac,(y + pg->GetLength() * sin(pg->GetHdg()))*mnfac*(-1)));
                break;

            case 1:
                pSpiral = (GeometrySpiral * )pg;
                {
                   int ncount = pSpiral->GetLength() * mnfac;
                   double sstep = pSpiral->GetLength()/((double)ncount);
                   int k;
                   double x0,y0,hdg0,s0;
                   x0 = pSpiral->GetX();
                   y0 = pSpiral->GetY();
                   s0 = pSpiral->GetS();
                   hdg0 = pSpiral->GetHdg() ;
                   painter->setPen(Qt::red);
                   for(k=0;k<ncount;k++)
                   {
                       pSpiral->GetCoords(s0+sstep*k,rel_x,rel_y,rel_hdg);

                       x = rel_x;
                       y = rel_y;
                       painter->drawPoint((int)(x*mnfac),(int)(y*(-1.0*mnfac)));

                   }
                   painter->setPen(Qt::blue);
                }

//                    qDebug("spi");
                break;
            case 2:
                {
                parc = (GeometryArc *)pg;
                R = abs(1.0/parc->GetCurvature());
                if(parc->GetCurvature() > 0)
                {
                    x_center = pg->GetX() + R *cos(pg->GetHdg() + M_PI/2.0);
                    y_center = pg->GetY() + R * sin(pg->GetHdg() + M_PI/2.0);
                }
                else
                {
                    x_center = pg->GetX() + R *cos(pg->GetHdg() -M_PI/2.0);
                    y_center = pg->GetY() + R * sin(pg->GetHdg() - M_PI/2.0);
                }

                int k;
                int ncount = parc->GetLength() * mnfac ;
                double curv = parc->GetCurvature();
                double hdgstep;
                double hdg0 = parc->GetHdg();
                double hdgnow = parc->GetHdg();
                if(ncount > 0) hdgstep= (parc->GetLength()/R)/ncount;
                for(k=0;k<ncount;k++)
                {
                    double x_draw,y_draw;

                    if(curv > 0)
                    {
                        hdgnow =  hdg0 + k*hdgstep;
                        x_draw = x_center + R *cos(hdgnow - M_PI/2.0);
                        y_draw = y_center + R * sin(hdgnow - M_PI/2.0);
                    }
                    else
                    {
                        hdgnow = hdg0 - k * hdgstep;
                        x_draw = x_center + R *cos(hdgnow  + M_PI/2.0);
                        y_draw = y_center + R * sin(hdgnow + M_PI/2.0);
                    }
                    painter->drawPoint(x_draw * mnfac ,y_draw * mnfac *(-1));
                }
                }
                break;
            case 4:
                {
                ppp3 = (GeometryParamPoly3 * )pg;
                int ncount = ppp3->GetLength()* mnfac;
                double sstep;
                if(ncount > 0)sstep = ppp3->GetLength()/ncount;
                else sstep = 10000.0;
                double s = 0;
                while(s < ppp3->GetLength())
                {
                    double xtem,ytem;
                    xtem = ppp3->GetuA() +  ppp3->GetuB() * s +  ppp3->GetuC() * s*s +  ppp3->GetuD() * s*s*s;
                    ytem = ppp3->GetvA() + ppp3->GetvB() * s + ppp3->GetvC() * s*s + ppp3->GetvD() * s*s*s;
                    x = xtem*cos(ppp3->GetHdg()) - ytem * sin(ppp3->GetHdg()) + ppp3->GetX();
                    y = xtem*sin(ppp3->GetHdg()) + ytem * cos(ppp3->GetHdg()) + ppp3->GetY();
                    painter->drawPoint((int)(x*mnfac),(int)(y*(-1.0*mnfac)));
                    s = s+ sstep;
                }
                }
                break;
            default:
                break;
            }

   //         painter->drawPoint((int)(x*mnfac),(int)(y*(-1.0*mnfac)));

        }
    }



    for(i=0;i<mvectorlp.size();i++)
    {
        if(!mbShowLane)continue;
        if(i == mnMarkLane)
        {
            painter->setPen(Qt::red);
        }
        else
        {
            painter->setPen(Qt::blue);
        }
        int npsize = mvectorlp[i].mvectorlpleft.size();
        int j;
        for(j=0;j<npsize;j++)
        {
            double x,y;
            double lon,lat;
            lon = mvectorlp[i].mvectorlpleft[j].mfLon;
            lat = mvectorlp[i].mvectorlpleft[j].mfLat;
            GaussProjCal(lon,lat,&x,&y);
            x = x-x0;
            y= y-y0;
            painter->drawPoint((int)(x*mnfac),(int)(y*(-1.0*mnfac)));
            lon = mvectorlp[i].mvectorlpright[j].mfLon;
            lat = mvectorlp[i].mvectorlpright[j].mfLat;
            GaussProjCal(lon,lat,&x,&y);
            x = x-x0;
            y= y-y0;
            painter->drawPoint((int)(x*mnfac),(int)(y*(-1.0*mnfac)));
        }
    }

       painter->setPen(Qt::green);



        painter->end();
}

//刷新
void MainWindow::paintEvent(QPaintEvent *)
{

    if(mbRefresh)
    {
        ExecPainter();
//        qDebug(" time is %d ",x.elapsed());
        mbRefresh = false;
    }
    scene->clear();
    scene->addPixmap(QPixmap::fromImage(*image));
    myview->setScene(scene);
    myview->show();

    if(mbInit == false)
    {
        myview->horizontalScrollBar()->setValue((mnMoveX - 800));
        myview->verticalScrollBar()->setValue((mnMoveY - 500));
        mbInit = true;
    }
//    qDebug(" time 2 is %d ",x.elapsed());



}


void MainWindow::onTimer()
{

}


void MainWindow::CreateTab1View(QTabWidget * p)
{
    QGroupBox * pGroup = new QGroupBox();
    pGroup->setGeometry(0,0,mnFontHeight * 21,mnFontHeight * 110);

    QLabel * pLabel;
    QLineEdit * pLE;
    QPushButton * pPB;
    QSlider * pSlider;
    QComboBox * pCB;



    int nXPos = 10;
    int nYPos = 30;

    int i;

    int nSpace = mnFontHeight * 65/10;
    int nLEWidth = mnFontHeight * 6;
    int nLEHeight = mnFontHeight * 3/2;

    pLabel = new QLabel(pGroup);
    pLabel->setText("Lat0");
    pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;

    pLabel = new QLabel(pGroup);
    pLabel->setText("Lon0");
    pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;

    pLabel = new QLabel(pGroup);
    pLabel->setText("Head0");
    pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;

    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 2;

    pLE = new QLineEdit(pGroup);
    pLE->setText(QString::number(glat0,'f',7));
    pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    mpLELat0 = pLE;
    nXPos = nXPos + nSpace;

    pLE = new QLineEdit(pGroup);
    pLE->setText(QString::number(glon0,'f',7));
    pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    mpLELon0 = pLE;
    nXPos = nXPos + nSpace;

    pLE = new QLineEdit(pGroup);
    pLE->setText(QString::number(ghdg0,'f',3));
    pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    mpLEHead0 = pLE;
    nXPos = nXPos + nSpace;

    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 4;

    pPB = new QPushButton(pGroup);
    pPB->setText("Restore Default View");
    pPB->setGeometry(nXPos,nYPos,mnFontHeight*10,nLEHeight);
    connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickDefView()));

    nXPos = nXPos + mnFontHeight * 11;
    pPB = new QPushButton(pGroup);
    pPB->setText("Zoom One");
    pPB->setGeometry(nXPos,nYPos,mnFontHeight*6,nLEHeight);
    connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickZoomOne()));

    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 4;

    pLabel = new QLabel(pGroup);
    pLabel->setText("Scale");
    pLabel->setGeometry(nXPos,nYPos,mnFontHeight*3,nLEHeight);

    nXPos = nXPos + mnFontHeight * 4;
    pSlider = new QSlider(pGroup);
    pSlider->setOrientation(Qt::Horizontal);
    pSlider->setGeometry(nXPos,nYPos,mnFontHeight * 10,nLEHeight);
    pSlider->setRange(1,100);
    pSlider->setValue(mnfac);
    connect(pSlider,SIGNAL(valueChanged(int)),this,SLOT(onChangeScale(int)));
    mpSlider_Scale = pSlider;

    nXPos = nXPos + mnFontHeight*11;
    pLE = new QLineEdit(pGroup);
    pLE->setGeometry(nXPos,nYPos,mnFontHeight*3,nLEHeight);
    pLE->setText(QString::number(pSlider->value()));
    mpLE_Scale = pLE;

    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 2;

    pLabel = new QLabel(pGroup);
    pLabel->setText("MoveX");
    pLabel->setGeometry(nXPos,nYPos,mnFontHeight*3,nLEHeight);

    nXPos = nXPos + mnFontHeight * 4;
    pSlider = new QSlider(pGroup);
    pSlider->setOrientation(Qt::Horizontal);
    pSlider->setGeometry(nXPos,nYPos,mnFontHeight * 10,nLEHeight);
    pSlider->setRange(0,100);
    pSlider->setValue(mnMoveX*100/VIEW_WIDTH);
    connect(pSlider,SIGNAL(valueChanged(int)),this,SLOT(onChangeMoveX(int)));
    mpSlider_MoveX = pSlider;

    nXPos = nXPos + mnFontHeight*11;
    pLE = new QLineEdit(pGroup);
    pLE->setGeometry(nXPos,nYPos,mnFontHeight*3,nLEHeight);
    pLE->setText(QString::number(pSlider->value()));
    mpLE_MoveX = pLE;

    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 2;

    pLabel = new QLabel(pGroup);
    pLabel->setText("MoveY");
    pLabel->setGeometry(nXPos,nYPos,mnFontHeight*3,nLEHeight);

    nXPos = nXPos + mnFontHeight * 4;
    pSlider = new QSlider(pGroup);
    pSlider->setOrientation(Qt::Horizontal);
    pSlider->setGeometry(nXPos,nYPos,mnFontHeight * 10,nLEHeight);
    pSlider->setRange(0,100);
    pSlider->setValue(mnMoveY*100/VIEW_HEIGHT);
    connect(pSlider,SIGNAL(valueChanged(int)),this,SLOT(onChangeMoveY(int)));
    mpSlider_MoveY = pSlider;

    nXPos = nXPos + mnFontHeight*11;
    pLE = new QLineEdit(pGroup);
    pLE->setGeometry(nXPos,nYPos,mnFontHeight*3,nLEHeight);
    pLE->setText(QString::number(pSlider->value()));
    mpLE_MoveY = pLE;

    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 4;

    pLabel = new QLabel(pGroup);
    pLabel->setText("Mark");
    pLabel->setGeometry(nXPos,nYPos,mnFontHeight*3,nLEHeight);

    nXPos = nXPos + mnFontHeight * 4;
    pSlider = new QSlider(pGroup);
    pSlider->setOrientation(Qt::Horizontal);
    pSlider->setGeometry(nXPos,nYPos,mnFontHeight * 10,nLEHeight);
    pSlider->setRange(5,100);
    pSlider->setValue(mnMarkSize);
    connect(pSlider,SIGNAL(valueChanged(int)),this,SLOT(onChangeMark(int)));
    mpSlider_Mark = pSlider;

    nXPos = nXPos + mnFontHeight*11;
    pLE = new QLineEdit(pGroup);
    pLE->setGeometry(nXPos,nYPos,mnFontHeight*3,nLEHeight);
    pLE->setText(QString::number(pSlider->value()));
    mpLE_Mark = pLE;

    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 4;

    pLabel = new QLabel(pGroup);
    pLabel->setText("SelLon");
    pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;

    pLabel = new QLabel(pGroup);
    pLabel->setText("SelLat");
    pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;

    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 2;

    pLE = new QLineEdit(pGroup);
    pLE->setText("");
    pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    mpLE_SelX = pLE;
    nXPos = nXPos + nSpace;

    pLE = new QLineEdit(pGroup);
    pLE->setText("");
    pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    mpLE_SelY = pLE;
    nXPos = nXPos + nSpace;



    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 2;

    pLE = new QLineEdit(pGroup);
    pLE->setText("");
    pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    mpLE_SelLon = pLE;
    nXPos = nXPos + nSpace;

    pLE = new QLineEdit(pGroup);
    pLE->setText("");
    pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    mpLE_SelLat = pLE;
    nXPos = nXPos + nSpace;

    pLE = new QLineEdit(pGroup);
    pLE->setText("");
    pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    pLE->setText("360");
    mpLE_StartHeading = pLE;



    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 4;

    pPB = new QPushButton(pGroup);
    pPB->setText("Load Lane");
    pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickLoadLane()));
    nXPos = nXPos + nSpace;

    pCB = new QComboBox(pGroup);
    pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;
    mpCBLane = pCB;

    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 2;

    pPB = new QPushButton(pGroup);
    pPB->setText("Remove");
    pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickRemoveLane()));
    nXPos = nXPos + nSpace;

    pPB = new QPushButton(pGroup);
    pPB->setText("Mark");
    pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickMarkLane()));
    nXPos = nXPos + nSpace;

    pPB = new QPushButton(pGroup);
    pPB->setText("Clear Road Lane");
    pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickClearRoadLane()));
    nXPos = nXPos + nSpace;

    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 2;

    pPB = new QPushButton(pGroup);
    pPB->setText("To Road");
    pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickToRoad()));
    nXPos = nXPos + nSpace;

    pPB = new QPushButton(pGroup);
    pPB->setText("To Opposite");
    pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickToOpposite()));
    nXPos = nXPos + nSpace;

    pLE = new QLineEdit(pGroup);
    pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;
    mpLE_RoadName = pLE;



    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 2;

    pCB = new QComboBox(pGroup);
    pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;
    mpCBSelLane = pCB;

    pCB = new QComboBox(pGroup);
    pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;
    mpCBSelOpLane = pCB;

    pPB = new QPushButton(pGroup);
    pPB->setText("Add Road");
    pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickAddRoad()));
    nXPos = nXPos + nSpace;

    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 4;

    pCB = new QComboBox(pGroup);
    pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;
    mpCBRoad = pCB;
    connect(pCB,SIGNAL(currentIndexChanged(int)),this,SLOT(onClickCBRoadChange(int)));

    pPB = new QPushButton(pGroup);
    pPB->setText("Mark");
    pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickRoadMark()));
    nXPos = nXPos + nSpace;

    pPB = new QPushButton(pGroup);
    pPB->setText("Del");
    pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickRoadDel()));
    nXPos = nXPos + nSpace;

    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 2;

    pLabel = new QLabel(pGroup);
    pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    pLabel->setText("Predecessor:");
    nXPos = nXPos + nSpace;

    pLabel = new QLabel(pGroup);
    pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    pLabel->setText("road");
    mpLabelRoadShowPreType1 = pLabel;
    nXPos = nXPos + nSpace;

    pLabel = new QLabel(pGroup);
    pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    pLabel->setText("10010");
    mpLabelRoadShowPreID = pLabel;
    nXPos = nXPos + nSpace;

    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 2;

    pLabel = new QLabel(pGroup);
    pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    pLabel->setText("start");
    mpLabelRoadShowPreType2 = pLabel;
    nXPos = nXPos + nSpace;

    pCB = new QComboBox(pGroup);
    pCB->setGeometry(nXPos,nYPos,nLEWidth*2,nLEHeight);
    mpCBRoadShowPre = pCB;

    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 2;

    pLabel = new QLabel(pGroup);
    pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    pLabel->setText("Successor:");
    nXPos = nXPos + nSpace;

    pLabel = new QLabel(pGroup);
    pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    pLabel->setText("road");
    mpLabelRoadShowNxtType1 = pLabel;
    nXPos = nXPos + nSpace;

    pLabel = new QLabel(pGroup);
    pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    pLabel->setText("10010");
    mpLabelRoadShowNxtID = pLabel;
    nXPos = nXPos + nSpace;

    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 2;

    pLabel = new QLabel(pGroup);
    pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    pLabel->setText("start");
    mpLabelRoadShowNxtType2 = pLabel;
    nXPos = nXPos + nSpace;

    pCB = new QComboBox(pGroup);
    pCB->setGeometry(nXPos,nYPos,nLEWidth*2,nLEHeight);
    mpCBRoadShowNext = pCB;

//    nXPos = 10;
//    nYPos = nYPos + mnFontHeight * 2;

//    pCB = new QComboBox(pGroup);
//    pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
//    nXPos = nXPos + nSpace;
//    mpCBPreNxtCurLane = pCB;

    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 2;

    pCB = new QComboBox(pGroup);
    pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;
    mpCBPreNxtRoad = pCB;
    connect(pCB,SIGNAL(currentIndexChanged(int)),this,SLOT(onClickPreNxtRoadChange(int)));

    pCB = new QComboBox(pGroup);
    pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;
    mpCBPreNxtRelLane = pCB;

    pCB = new QComboBox(pGroup);
    pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;
    mpCBPreNxtConatact = pCB;
    pCB->addItem("start");
    pCB->addItem("end");

    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 2;

    pPB = new QPushButton(pGroup);
    pPB->setGeometry(nXPos,nYPos,nLEWidth*15/10,nLEHeight);
    nXPos = nXPos + nSpace*15/10;
    pPB->setText("Set Predecessor");
    connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickSetRoadPredecessor()));

    pPB = new QPushButton(pGroup);
    pPB->setGeometry(nXPos,nYPos,nLEWidth*15/10,nLEHeight);
    nXPos = nXPos + nSpace*15/10;
    pPB->setText("Set Successor");
    connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickSetRoadSuccessor()));

    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 2;

    pPB = new QPushButton(pGroup);
    pPB->setGeometry(nXPos,nYPos,nLEWidth*15/10,nLEHeight);
    nXPos = nXPos + nSpace*15/10;
    pPB->setText("Del Predecessor");
    connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickDelRoadPredecessor()));

    pPB = new QPushButton(pGroup);
    pPB->setGeometry(nXPos,nYPos,nLEWidth*15/10,nLEHeight);
    nXPos = nXPos + nSpace*15/10;
    pPB->setText("Del Successor");
    connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickDelRoadSuccessor()));


    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 4;

    pCB = new QComboBox(pGroup);
    pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;
    mpCBRoad1 = pCB;

    pCB = new QComboBox(pGroup);
    pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;
    pCB->addItem("start");
    pCB->addItem("end");
    mpCBRC1 = pCB;

    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 2;

    pCB = new QComboBox(pGroup);
    pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;
    mpCBRoad2 = pCB;

    pCB = new QComboBox(pGroup);
    pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;
    pCB->addItem("start");
    pCB->addItem("end");
    mpCBRC2 = pCB;

    pPB = new QPushButton(pGroup);
    pPB->setText("Road Contact");
    pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickRoadContact()));

    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 4;


    pCB = new QComboBox(pGroup);
    pCB->setGeometry(nXPos,nYPos,nLEWidth*2,nLEHeight);
    nXPos = nXPos + nSpace;
    mpCBRoadCon = pCB;

    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 2;

    pCB = new QComboBox(pGroup);
    pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;
    mpCBLane1 = pCB;

    pCB = new QComboBox(pGroup);
    pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;
    mpCBLane2 = pCB;


    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 2;

    pPB = new QPushButton(pGroup);
    pPB->setText("Lane Contact");
    pPB->setGeometry(nXPos,nYPos,nLEWidth*15/10,nLEHeight);
    connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickLaneContact()));
    nXPos = nXPos + nSpace*15/10;

    pCB = new QComboBox(pGroup);
    pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;
    mpCBLane1Lane2 = pCB;

    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 2;

    pPB = new QPushButton(pGroup);
    pPB->setText("OpLane Contact");
    pPB->setGeometry(nXPos,nYPos,nLEWidth*15/10,nLEHeight);
    connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickOpLaneContact()));
    nXPos = nXPos + nSpace*15/10;

    pCB = new QComboBox(pGroup);
    pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;
    mpCBLane1Lane2op = pCB;

    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 2;

    pPB = new QPushButton(pGroup);
    pPB->setText("Clear");
    pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;

    pPB = new QPushButton(pGroup);
    pPB->setText("Create Road");
    pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickCreateRoad()));

    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 2;

    pCB = new QComboBox(pGroup);
    pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;
    mpCBRoadType = pCB;
    pCB->addItem("Turn");
    pCB->addItem("Straight");
    pCB->addItem("U-Turn");
    connect(pCB,SIGNAL(currentIndexChanged(int)),this,SLOT(onChangeRoadType(int)));

    pLabel = new QLabel(pGroup);
    pLabel->setText("Radius:");
    pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;
    mpLBRoadType = pLabel;

    pLE = new QLineEdit(pGroup);
    pLE->setText("6.0");
    pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    mpLERoadType = pLE;

    mpCBRoadType->setCurrentIndex(0);
    mpLBRoadType->setVisible(true);
    mpLERoadType->setVisible(true);

    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 4;

    pPB = new QPushButton(pGroup);
    pPB->setText("Create Junction");
    pPB->setGeometry(nXPos,nYPos,nLEWidth*15/10,nLEHeight);
    nXPos = nXPos + nSpace*15/10;
    connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickCreateJunction()));

    pCB = new QComboBox(pGroup);
    pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;
    mpCBJunction = pCB;
    connect(pCB,SIGNAL(currentIndexChanged(int)),this,SLOT(onClickCBJunctionChange(int)));

    pCB = new QComboBox(pGroup);
    pCB->setGeometry(nXPos,nYPos,nLEWidth*5/10,nLEHeight);
    mpCBJunctionConnection = pCB;
    connect(pCB,SIGNAL(currentIndexChanged(int)),this,SLOT(onClickCBJunctionConnectionChange(int)));

    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 2;

    pLabel = new QLabel(pGroup);
    pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;
    mpLabelJunctionIncommingRoad = pLabel;

    pLabel = new QLabel(pGroup);
    pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;
    mpLabelJunctionContactPoint = pLabel;

    pLabel = new QLabel(pGroup);
    pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;
    mpLabelJunctionConnectingRoad = pLabel;

    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 2;

    pCB = new QComboBox(pGroup);
    pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    mpCBJunctionFromTo = pCB;

    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 2;

    pLabel = new QLabel(pGroup);
    pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;
    pLabel->setText("Incomming");

    pCB = new QComboBox(pGroup);
    pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;
    mpCBJunctionIncommingRoad = pCB;
    connect(pCB,SIGNAL(currentIndexChanged(int)),this,SLOT(onClickCBJunctionIncommingChange(int)));

    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 2;

    pLabel = new QLabel(pGroup);
    pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;
    pLabel->setText("Connecting");

    pCB = new QComboBox(pGroup);
    pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;
    mpCBJunctionConnectingRoad= pCB;
    connect(pCB,SIGNAL(currentIndexChanged(int)),this,SLOT(onClickCBJunctionConnectionroadChange(int)));

    pCB = new QComboBox(pGroup);
    pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;
    pCB->addItem("start");
    pCB->addItem("end");
    mpCBJunctionContactPoint = pCB;

    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 2;

    pCB = new QComboBox(pGroup);
    pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;
    mpCBJunctionFromLane = pCB;

    pCB = new QComboBox(pGroup);
    pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    nXPos = nXPos + nSpace;
    mpCBJunctionToLane = pCB;

    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 2;

    pPB = new QPushButton(pGroup);
    pPB->setText("Create Lane Link");
    pPB->setGeometry(nXPos,nYPos,nLEWidth*15/10,nLEHeight);
    nXPos = nXPos + nSpace*15/10;
    connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickCreateJunctionLaneLink()));

    pPB = new QPushButton(pGroup);
    pPB->setText("Delete Lane Link");
    pPB->setGeometry(nXPos,nYPos,nLEWidth*15/10,nLEHeight);
    nXPos = nXPos + nSpace*15/10;
    connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickDeleteJunctionLaneLink()));

    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 4;

    pPB = new QPushButton(pGroup);
    pPB->setText("Auto Connect Road");
    pPB->setGeometry(nXPos,nYPos,nLEWidth*15/10,nLEHeight);
    nXPos = nXPos + nSpace*15/10;
    connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickAutoConnect()));

    nXPos = 10;
    nYPos = nYPos + mnFontHeight * 4;


    pPB = new QPushButton(pGroup);
    pPB->setText("Save");
    pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickSave()));
    nXPos = nXPos + nSpace;

    pPB = new QPushButton(pGroup);
    pPB->setText("Load");
    pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
    connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickLoad()));
    nXPos = nXPos + nSpace;

    QScrollArea * pScroll = new QScrollArea();
    pScroll->setWidget(pGroup);

    p->addTab(pScroll,"Calculate");
}



void MainWindow::onClickXY(double x, double y)
{
    mClickX = x - mnMoveX;
    mClickY = y - mnMoveY;
    mbClick = true;
    mbRefresh = true;

    double selx,sely;
    double lon,lat;
    selx = mClickX;
    sely = mClickY * (-1);
    selx = selx/((double )mnfac);
    sely = sely/((double)mnfac);
    mpLE_SelX->setText(QString::number(selx,'f',3));
    mpLE_SelY->setText(QString::number(sely,'f',3));

    double x0,y0;
    GaussProjCal(glon0,glat0,&x0,&y0);
    GaussProjInvCal(x0+selx,y0+sely,&lon,&lat);

    mpLE_SelLon->setText(QString::number(lon,'f',7));
    mpLE_SelLat->setText(QString::number(lat,'f',7));
    update();
}

void MainWindow::onChangeScale(int scale)
{
    mnfac = scale;
    mpLE_Scale->setText(QString::number(scale));
    mbRefresh = true;
    update();
}

void MainWindow::onChangeMoveX(int scale)
{
    mnMoveX = VIEW_WIDTH * scale/100;
    mpLE_MoveX->setText(QString::number(scale));
    mbRefresh = true;
    update();
}

void MainWindow::onChangeMoveY(int scale)
{
    mnMoveY = VIEW_HEIGHT * scale/100;
    mpLE_MoveY->setText(QString::number(scale));
    mbRefresh = true;
    update();
}

void MainWindow::onChangeMark(int scale)
{
    mnMarkSize = scale;
    mpLE_Mark->setText(QString::number(scale));
    mbRefresh = true;
    update();
}

void MainWindow::onClickDefView()
{


    mnfac = mnDefmnfac;
    mnMoveX = mnDefMoveX;
    mnMoveY = mnDefMoveY;

    mpSlider_Scale->setValue(mnfac);
    mpSlider_MoveX->setValue(mnMoveX*100/VIEW_WIDTH);
    mpSlider_MoveY->setValue(mnMoveY*100/VIEW_HEIGHT);
//    mbRefresh = true;
    update();
}

void MainWindow::onClickZoomOne()
{
    myview->zoomone();
}

void MainWindow::onClickAsStart()
{



}

void MainWindow::onClickAsDst()
{
    double lon,lat;
    lon = mpLE_SelLon->text().toDouble();
    lat = mpLE_SelLat->text().toDouble();
    mpLE_DstLon->setText(QString::number(lon,'f',7));
    mpLE_DstLat->setText(QString::number(lat,'f',7));

    double x,y;
    x = mpLE_SelX->text().toDouble();
    y = mpLE_SelY->text().toDouble();

    mfObjX = x;
    mfObjY = y;

    mbSetObj = true;
    mbRefresh = true;
    update();
}

void MainWindow::onClickSetDst()
{

}

void MainWindow::onClickPlan()
{

}

void MainWindow::onClickSetStart()
{

}

/**
 * @brief ADCIntelligentVehicle::UpdateMap
 * @param strdata
 * @param nSize
 * @param index
 * @param dt
 * @param strmemname
 */
void MainWindow::UpdateMap(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{

    //    std::cout<<"update map "<<std::endl;
    int gpsunitsize = sizeof(iv::GPS_INS);
    int nMapSize = nSize/gpsunitsize;
    //    std::cout<<"map size is "<<nMapSize<<std::endl;

    if(nMapSize < 1)return;

    int i;
    mMutexNavi.lock();
    m_navigation_data.clear();
    for(i=0;i<nMapSize;i++)
    {
        iv::GPS_INS x;
        memcpy(&x,strdata + i*gpsunitsize,gpsunitsize);
        iv::GPSData data(new iv::GPS_INS);
        *data = x;
        m_navigation_data.push_back(data);
    }
    mMutexNavi.unlock();

    mbRefresh = true;
    update();

}

/**
 * @brief MainWindow::UpdateGPSIMU
 * @param strdata
 * @param nSize
 * @param index
 * @param dt
 * @param strmemname
 */

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

    iv::gps::gpsimu  xgpsimu;
    if(!xgpsimu.ParseFromArray(strdata,nSize))
    {
//        gIvlog->warn("ADCIntelligentVehicle::UpdateGPSIMU parse error. nSize is %d",nSize);
        return;
    }

    double flat,flon,fhead;
    flat = xgpsimu.lat();
    flon = xgpsimu.lon();
    fhead = xgpsimu.heading();

    double x,y;
    GaussProjCal(flon,flat,&x,&y);
    mfNowX = x - mx0;
    mfNowY = y- my0;
    mfNowHdg = (90- fhead)*M_PI/180.0;
    mbGPSUpdate = true;
    mbGPSViewUpdate = true;

}






void MainWindow::onClickLoadLane()
{
    QString str = QFileDialog::getOpenFileName(this,tr("Open Lane Info file"),"",tr("Lane File(*.txt)"));
    if(str.isEmpty())return;

    QFile xFile;
    xFile.setFileName(str);
    QFileInfo fi(str);
    QString filename = fi.fileName();
    double fLastLat = 39;
    double fLastLon = 117;
    if(filename.contains(".txt"))
    {
        filename = filename.left(filename.length() -4);
    }
    if(xFile.open(QIODevice::ReadWrite))
    {
        iv::lpunit lpu;
        strncpy(lpu.strlanename,filename.toLatin1().data(),filename.size());

        QByteArray ba;
        ba = xFile.read(xFile.size());
        QString strdata = ba;
        QStringList strlinedata=  strdata.split("\n",QString::SkipEmptyParts);
        int i;
        int nsize = strlinedata.size();
//        qDebug("line is %d",nsize);
        for(i=0;i<nsize;i++)
        {
            QString strline = strlinedata.at(i);
            QStringList strvaluedata = strline.split(" ",QString::SkipEmptyParts);
//            qDebug("%d value size is %d ",i,strvaluedata.size());
            iv::lanepoint lpleft,lpright;
            if(strvaluedata.size() == 12)
            {
                int j;
                QString strx[12];
                for(j=0;j<12;j++)strx[j] = strvaluedata.at(j);
                lpleft.mfLon = strx[0].toDouble();
                lpleft.mfLat = strx[1].toDouble();
                lpleft.mfHeight = strx[2].toDouble();
                lpleft.nLaneType = strx[3].toInt();
                lpleft.mfHeading = strx[4].toDouble();
                lpleft.mfDis = strx[5].toDouble();
                lpright.mfLon = strx[6].toDouble();
                lpright.mfLat = strx[7].toDouble();
                lpright.mfHeight = strx[8].toDouble();
                lpright.nLaneType = strx[9].toInt();
                lpright.mfHeading = strx[10].toDouble();
                lpright.mfDis = strx[11].toDouble();
                lpleft.mfHdg = (90-lpleft.mfHeading) * M_PI/180.0;
                if(lpleft.mfHdg < 0)lpleft.mfHdg  = lpleft.mfHdg  + 2.0*M_PI;
                lpright.mfHdg = (90-lpright.mfHeading) * M_PI/180.0;
                if(lpright.mfHdg < 0)lpright.mfHdg  = lpright.mfHdg  + 2.0*M_PI;

                double x,y;
                double xlast,ylast;
                GaussProjCal(lpleft.mfLon,lpleft.mfLat,&x,&y);
                GaussProjCal(fLastLon,fLastLat,&xlast,&ylast);
                if(sqrt(pow(xlast -x ,2)+pow(ylast -y,2))> 0.1)
                {
                    lpu.mvectorlpleft.push_back(lpleft);
                    lpu.mvectorlpright.push_back(lpright);
                    fLastLat = lpleft.mfLat;
                    fLastLon = lpleft.mfLon;
                }
                else
                {
//                    qDebug("no use this point");
                }
            }

        }

        if((mbSetOrigin == false)&&(lpu.mvectorlpleft.size() > 0))
        {
            glat0 = lpu.mvectorlpleft[0].mfLat;
            glon0 = lpu.mvectorlpright[0].mfLon;
            mpLELat0->setText(QString::number(glat0,'f',7));
            mpLELon0->setText(QString::number(glon0,'f',7));
            mbSetOrigin = true;
        }

        double x0,y0;
        GaussProjCal(glon0,glat0,&x0,&y0);
        int j;
        for(j=0;j<lpu.mvectorlpleft.size();j++)
        {
            double x,y;
            GaussProjCal(lpu.mvectorlpleft[j].mfLon,lpu.mvectorlpleft[j].mfLat,&x,&y);
            lpu.mvectorlpleft[j].mfX = x - x0;
            lpu.mvectorlpleft[j].mfY = y- y0;
            GaussProjCal(lpu.mvectorlpright[j].mfLon,lpu.mvectorlpright[j].mfLat,&x,&y);
            lpu.mvectorlpright[j].mfX = x - x0;
            lpu.mvectorlpright[j].mfY = y- y0;
        }

        if(lpu.mvectorlpleft.size() >  0)
        {
            mvectorlp.push_back(lpu);
            mpCBLane->addItem(filename);
            int N = lpu.mvectorlpleft.size();
            VectorXd x_veh(N);
            VectorXd y_veh(N);
            int j;
            int ntype = 0;
            int ntypecount = 1;

            for(j=0;j<N;j++)
            {
                double x,y;
                GaussProjCal(glon0,glat0,&x0,&y0);
                GaussProjCal(lpu.mvectorlpleft[j].mfLon,lpu.mvectorlpleft[j].mfLat,&x,&y);
                x_veh[j] = x - x0;
                y_veh[j] = y - y0;
                if(lpu.mvectorlpright[j].nLaneType != ntype)
                {
                    ntype = lpu.mvectorlpright[j].nLaneType;
                    ntypecount++;
                }
            }

//            CreateRoad(x_veh,y_veh);

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

//            double dismax = 0;
//            for(j=0;j<N;j++)
//            {
//                double A = coeffs[1];
//                double B = -1;
//                double C = coeffs[0];
//                double dis = fabs(A*x_veh[j] + B*y_veh[j] +C )/sqrt(pow(A,2)+pow(B,2));
//                if(dis>dismax)dismax = dis;
//            }
//            int nxxx = coeffs.size();

        }
    }

    xFile.close();
    mbRefresh = true;
    update();
}

/**
 * @brief MainWindow::CreateRoad
 * Create Road Geometry
 * @param xvals
 * @param yvals
 * @return
 */
Road MainWindow::CreateRoad(VectorXd xvals, VectorXd yvals)
{
//    Road xRoad;
//    double LINE_ERROR = 0.15;
//    int nsize =  xvals.size();
//    int nnotfit = nsize;
//    int ncurpos = 0;
//    int nrange = nsize;
//    while(ncurpos  <  nsize)
//    {
// //       int N = nrange - ncurpos;
//        VectorXd x_veh(nrange);
//        VectorXd y_veh(nrange);

//        int j;
//        for(j=ncurpos;j<(ncurpos +nrange);j++)
//        {
//            x_veh[j-ncurpos] = xvals[j];
//            y_veh[j - ncurpos] = yvals[j];
//        }

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

//        double dismax = 0;
//        for(j=ncurpos;j<(ncurpos +nrange);j++)
//        {
//            double A = coeffs[1];
//            double B = -1;
//            double C = coeffs[0];
//            double dis = fabs(A*x_veh[j-ncurpos] + B*y_veh[j-ncurpos] +C )/sqrt(pow(A,2)+pow(B,2));
//            if(dis>dismax)dismax = dis;
//        }
//        std::cout<<"dis is "<<dismax<<std::endl;
//        if((dismax > LINE_ERROR)&&((nsize -(nrange+ncurpos))>50))
//        {
//            nrange = nrange/2;
//        }
//        else
//        {
//            std::cout<<"nrange is "<<nrange<<std::endl;

//            xRoad.AddGeometryBlock();
//            GeometryBlock * pgb = xRoad.GetGeometryBlock(xRoad.GetGeometryBlockCount()-1);
// //           pgb->AddGeometryLine(1.0,x,y,len);

//            ncurpos = ncurpos + nrange;
//            nrange = nsize - ncurpos;


//            std::cout<<"add a geo."<<std::endl;
//        }
//    }

//    return xRoad;
}

void MainWindow::onClickMarkLane()
{
    mnMarkLane = mpCBLane->currentIndex();
    mbRefresh = true;
    update();
}

void MainWindow::onClickRemoveLane()
{
    int i = mpCBLane->currentIndex();
    if((i>=0) && (i<mvectorlp.size()))
    {
        int j;
        for(j=0;j<mvectorroadlane.size();j++)
        {
            if(mvectorroadlane[j] == i)
            {
                mvectorroadlane.erase(mvectorroadlane.begin() + j);
                mpCBSelLane->removeItem(j);
                break;
            }
        }
        mvectorlp.erase(mvectorlp.begin() + i);
        mpCBLane->removeItem(i);
        mbRefresh = true;
        update();
    }
}

void MainWindow::onClickToRoad()
{
    int i = mpCBLane->currentIndex();
    if((i>=0) && (i<mvectorlp.size()))
    {
        int j;
        bool bHave = false;
        for(j=0;j<mvectorroadlane.size();j++)
        {
            if(mvectorroadlane[j] == i)
            {
                bHave = true;
                break;
            }
        }
        if(bHave == false)
        {
            mvectorroadlane.push_back(i);
            mpCBSelLane->addItem(mvectorlp.at(i).strlanename);
        }
    }
}

void MainWindow::onClickToOpposite()
{
    int i = mpCBLane->currentIndex();
    if((i>=0) && (i<mvectorlp.size()))
    {
        int j;
        bool bHave = false;
        for(j=0;j<mvectorroadopposite.size();j++)
        {
            if(mvectorroadopposite[j] == i)
            {
                bHave = true;
                break;
            }
        }
        if(bHave == false)
        {
            mvectorroadopposite.push_back(i);
            mpCBSelOpLane->addItem(mvectorlp.at(i).strlanename);
        }
    }
}

void MainWindow::onClickClearRoadLane()
{
    mvectorroadlane.clear();
    mvectorroadopposite.clear();
    mpCBSelLane->clear();
    mpCBSelOpLane->clear();
}

void MainWindow::onClickAddRoad()
{
    if(mvectorroadlane.size() < 1)return;

    int nlanesize = mvectorroadlane.size();
    int noplanesize = mvectorroadopposite.size();
    double notlinethresh = 1.0;  //When heading change more than this value, is a arc.

    int nsize = mvectorlp[mvectorroadlane[0]].mvectorlpleft.size();
    int * pntype = new int[nsize];
    std::shared_ptr<int> ppntype; ppntype.reset(pntype);

    std::vector<iv::lanepoint > * pvectorlp = &(mvectorlp[mvectorroadlane[0]].mvectorlpleft);
    int i;

    //Go  throuh point  for fit type.
    for(i=0;i<5;i++)pntype[i] = 0;
    for(i=5;i<(nsize-5);i++)
    {
        double s = pvectorlp->at(i).mfDis;
        double head0 = pvectorlp->at(i).mfHeading;
        int j;
        double xcount = 0;
        double headdifftotal = 0;
        double headdiffavg = 0.0;
        for(j=1;j<nsize;j++)
        {
            if((j>1)&&((pvectorlp->at(j).mfDis - s)>1.0))
            {
                break;
            }
            double headdiff = pvectorlp->at(j).mfHeading - head0;
            if(headdiff > 300)headdiff = headdiff - 360;
            if(headdiff < -300)headdiff = headdiff + 360;
            headdifftotal = headdifftotal + headdiff;
            xcount = xcount + 1.0;
        }
        if(xcount > 0)headdiffavg = headdifftotal/xcount;
        if(fabs(headdiffavg) > (notlinethresh*2))
        {
            pntype[i] = 1;
        }
        else
        {
            pntype[i] = 0;
        }

    }
    for(i=(nsize -5);i<nsize;i++)
    {
        pntype[i] = 0;
    }

    for(i=0;i<nsize;i++)  //Remove Few Not Line Point.
    {
        if(pntype[i] !=  0)
        {
            int j;
            int ncount = 1;
            for(j=(i+1);j<nsize;j++)
            {
                if(pntype[i] != 0)ncount++;
                else break;
            }
            if(ncount < 10)pntype[i] = 0;
            else
            {
                i = i+ ncount;
            }
        }
    }
    for(i=5;i<nsize;i++)
    {
        if(pntype[i] != pntype[i-1])
        {
            int j = i-1;
            double disx = pvectorlp->at(i).mfDis;

            for(j=(i-2);j>5;j--)
            {
                if(pntype[j] == pntype[i-1])
                {
                    pntype[j] = 2; //Besel
                }
                if(fabs(pvectorlp->at(j).mfDis - disx)>1)  //1 m besel
                {
                    break;
                }
            }
            pntype[i-1] = 2;
        }
    }


    //Calc dis  to ref  line
    double xor0,yor0;
    GaussProjCal(glon0,glat0,&xor0,&yor0);
    std::vector<std::vector<iv::distogeo>> xvectordiss;
    std::vector<std::vector<iv::distogeo>> xvectoropdiss;
    for(i=0;i<mvectorroadlane.size();i++)
    {
        std::vector<iv::distogeo> xvectordistogeo;
        int j;
        int nsizepoint = mvectorlp[mvectorroadlane[i]].mvectorlpright.size();

        bool bHaveLast = false;
        int nLast = 0;
        for(j=0;j<nsizepoint;j++)
        {
            double fdismin = 1000;
            double fS = 0;
            int k;
            double x,y;
            double x0,y0;
            iv::distogeo xdistogeo;

            int nLastBig = 0;
            bool bFindDisMin = false;

            x = mvectorlp[mvectorroadlane[i]].mvectorlpright[j].mfX;
            y = mvectorlp[mvectorroadlane[i]].mvectorlpright[j].mfY;
            int kstart = 0;
            if(bHaveLast)
            {
                kstart= nLast;
            }
            for(k=kstart;k<mvectorlp[mvectorroadlane[0]].mvectorlpleft.size();k++)
            {

                x0 = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfX;
                y0 = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfY;
                double fdis = sqrt(pow(x-x0,2)+pow(y-y0,2));
                if(fdismin >  fdis)
                {
                    fdismin = fdis;
                    fS = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfDis;
                    bFindDisMin = true;
                    nLast = k;

                }
                if((bFindDisMin)&&(bHaveLast))
                {
                    if(fdis>fdismin)
                    {
                        nLastBig++;
                    }
                    else
                    {
                        nLastBig = 0;
                    }
                }
                if(nLastBig > 10)break;


            }
//            std::cout<<" k is "<<k<<std::endl;
            if(bHaveLast == false)
            {
                if(fdismin<10)bHaveLast = true;
            }
            else
            {
                if(fdismin > 10)bHaveLast = false;
            }

            double fHdg = geofit::CalcHdg(x,y,mvectorlp[mvectorroadlane[0]].mvectorlpleft[nLast].mfX,
                    mvectorlp[mvectorroadlane[0]].mvectorlpleft[nLast].mfY);
            double fHdgDiff = fHdg - mvectorlp[mvectorroadlane[i]].mvectorlpright[j].mfHdg;
            if(fHdgDiff < 0)fHdgDiff = fHdgDiff + 2.0*M_PI;
            if(fHdgDiff >= 2.0*M_PI)fHdgDiff = fHdgDiff - 2.0*M_PI;
            if(((fHdgDiff>(M_PI/2.0-0.3))&&(fHdgDiff<(M_PI/2.0+0.3)))||(fdismin < 3.2))
            {
                xdistogeo.mfdis = fdismin;
                xdistogeo.mfs = fS;

                mvectorlp[mvectorroadlane[i]].mvectorlpright[j].mfRelS = fS;
            }
            else
            {
                xdistogeo.mfs = -1;
                std::cout<<"point extend."<<std::endl;
                mvectorlp[mvectorroadlane[i]].mvectorlpright[j].mfRelS =  -1;
            }
            xvectordistogeo.push_back(xdistogeo);
        }

        xvectordiss.push_back(xvectordistogeo);
    }

    for(i=1;i<mvectorroadlane.size();i++)
    {
        std::vector<iv::distogeo> xvectordistogeo;
        int j;
        int nsizepoint = mvectorlp[mvectorroadlane[i]].mvectorlpleft.size();
        bool bHaveLast = false;
        int nLast = 0;
        for(j=0;j<nsizepoint;j++)
        {
            double fdismin = 1000;
            double fS = 0;
            int k;
            double x,y;
            double x0,y0;
            iv::distogeo xdistogeo;
            int nLastBig = 0;
            bool bFindDisMin = false;
            x = mvectorlp[mvectorroadlane[i]].mvectorlpleft[j].mfX;
            y = mvectorlp[mvectorroadlane[i]].mvectorlpleft[j].mfY;

            int kstart = 0;
            if(bHaveLast)
            {
                kstart= nLast;
            }
            for(k=kstart;k<mvectorlp[mvectorroadlane[0]].mvectorlpleft.size();k++)
            {
                x0 = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfX;
                y0 = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfY;
                double fdis = sqrt(pow(x-x0,2)+pow(y-y0,2));
                if(fdismin >  fdis)
                {
                    fdismin = fdis;
                    fS = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfDis;
                    bFindDisMin = true;
                    nLast = k;

                }
                if((bFindDisMin)&&(bHaveLast))
                {
                    if(fdis>fdismin)
                    {
                        nLastBig++;
                    }
                    else
                    {
                        nLastBig = 0;
                    }
                }
                if(nLastBig > 10)break;
            }
 //           std::cout<<" k is "<<k<<std::endl;
            if(bHaveLast == false)
            {
                if(fdismin<10)bHaveLast = true;
            }
            else
            {
                if(fdismin > 10)bHaveLast = false;
            }

            double fHdg = geofit::CalcHdg(x,y,mvectorlp[mvectorroadlane[0]].mvectorlpleft[nLast].mfX,
                    mvectorlp[mvectorroadlane[0]].mvectorlpleft[nLast].mfY);
            double fHdgDiff = fHdg - mvectorlp[mvectorroadlane[i]].mvectorlpleft[j].mfHdg;
            if(fHdgDiff < 0)fHdgDiff = fHdgDiff + 2.0*M_PI;
            if(fHdgDiff >= 2.0*M_PI)fHdgDiff = fHdgDiff - 2.0*M_PI;
            if(((fHdgDiff>(M_PI/2.0-0.3))&&(fHdgDiff<(M_PI/2.0+0.3)))||(fdismin < 3.2))
            {
                xdistogeo.mfdis = fdismin;
                xdistogeo.mfs = fS;

                mvectorlp[mvectorroadlane[i]].mvectorlpleft[j].mfRelS = fS;
            }
            else
            {
                xdistogeo.mfs = -1;
                std::cout<<"point extend."<<std::endl;
                mvectorlp[mvectorroadlane[i]].mvectorlpleft[j].mfRelS = -1;
            }
            xvectordistogeo.push_back(xdistogeo);

        }

        xvectordiss.push_back(xvectordistogeo);
    }


    for(i=0;i<mvectorroadopposite.size();i++)
    {
        std::vector<iv::distogeo> xvectordistogeo;
        int j;
        int nsizepoint = mvectorlp[mvectorroadopposite[i]].mvectorlpleft.size();
        bool bHaveLast = false;
        int nLast = 0;
        for(j=0;j<nsizepoint;j++)
        {
            double fdismin = 1000;
            double fS = 0;
            int k;
            double x,y;
            double x0,y0;
            iv::distogeo xdistogeo;
            int nLastBig = 0;
            bool bFindDisMin = false;
            x = mvectorlp[mvectorroadopposite[i]].mvectorlpleft[j].mfX;
            y = mvectorlp[mvectorroadopposite[i]].mvectorlpleft[j].mfY;

            int nrefsize = mvectorlp[mvectorroadlane[0]].mvectorlpleft.size();
            int kstart = nrefsize-1;
            if(bHaveLast)
            {
                kstart= nLast;
            }

            for(k=kstart;k>=0;k--)
            {
                x0 = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfX;
                y0 = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfY;
                double fdis = sqrt(pow(x-x0,2)+pow(y-y0,2));
                if(fdismin >  fdis)
                {
                    fdismin = fdis;
                    fS = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfDis;
                    bFindDisMin = true;
                    nLast = k;

                }
                if((bFindDisMin)&&(bHaveLast))
                {
                    if(fdis>fdismin)
                    {
                        nLastBig++;
                    }
                    else
                    {
                        nLastBig = 0;
                    }
                }
                if(nLastBig > 10)break;
            }
 //           std::cout<<" k is "<<k<<std::endl;
            if(bHaveLast == false)
            {
                if(fdismin<10)bHaveLast = true;
            }
            else
            {
                if(fdismin > 10)bHaveLast = false;
            }

            double fHdg = geofit::CalcHdg(x,y,mvectorlp[mvectorroadlane[0]].mvectorlpleft[nLast].mfX,
                    mvectorlp[mvectorroadlane[0]].mvectorlpleft[nLast].mfY);
            double fHdgDiff = fHdg - mvectorlp[mvectorroadopposite[i]].mvectorlpleft[j].mfHdg;
            if(fHdgDiff < 0)fHdgDiff = fHdgDiff + 2.0*M_PI;
            if(fHdgDiff >= 2.0*M_PI)fHdgDiff = fHdgDiff - 2.0*M_PI;
            if(((fHdgDiff>(M_PI/2.0-0.3))&&(fHdgDiff<(M_PI/2.0+0.3)))||(fdismin < 0.5))
            {
                xdistogeo.mfdis = fdismin;
                xdistogeo.mfs = fS;

                mvectorlp[mvectorroadopposite[i]].mvectorlpleft[j].mfRelS = fS;
            }
            else
            {
                xdistogeo.mfs = -1;
                std::cout<<"point extend."<<std::endl;
                mvectorlp[mvectorroadopposite[i]].mvectorlpleft[j].mfRelS = -1;
            }

            xvectordistogeo.push_back(xdistogeo);

        }

        xvectoropdiss.push_back(xvectordistogeo);
    }

    for(i=0;i<mvectorroadopposite.size();i++)
    {
        std::vector<iv::distogeo> xvectordistogeo;
        int j;
        int nsizepoint = mvectorlp[mvectorroadopposite[i]].mvectorlpright.size();
        bool bHaveLast = false;
        int nLast = 0;
        for(j=0;j<nsizepoint;j++)
        {
            double fdismin = 1000;
            double fS = 0;
            int k;
            double x,y;
            double x0,y0;
            iv::distogeo xdistogeo;
            int nLastBig = 0;
            bool bFindDisMin = false;
            x = mvectorlp[mvectorroadopposite[i]].mvectorlpright[j].mfX;
            y = mvectorlp[mvectorroadopposite[i]].mvectorlpright[j].mfY;

            int nrefsize = mvectorlp[mvectorroadlane[0]].mvectorlpleft.size();
            int kstart = nrefsize-1;
            if(bHaveLast)
            {
                kstart= nLast;
            }

            for(k=kstart;k>=0;k--)
            {
                x0 = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfX;
                y0 = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfY;
                double fdis = sqrt(pow(x-x0,2)+pow(y-y0,2));
                if(fdismin >  fdis)
                {
                    fdismin = fdis;
                    fS = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfDis;
                    bFindDisMin = true;
                    nLast = k;

                }
                if((bFindDisMin)&&(bHaveLast))
                {
                    if(fdis>fdismin)
                    {
                        nLastBig++;
                    }
                    else
                    {
                        nLastBig = 0;
                    }
                }
                if(nLastBig > 10)break;
            }
 //           std::cout<<" k is "<<k<<std::endl;
            if(bHaveLast == false)
            {
                if(fdismin<10)bHaveLast = true;
            }
            else
            {
                if(fdismin > 10)bHaveLast = false;
            }

            double fHdg = geofit::CalcHdg(x,y,mvectorlp[mvectorroadlane[0]].mvectorlpleft[nLast].mfX,
                    mvectorlp[mvectorroadlane[0]].mvectorlpleft[nLast].mfY);
            double fHdgDiff = fHdg - mvectorlp[mvectorroadopposite[i]].mvectorlpright[j].mfHdg;
            if(fHdgDiff < 0)fHdgDiff = fHdgDiff + 2.0*M_PI;
            if(fHdgDiff >= 2.0*M_PI)fHdgDiff = fHdgDiff - 2.0*M_PI;
            if(((fHdgDiff>(M_PI/2.0-0.3))&&(fHdgDiff<(M_PI/2.0+0.3)))||(fdismin < 0.5))
            {
                xdistogeo.mfdis = fdismin;
                xdistogeo.mfs = fS;

                mvectorlp[mvectorroadopposite[i]].mvectorlpright[j].mfRelS = fS;
            }
            else
            {
                xdistogeo.mfs = -1;
                std::cout<<"point extend."<<std::endl;
                mvectorlp[mvectorroadopposite[i]].mvectorlpright[j].mfRelS = -1;
            }
            xvectordistogeo.push_back(xdistogeo);

        }

        xvectoropdiss.push_back(xvectordistogeo);
    }

    //Get Lane Type
    std::vector<std::vector<iv::lanetype>> xvevelanetype;
    std::vector<std::vector<iv::lanetype>> xvectoroplanetype;
    for(i=0;i<nlanesize;i++)
    {
        std::vector<iv::lanetype> xvelanetype;
        int j;
        int nlasttype;
        iv::lanetype xlt;
        xlt.s = 0;
        xlt.ntype = 0;
        int nsizelp = mvectorlp[mvectorroadlane[i]].mvectorlpright.size();
        if(nsizelp > 0)
        {
            xlt.ntype = mvectorlp[mvectorroadlane[i]].mvectorlpright[0].nLaneType;
            nlasttype = xlt.ntype;
        }
        for(j=1;j<nsizelp;j++)
        {
            int nlt = mvectorlp[mvectorroadlane[i]].mvectorlpright[j].nLaneType;
            if((nlt != nlasttype)&&(mvectorlp[mvectorroadlane[i]].mvectorlpright[j].mfRelS != -1))
            {
                xvelanetype.push_back(xlt);
                xlt.ntype = nlt;
                xlt.s = mvectorlp[mvectorroadlane[i]].mvectorlpright[j].mfRelS;
            }
            nlasttype = nlt;
        }
        xvelanetype.push_back(xlt);
        xvevelanetype.push_back(xvelanetype);
    }

    for(i=0;i<noplanesize;i++)
    {
        std::vector<iv::lanetype> xvelanetype;
        int j;
        int nlasttype;
        iv::lanetype xlt;
        xlt.s = 0;
        xlt.ntype = 0;
        int nsizelp = mvectorlp[mvectorroadopposite[i]].mvectorlpright.size();
        if(nsizelp > 0)
        {
            xlt.ntype = mvectorlp[mvectorroadopposite[i]].mvectorlpright[nsizelp-1].nLaneType;
            nlasttype = xlt.ntype;
        }
        for(j=(nsizelp-2);j>=0;j--)
        {
            int nlt = mvectorlp[mvectorroadopposite[i]].mvectorlpright[j].nLaneType;
            if((nlt != nlasttype)&&(mvectorlp[mvectorroadopposite[i]].mvectorlpright[j].mfRelS != -1))
            {
                xvelanetype.push_back(xlt);
                xlt.ntype = nlt;
                xlt.s = mvectorlp[mvectorroadopposite[i]].mvectorlpright[j].mfRelS;
            }
            nlasttype = nlt;
        }
        xvelanetype.push_back(xlt);
        xvectoroplanetype.push_back(xvelanetype);
    }

    //Get Lane Mark Width
    std::vector<double> xvectormarkwidth;
    std::vector<double> xvectoravgdis;
    for(i=0;i<xvectordiss.size();i++)
    {
        double fdis = 0;
        int j;
        int ncount = 0;
        for(j=0;j<xvectordiss[i].size();j++)
        {
            if(xvectordiss[i].at(j).mfs !=  -1)
            {
                fdis = fdis + xvectordiss[i].at(j).mfdis;
                ncount++;
            }
        }
        if(ncount>0)fdis = fdis/ncount;
        xvectoravgdis.push_back(fdis);
    }

    for(i=1;i<mvectorroadlane.size();i++)
    {
        xvectormarkwidth.push_back(xvectoravgdis[mvectorroadlane.size()+i-1] - xvectoravgdis[i-1]);
    }

    std::vector<double> xvectoropmarkwidth;
    std::vector<double> xvectoropavgdis;

    for(i=0;i<xvectoropdiss.size();i++)
    {
        double fdis = 0;
        int j;
        int ncount = 0;
        int nsize = xvectoropdiss[i].size();
        for(j=0;j<nsize;j++)
        {
            if(xvectoropdiss[i].at(j).mfs !=  -1)
            {
                fdis = fdis + xvectoropdiss[i].at(j).mfdis;
                ncount++;
            }
        }
        if(ncount>0)fdis = fdis/ncount;
        xvectoropavgdis.push_back(fdis);
    }

    if(xvectoropavgdis.size()>0)xvectoropmarkwidth.push_back(xvectoropavgdis[0]);
    for(i=1;i<mvectorroadopposite.size();i++)
    {
        xvectoropmarkwidth.push_back(xvectoropavgdis[i] - xvectoropavgdis[i+mvectorroadopposite.size()-1]);
    }

    //Get Lane Width
    std::vector<std::vector<iv::distogeo>> xvectorlanewidth;
    std::vector<std::vector<iv::distogeo>> xvectoroplanewidth;
    std::vector<iv::distogeo> xlanewidth;
    xlanewidth.clear();
    int j;
    for(j=0;j<xvectordiss[0].size();j++)
    {
        if(xvectordiss[0].at(j).mfs !=  -1)xlanewidth.push_back(xvectordiss[0].at(j));
    }
    xvectorlanewidth.push_back(xlanewidth);
    for(i=1;i<mvectorroadlane.size();i++)
    {
        xlanewidth.clear();
        for(j=0;j<xvectordiss[i].size();j++)
        {
            if((xvectordiss[i].at(j).mfs!=-1)&&(xvectordiss[i-1+nlanesize].at(j).mfs!=-1))
            {
                iv::distogeo xdisg;
                xdisg = xvectordiss[i].at(j);
                xdisg.mfdis = xdisg.mfdis - xvectordiss[i-1+nlanesize].at(j).mfdis + xvectormarkwidth[i-1];
                xlanewidth.push_back(xdisg);
            }
        }
        xvectorlanewidth.push_back(xlanewidth);
    }
    xlanewidth.clear();
    if(xvectoropmarkwidth.size()>0)
    {
        if(xvectoropmarkwidth[0] >  0.4)
        {
            int nsize = xvectoropdiss[0].size();
            for(j=0;j<nsize;j++)
            {
                if(xvectoropdiss[0].at(j).mfs != -1)xlanewidth.push_back(xvectoropdiss[0].at(j));
            }
            xvectoroplanewidth.push_back(xlanewidth);
            xlanewidth.clear();
            nsize = xvectoropdiss[noplanesize].size();
            for(j=0;j<nsize;j++)
            {
                if((xvectoropdiss[noplanesize].at(j).mfs!=-1)&&(xvectoropdiss[0].at(j).mfs!=-1))
                {
                    iv::distogeo xdisg;
                    xdisg = xvectoropdiss[noplanesize].at(j);
                    xdisg.mfdis = xdisg.mfdis - xvectoropdiss[0].at(j).mfdis ;
                    xlanewidth.push_back(xdisg);
                }

            }
            xvectoroplanewidth.push_back(xlanewidth);
        }
        else
        {
            nsize = xvectoropdiss[noplanesize].size();
            for(j=0;j<nsize;j++)
            {
                if((xvectoropdiss[noplanesize].at(j).mfs!=-1)&&(xvectoropdiss[0].at(j).mfs!=-1))
                {
                    iv::distogeo xdisg;
                    xdisg = xvectoropdiss[noplanesize].at(j);
                    xdisg.mfdis = xdisg.mfdis - xvectoropdiss[0].at(j).mfdis + xvectoropmarkwidth[0];
                    xlanewidth.push_back(xdisg);
                }

            }
            xvectoroplanewidth.push_back(xlanewidth);
        }
        for(i=1;i<noplanesize;i++)
        {
            xlanewidth.clear();
            nsize = xvectoropdiss[noplanesize+i].size();
            for(j=0;j<nsize;j++)
            {
                if((xvectoropdiss[noplanesize+i].at(j).mfs != -1)&&(xvectoropdiss[i].at(j).mfs != -1))
                {
                    iv::distogeo xdisg;
                    xdisg = xvectoropdiss[noplanesize+i].at(j);
                    xdisg.mfdis = xdisg.mfdis - xvectoropdiss[i].at(j).mfdis + xvectoropmarkwidth[i];
                    xlanewidth.push_back(xdisg);
                }
            }
            xvectoroplanewidth.push_back(xlanewidth);

        }

    }

    std::vector<iv::lanecoff> xvectorlanecoff;
    std::vector<iv::lanecoff> xvectoroplanecoff;
    for(i=0;i<nlanesize;i++)
    {
        int N = xvectorlanewidth[i].size();
        VectorXd x_veh(N);
        VectorXd y_veh(N);
        for(j=0;j<N;j++)
        {
            x_veh[j] = xvectorlanewidth[i].at(j).mfs;
            y_veh[j] =xvectorlanewidth[i].at(j).mfdis;
        }
        VectorXd coeffs = polyfit(x_veh, y_veh, 3);
        iv::lanecoff xlanecoff;
        xlanecoff.A = coeffs[0];
        xlanecoff.B = coeffs[1];
        xlanecoff.C = coeffs[2];
        xlanecoff.D = coeffs[3];
        xvectorlanecoff.push_back(xlanecoff);
    }

    for(i=0;i<xvectoroplanewidth.size();i++)
    {
        int N = xvectoroplanewidth[i].size();
        VectorXd x_veh(N);
        VectorXd y_veh(N);
        for(j=0;j<N;j++)
        {
            x_veh[j] = xvectoroplanewidth[i].at(j).mfs;
            y_veh[j] =xvectoroplanewidth[i].at(j).mfdis;
        }
        VectorXd coeffs = polyfit(x_veh, y_veh, 3);
        iv::lanecoff xlanecoff;
        xlanecoff.A = coeffs[0];
        xlanecoff.B = coeffs[1];
        xlanecoff.C = coeffs[2];
        xlanecoff.D = coeffs[3];
        xvectoroplanecoff.push_back(xlanecoff);
    }

    iv::lanecoff xlaneheightcoff;
    int M = mvectorlp[0].mvectorlpleft.size();
    VectorXd x_vehhg(M);
    VectorXd y_vehhg(M);
    for(j=0;j<M;j++)
    {
        x_vehhg[j] = mvectorlp[0].mvectorlpleft.at(j).mfDis;
        y_vehhg[j] =mvectorlp[0].mvectorlpleft.at(j).mfHeight;
    }
    VectorXd coeffs = polyfit(x_vehhg, y_vehhg, 3);
    xlaneheightcoff.A = coeffs[0];
    xlaneheightcoff.B = coeffs[1];
    xlaneheightcoff.C = coeffs[2];
    xlaneheightcoff.D = coeffs[3];



    geofit xgeofit;

    int N = pvectorlp->size();
    VectorXd x_veh(N);
    VectorXd y_veh(N);
    VectorXi t_veh(N);


    double x0,y0;
    GaussProjCal(glon0,glat0,&x0,&y0);

    for(j=0;j<N;j++)
    {
        double x,y;

        GaussProjCal(pvectorlp->at(j).mfLon,pvectorlp->at(j).mfLat,&x,&y);
        x_veh[j] = x - x0;
        y_veh[j] = y - y0;
        t_veh[j] = pntype[j];
    }

    std::vector<geobase> xvectorgeo = xgeofit.getgeo(x_veh,y_veh,t_veh);

//    OpenDrive od;
//    std::string mapx = "map";
//    od.SetHeader(1,1,mapx,1.1,QDateTime::currentDateTime().toString("yyyy-MM-dd").toLatin1().data(),0,0,0,0,glat0,glon0,ghdg0);

    j= 0;
    double xroadlen = 0;
    for(j=0;j<xvectorgeo.size();j++)
    {
        xroadlen = xroadlen + xvectorgeo[j].mfLen;
    }

        mxodr.AddRoad(mpLE_RoadName->text().toLatin1().data(),xroadlen, QString::number(CreateRoadID()).toStdString(),"-1");
        Road * p = mxodr.GetRoad(mxodr.GetRoadCount() - 1);

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

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

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

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

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

        p->AddLaneSection(0);
        LaneSection * pLS = p->GetLaneSection(0);
        Lane * pLL;
        pLS->SetS(0);
        pLS->AddLane(0,0,"none",false);
        pLL = pLS->GetLane(0);
        if(noplanesize != xvectoroplanewidth.size())
        {
            pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");
        }
    for(i=0;i<nlanesize;i++)
    {
        pLS->AddLane(-1,(i+1)*(-1),"driving",false,false);
        pLL = pLS->GetLane(pLS->GetLaneCount() - 1);

        pLL->AddWidthRecord(0,xvectorlanecoff[i].A,xvectorlanecoff[i].B,
                            xvectorlanecoff[i].C,xvectorlanecoff[i].D);



        int j;
        for(j=0;j<xvevelanetype[i].size();j++)
        {
            std::string strlanetype = "broken";
            if(xvevelanetype[i].at(j).ntype != 0)
            {
                strlanetype = "solid";
            }
            pLL->AddRoadMarkRecord(xvevelanetype[i].at(j).s,strlanetype,"standard","standard",0.15,"false");
        }

    }

    for(i=0;i<xvectoroplanewidth.size();i++)
    {
        pLS->AddLane(1,(i+1)*(1),"driving",false,false);
        pLL = pLS->GetLane(pLS->GetLaneCount() - 1);
        if((i==0)&&(noplanesize != xvectoroplanewidth.size()))
        {
            pLL->SetType("shoulder");
            pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");
        }

        pLL->AddWidthRecord(0,xvectoroplanecoff[i].A,xvectoroplanecoff[i].B,
                            xvectoroplanecoff[i].C,xvectoroplanecoff[i].D);

        int j;
        int index = i;
        if((i==0)&&(noplanesize != xvectoroplanewidth.size()))
        {
            continue;
        }
        if(noplanesize != xvectoroplanewidth.size())
        {
            index = i-1;
        }
        else
            index = i;
        for(j=0;j<xvectoroplanetype[index].size();j++)
        {
            std::string strlanetype = "broken";
            if(xvectoroplanetype[index].at(j).ntype != 0)
            {
                strlanetype = "solid";
            }
            pLL->AddRoadMarkRecord(xvectoroplanetype[index].at(j).s,strlanetype,"standard","standard",0.15,"false");
        }

    }

    mpCBSelLane->clear();
    mpCBSelOpLane->clear();
    mvectorroadlane.clear();
    mvectorroadopposite.clear();
    updateCBRoad();
    mbRefresh = true;
    update();



//    OpenDriveXmlWriter x(&mxodr);
 //   x.WriteFile("/home/nvidia/text.xodr");


}


int MainWindow::CreateRoadID(int ntype)
{
    int i;
    bool bUsed = false;
    int nroadidstart  = 10000;
    if(ntype == 1)  //not create by lane  roaid
    {
        nroadidstart = 20000;
    }

    int nroadcount = mxodr.GetRoadCount();
    if(nroadcount == 0)return nroadidstart;
    int * proadid = new int[nroadcount];
    for(i=0;i<nroadcount;i++)
    {
        proadid[i]=atoi(mxodr.GetRoad(i)->GetRoadId().data());
    }
    do
    {
        bUsed = false;
    for(i=0;i<nroadcount;i++)
    {
        if(proadid[i] == nroadidstart)
        {
            nroadidstart++;
            bUsed = true;
        }
    }
    }while(bUsed);
    delete proadid;
    return nroadidstart;

}

int MainWindow::CreateJunctionID()
{
    int i;
    bool bUsed = false;
    int njunctionidstart  = 800000;

    int njunctioncount = mxodr.GetJunctionCount();
    if(njunctioncount == 0)return njunctionidstart;
    int * pjunctionid = new int[njunctioncount];
    for(i=0;i<njunctioncount;i++)
    {
        pjunctionid[i]=atoi(mxodr.GetJunction(i)->GetId().data());
    }
    do
    {
        bUsed = false;
    for(i=0;i<njunctioncount;i++)
    {
        if(pjunctionid[i] == njunctionidstart)
        {
            njunctionidstart++;
            bUsed = true;
        }
    }
    }while(bUsed);
    delete pjunctionid;
    return njunctionidstart;

}

void MainWindow::onClickLoad()
{
    QString str = QFileDialog::getOpenFileName(this,"Load XODR",".","*.xodr");
    if(str.isEmpty())return;

    OpenDrive * pxodr = new OpenDrive();  //because add to xodr,so don't delete
    OpenDriveXmlParser x(pxodr);
    if(!x.ReadFile(str.toStdString()))
    {
        QMessageBox::warning(this,"warn","Can't  load xodr file.");
        return;
    }



    bool bNeedMove = false;
    bool bNeedAjustID = false;
    unsigned short int revMajor,revMinor;
    std::string name,date;
    float version;
    double north,south,east,west,lat0,lon0,hdg0;
    if(pxodr->GetHeader() != 0)
    {
        pxodr->GetHeader()->GetAllParams(revMajor,revMinor,name,version,date,north,south,east,west,lat0,lon0,hdg0);
    }

    double xMoveX = 0;
    double xMoveY = 0;

    if(mxodr.GetRoadCount() > 0)
    {
        bNeedAjustID = true;
    }

    if(mbSetOrigin == false)
    {
        bNeedMove = false;
        glat0 = lat0;
        glon0 = lon0;
        mpLELon0->setText(QString::number(glon0,'f',7));
        mpLELat0->setText(QString::number(glat0,'f',7));
        mbSetOrigin = true;
    }
    else
    {
        if((glat0 == lat0)&&(glon0 == lon0))
        {
            bNeedMove = false;
        }
        else
        {
            bNeedMove = true;
            double x0,y0,x1,y1;
            GaussProjCal(glon0,glat0,&x0,&y0);
            GaussProjCal(lon0,lat0,&x1,&y1);
            xMoveX = x1 - x0;
            xMoveY = y1 - y0;
        }
    }

    if(bNeedMove)
    {
        MoveXODR(pxodr,xMoveX,xMoveY);
    }

    int noldroadcount = mxodr.GetRoadCount();
    int noldjunctioncount = mxodr.GetJunctionCount();
    int nnewcount = pxodr->GetRoadCount();
    int nnewjunctioncount = pxodr->GetJunctionCount();
    int i;

    if(bNeedAjustID)
    {

        for(i=0;i<nnewcount;i++)
        {
            int nid = atoi(pxodr->GetRoad(i)->GetRoadId().data());
            int j;
            bool bNeedChange = false;

            for(j=0;j<noldroadcount;j++)
            {
                if(nid == atoi(mxodr.GetRoad(j)->GetRoadId().data()))
                {
                    bNeedChange  = true;
                    break;
                }
            }

            if(bNeedChange)
            {
                int nnewid = FindNewRoadID(&mxodr,pxodr);
                ChangeXODRRoadID(pxodr,i,nnewid);
            }

        }
        for(i=0;i<nnewjunctioncount;i++)
        {
            int nid = atoi(pxodr->GetJunction(i)->GetId().data());
            int j;
            bool bNeedChange = false;

            for(j=0;j<noldjunctioncount;j++)
            {
                if(nid == atoi(mxodr.GetJunction(j)->GetId().data()))
                {
                    bNeedChange  = true;
                    break;
                }
            }

            if(bNeedChange)
            {
                int nnewid = FindNewJunctionID(&mxodr,pxodr);
                ChangeXODRJunctionID(pxodr,i,nnewid);
            }

        }



    }

    for(i=0;i<nnewcount;i++)
    {
        mxodr.GetRoadVector()->push_back(pxodr->GetRoadVector()->at(i));
 //       OpenDrive * px = &mxodr;
    }

    for(i=0;i<nnewjunctioncount;i++)
    {
        mxodr.GetJunctionVector()->push_back(pxodr->GetJunctionVector()->at(i));
    }

    updateCBRoad();
    updateJunction();

    mbRefresh = true;
    update();






}

void MainWindow::onClickSave()
{
    QString str = QFileDialog::getSaveFileName(this,"Save XODR",".","*.xodr");

    if(str.isEmpty())return;

    if(str.indexOf(".xodr")<0)str = str + ".xodr";

    mxodr.SetHeader(1,1,"adcmap",1.1,QDateTime::currentDateTime().toString("yyyy-MM-dd").toLatin1().data(),0,0,0,0,glat0,glon0,ghdg0);
    OpenDriveXmlWriter x(&mxodr);
    x.WriteFile(str.toStdString());
}

void MainWindow::onClickRoadContact()
{
    int ch1,ch2;
    ch1  = mpCBRoad1->currentIndex();
    ch2 = mpCBRoad2->currentIndex();
    if((ch1 == -1)||(ch2 == -1))
    {
        return;
    }
//    if(ch1 == ch2)
//    {
//        QMessageBox::warning(this,"warn","road same");
//        return;
//    }
    int ntype1,ntype2;
    ntype1 = mpCBRC1->currentIndex();
    ntype2 = mpCBRC2->currentIndex();
    Road * p1 = mxodr.GetRoad(ch1);
    Road * p2 = mxodr.GetRoad(ch2);
    iv::roadcontact rc;
    rc.mnroad1id = atoi(p1->GetRoadId().data());
    rc.mnroad2id = atoi(p2->GetRoadId().data());
    rc.mncon1 = ntype1;
    rc.mncon2 = ntype2;

    char strname[256];
    snprintf(strname,256,"%s_%s_%s_%s",p1->GetRoadId().data(),mpCBRC1->currentText().toLatin1().data(),
             p2->GetRoadId().data(),mpCBRC2->currentText().toLatin1().data());
    mpCBRoadCon->clear();
    mpCBRoadCon->addItem(strname);
    mpCBLane1->clear();
    mpCBLane2->clear();


    mpCBLane1Lane2->clear();
    mpCBLane1Lane2op->clear();

    if(p1->GetLaneSectionCount()>0)
    {
        LaneSection * pLS;
        if(ntype1 == 0)
            pLS = p1->GetLaneSection(0);
        else
            pLS = p1->GetLaneSection(p1->GetLaneSectionCount()-1);
        int i;
        for(i=0;i<pLS->GetLaneCount();i++)
        {
            Lane * pL = pLS->GetLane(i);
//            if((pL->GetId() != 0)&&(strncmp(pL->GetType().data(),"driving",255)==0))
            if(pL->GetId() != 0)
                mpCBLane1->addItem(QString::number(pL->GetId()));
        }

    }

    if(p2->GetLaneSectionCount()>0)
    {
        LaneSection * pLS;
        if(ntype1 == 0)
            pLS = p2->GetLaneSection(0);
        else
            pLS = p2->GetLaneSection(p1->GetLaneSectionCount()-1);
        int i;
        for(i=0;i<pLS->GetLaneCount();i++)
        {
            Lane * pL = pLS->GetLane(i);
 //           if((pL->GetId() != 0)&&(strncmp(pL->GetType().data(),"driving",255)==0))
            if(pL->GetId() != 0)
                mpCBLane2->addItem(QString::number(pL->GetId()));
        }

    }

    mvectorrc.clear();
    mvectorrc.push_back(rc);
}

void MainWindow::onClickLaneContact()
{
    int n1,n2;
    n1 = mpCBLane1->currentText().toInt();
    n2 = mpCBLane2->currentText().toInt();

    iv::lanecontact xlc;
    xlc.ml1 = n1;
    xlc.ml2 = n2;
    if(mvectorrc.size()  < 1)return;
    mvectorrc[0].mvectorlc.push_back(xlc);

    char strname[256];
    snprintf(strname,255,"%dto%d",n1,n2);
    mpCBLane1Lane2->addItem(strname);

}

void MainWindow::onClickOpLaneContact()
{
    int n1,n2;
    n1 = mpCBLane1->currentText().toInt();
    n2 = mpCBLane2->currentText().toInt();

    iv::lanecontact xlc;
    xlc.ml1 = n1;
    xlc.ml2 = n2;
    if(mvectorrc.size()  < 1)return;
    mvectorrc[0].mvectorlcop.push_back(xlc);

    char strname[256];
    snprintf(strname,255,"%dto%d",n1,n2);
    mpCBLane1Lane2op->addItem(strname);
}

void MainWindow::onClickCreateRoad()
{
    if(mvectorrc.size()<1)return;

    Road * p1, *p2;
    int nroad1index;
    int nroad2index;
//    p1 = mvectorrc[0].mp1;
//    p2 = mvectorrc[0].mp2;

    int i;
    bool bhavep1 = false;
    bool bhavep2 = false;
    for(i=0;i<mxodr.GetRoadCount();i++)
    {
        if(mvectorrc[0].mnroad1id == atoi(mxodr.GetRoad(i)->GetRoadId().data()))
        {
            bhavep1 = true;
            p1 = mxodr.GetRoad(i);
            nroad1index = i;
            break;
        }
    }

    if(bhavep1 == false)
    {
        QMessageBox::warning(this,"Warn","Road not found.");
        return;
    }

    double off1,off2;



    for(i=0;i<mxodr.GetRoadCount();i++)
    {
        if(mvectorrc[0].mnroad2id == atoi(mxodr.GetRoad(i)->GetRoadId().data()))
        {
            bhavep2 = true;
            p2 = mxodr.GetRoad(i);
            nroad2index = i;
            break;
        }
    }

    if(bhavep2 == false)
    {
        QMessageBox::warning(this,"Warn","Road not found.");
        return;
    }

    if(mvectorrc[0].mvectorlc.size()<1)
    {
        QMessageBox::warning(this,"warn","No Lane Contact.");
        return;
    }

    double startx,starty,starthdg;
    double endx,endy,endhdg;

    double startheight,endheight;

    bool bFromstart,bTostart;
    if(mvectorrc[0].mncon1 == 0)
    {
        bFromstart = true;
        starthdg = p1->GetGeometryBlock(0)->GetGeometryAt(0)->GetHdg();
        off1 = getoff(p1,mvectorrc[0].mvectorlc[0].ml1,true);
        startx = p1->GetGeometryBlock(0)->GetGeometryAt(0)->GetX();
        starty = p1->GetGeometryBlock(0)->GetGeometryAt(0)->GetY();

        startx = startx + off1 * cos(starthdg -M_PI/2.0);
        starty = starty + off1 * sin(starthdg -M_PI/2.0);

        startheight = 0;
        if(p1->GetElevationCount()>0)
        {
            startheight = p1->GetElevation(0)->GetA();
        }

 //       if(mvectorrc[0].mvectorlc[0].ml1<0)
            starthdg = starthdg +M_PI;if(starthdg >=2.0*M_PI)starthdg = starthdg -2.0*M_PI;
    }
    else
    {
        bFromstart = false;
        if(GetEndPoint(p1,startx,starty,starthdg) != 0)
        {
            QMessageBox::warning(this,"warn","get start error.");
            return;
        }
        off1 = getoff(p1,mvectorrc[0].mvectorlc[0].ml1,false);
        startx = startx + off1 * cos(starthdg -M_PI/2.0);
        starty = starty + off1 * sin(starthdg -M_PI/2.0);

        startheight = 0;
        if(p1->GetElevationCount()>0)
        {
            startheight = p1->GetElevation(0)->GetA()
                    +p1->GetElevation(0)->GetB() * pow(p1->GetRoadLength(),1)
                    +p1->GetElevation(0)->GetC() * pow(p1->GetRoadLength(),2)
                    +p1->GetElevation(0)->GetD() * pow(p1->GetRoadLength(),3);
        }
    }

    if(mvectorrc[0].mncon2 == 0)
    {
        bTostart = true;
        off2 = getoff(p2,mvectorrc[0].mvectorlc[0].ml2,true);
        endx = p2->GetGeometryBlock(0)->GetGeometryAt(0)->GetX();
        endy = p2->GetGeometryBlock(0)->GetGeometryAt(0)->GetY();
        endhdg = p2->GetGeometryBlock(0)->GetGeometryAt(0)->GetHdg();

        endx = endx + off2 * cos(endhdg -M_PI/2.0);
        endy = endy + off2 * sin(endhdg -M_PI/2.0);

        endheight = 0;
        if(p2->GetElevationCount()>0)
        {
            endheight = p2->GetElevation(0)->GetA();
        }
    }
    else
    {
        bTostart = false;
        off2 = getoff(p2,mvectorrc[0].mvectorlc[0].ml2,false);
        if(GetEndPoint(p2,endx,endy,endhdg) != 0)
        {
            QMessageBox::warning(this,"warn","get end error.");
            return;
        }
        endx = endx + off2 * cos(endhdg -M_PI/2.0);
        endy = endy + off2 * sin(endhdg -M_PI/2.0);
        endhdg = endhdg +M_PI;if(endhdg >=2.0*M_PI)endhdg = endhdg -2.0*M_PI;

        endheight = 0;
        if(p2->GetElevationCount()>0)
        {
            endheight = p2->GetElevation(0)->GetA()
                    +p2->GetElevation(0)->GetB() * pow(p2->GetRoadLength(),1)
                    +p2->GetElevation(0)->GetC() * pow(p2->GetRoadLength(),2)
                    +p2->GetElevation(0)->GetD() * pow(p2->GetRoadLength(),3);
        }
    }


    //Create Geo
    double R = mpLERoadType->text().toDouble();
    std::vector<geobase> xvectorgeo;
    std::vector<geobase> xvectorgeo1,xvectorgeo2;
    switch(mpCBRoadType->currentIndex())
    {
    case 0:
        xvectorgeo = CreateTurnGeo(startx,starty,starthdg,endx,endy,endhdg,R);
        break;
    case 1:
        xvectorgeo = CreateLineGeo(startx,starty,starthdg,endx,endy,endhdg);
        break;
    case 2:
        xvectorgeo = CreateUTurnGeo(startx,starty,starthdg,endx,endy,endhdg,R);
        break;
    default:
        break;
    }

    if(mpCBRoadType->currentIndex() == 2)
    {
        for(i=0;i<xvectorgeo.size()/2;i++)
        {
            xvectorgeo1.push_back(xvectorgeo.at(i));
        }
        for(i=xvectorgeo.size()/2;i<xvectorgeo.size();i++)
        {
            xvectorgeo2.push_back(xvectorgeo.at(i));
        }
    }

    if(xvectorgeo.size() == 0)
    {
        QMessageBox::warning(this,"warn","Create Road Fail.");
        return;
    }

    double xroadlen = 0;

    if(mpCBRoadType->currentIndex() != 2)
    {
        for(i=0;i<xvectorgeo.size();i++)xroadlen = xroadlen + xvectorgeo[i].mfLen;

        mxodr.AddRoad("",xroadlen, QString::number(CreateRoadID()).toStdString(),"-1");
        Road * p = mxodr.GetRoad(mxodr.GetRoadCount() - 1);

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

        p1 = mxodr.GetRoad(nroad1index);
        p2 = mxodr.GetRoad(nroad2index);

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

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

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

        double * pswidth,*pewidth;
        std::vector<std::string> strvectorlanetype;
        int nlanecount = mvectorrc[0].mvectorlc.size();
        pswidth = new double[nlanecount];
        pewidth = new double[nlanecount];
        std::shared_ptr<double> ppswidth,ppewidth;
        ppswidth.reset(pswidth);
        ppewidth.reset(pewidth);

        for(i=0;i<nlanecount;i++)
        {
            pswidth[i] =  getlanewidth(p1,mvectorrc[0].mvectorlc.at(i).ml1,bFromstart);
            strvectorlanetype.push_back(getlanetype(p1,mvectorrc[0].mvectorlc.at(i).ml1,bFromstart));

        }
        for(i=0;i<nlanecount;i++)
        {
            pewidth[i] =  getlanewidth(p2,mvectorrc[0].mvectorlc.at(i).ml2,bTostart);
        }

        double * pa,*pb;
        pa = new double[nlanecount];
        pb = new double[nlanecount];
        std::shared_ptr<double> ppa,ppb;
        ppa.reset(pa);
        ppb.reset(pb);

        for(i=0;i<nlanecount;i++)
        {
            pa[i] = pswidth[i];
            pb[i] = (pewidth[i] - pa[i])/xroadlen;
        }

        for(i=0;i<nlanecount;i++)
        {
            pLS->AddLane(-1,(i+1)*(-1),strvectorlanetype[i],false,false);
            Lane * pLL = pLS->GetLane(pLS->GetLaneCount() - 1);

            pLL->AddWidthRecord(0,pa[i],pb[i],
                                0,0);
            pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");

        }

        int noplanecount = mvectorrc[0].mvectorlcop.size();
        if(noplanecount > 0)
        {
            pswidth = new double[noplanecount];
            pewidth = new double[noplanecount];
            ppswidth.reset(pswidth);
            ppewidth.reset(pewidth);

            strvectorlanetype.clear();
            for(i=0;i<noplanecount;i++)
            {
                pswidth[i] =  getlanewidth(p1,mvectorrc[0].mvectorlcop.at(i).ml1,bFromstart);
                strvectorlanetype.push_back(getlanetype(p1,mvectorrc[0].mvectorlcop.at(i).ml1,bFromstart));
            }
            for(i=0;i<noplanecount;i++)
            {
                pewidth[i] =  getlanewidth(p2,mvectorrc[0].mvectorlcop.at(i).ml2,bTostart);
            }

            pa = new double[noplanecount];
            pb = new double[noplanecount];
            ppa.reset(pa);
            ppb.reset(pb);

            for(i=0;i<noplanecount;i++)
            {
                pa[i] = pswidth[i];
                pb[i] = (pewidth[i] - pa[i])/xroadlen;
            }

            for(i=0;i<noplanecount;i++)
            {
                pLS->AddLane(1,(i+1),strvectorlanetype[i],false,false);
                Lane * pLL = pLS->GetLane(pLS->GetLaneCount() - 1);

                pLL->AddWidthRecord(0,pa[i],pb[i],
                                    0,0);
                pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");

            }
        }

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

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

        proad1->AddElevation(0,startheight,(endheight-startheight)/(xroadlen1+xroadlen2),0,0);
        proad2->AddElevation(0,startheight+xroadlen1*(endheight-startheight)/(xroadlen1+xroadlen2),
                             (endheight-startheight)/(xroadlen1+xroadlen2),
                             0,0);

        p1 = mxodr.GetRoad(nroad1index);
        p2 = mxodr.GetRoad(nroad2index);

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

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

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

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

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

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

        double * pswidth,*pewidth;
        int nlanecount = mvectorrc[0].mvectorlc.size();
        std::vector<std::string> strvectorlanetype;
        pswidth = new double[nlanecount];
        pewidth = new double[nlanecount];
        std::shared_ptr<double> ppswidth,ppewidth;
        ppswidth.reset(pswidth);
        ppewidth.reset(pewidth);

        for(i=0;i<nlanecount;i++)
        {
            pswidth[i] =  getlanewidth(p1,mvectorrc[0].mvectorlc.at(i).ml1,bFromstart);
            strvectorlanetype.push_back(getlanetype(p1,mvectorrc[0].mvectorlc.at(i).ml1,bFromstart));

        }
        for(i=0;i<nlanecount;i++)
        {
            pewidth[i] =  getlanewidth(p2,mvectorrc[0].mvectorlc.at(i).ml2,bTostart);
        }

        double * pa,*pb;
        pa = new double[nlanecount];
        pb = new double[nlanecount];
        std::shared_ptr<double> ppa,ppb;
        ppa.reset(pa);
        ppb.reset(pb);

        for(i=0;i<nlanecount;i++)
        {
            pa[i] = pswidth[i];
            pb[i] = (pewidth[i] - pa[i])/(xroadlen1+xroadlen2);
        }

        for(i=0;i<nlanecount;i++)
        {
            pLS1->AddLane(-1,(i+1)*(-1),strvectorlanetype[i],false,false);
            Lane * pLL = pLS1->GetLane(pLS1->GetLaneCount() - 1);

            pLL->AddWidthRecord(0,pa[i],pb[i],
                                0,0);
            pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");

            pLS2->AddLane(-1,(i+1)*(-1),strvectorlanetype[i],false,false);
            pLL = pLS2->GetLane(pLS2->GetLaneCount() - 1);

            pLL->AddWidthRecord(0,pa[i]+pb[i]*xroadlen1 ,pb[i],
                                0,0);
            pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");

        }
        int noplanecount = mvectorrc[0].mvectorlcop.size();
        if(noplanecount > 0)
        {
            pswidth = new double[noplanecount];
            pewidth = new double[noplanecount];
            ppswidth.reset(pswidth);
            ppewidth.reset(pewidth);

            strvectorlanetype.clear();
            for(i=0;i<noplanecount;i++)
            {
                pswidth[i] =  getlanewidth(p1,mvectorrc[0].mvectorlcop.at(i).ml1,bFromstart);
                strvectorlanetype.push_back(getlanetype(p1,mvectorrc[0].mvectorlcop.at(i).ml1,bFromstart));
            }
            for(i=0;i<noplanecount;i++)
            {
                pewidth[i] =  getlanewidth(p2,mvectorrc[0].mvectorlcop.at(i).ml2,bTostart);
            }

            pa = new double[noplanecount];
            pb = new double[noplanecount];
            ppa.reset(pa);
            ppb.reset(pb);

            for(i=0;i<noplanecount;i++)
            {
                pa[i] = pswidth[i];
                pb[i] = (pewidth[i] - pa[i])/xroadlen;
            }

            for(i=0;i<noplanecount;i++)
            {
                pLS1->AddLane(1,(i+1),strvectorlanetype[i],false,false);
                Lane * pLL = pLS1->GetLane(pLS1->GetLaneCount() - 1);

                pLL->AddWidthRecord(0,pa[i],pb[i],
                                    0,0);
                pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");

                pLS2->AddLane(1,(i+1),strvectorlanetype[i],false,false);
                pLL = pLS2->GetLane(pLS2->GetLaneCount() - 1);

                pLL->AddWidthRecord(0,pa[i]+pb[i]*xroadlen1 ,pb[i],
                                    0,0);
                pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");

            }
        }


    }
    updateCBRoad();
    mbRefresh = true;
    update();

}

int MainWindow::GetEndPoint(Road *proad, double &x, double &y, double &hdg)
{
    GeometryBlock * pblock = proad->GetLastGeometryBlock();

    RoadGeometry * pgeo = pblock->GetLastGeometry();

    //0-line, 1-arc, 2-spiral 3-poly3 4-parampoly3
    switch (pgeo->GetGeomType()) {
    case 0:
        {
        GeometryLine * pline = (GeometryLine *)pgeo;
        x = pline->GetX() + pline->GetLength() * cos(pline->GetHdg());
        y = pline->GetY() + pline->GetLength() * sin(pline->GetHdg());
        hdg = pline->GetHdg();
        }
        return 0;
        break;
    case 1:
        {
        GeometryArc * parc = (GeometryArc *)pgeo;
        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);
        x = x_center + fabs(1.0/parc->GetCurvature()) * cos(parc->GetHdg() + parc->GetLength() * parc->GetCurvature() - M_PI/2.0);
        y = y_center + fabs(1.0/parc->GetCurvature()) * sin(parc->GetHdg() + parc->GetLength() * parc->GetCurvature() - M_PI/2.0);
        hdg = parc->GetHdg() + parc->GetLength() * parc->GetCurvature();
        return 0;
        }
        break;
    case 2:
        {
        GeometrySpiral * pspiral = (GeometrySpiral *)pgeo;
        pspiral->GetCoords(pspiral->GetS()+pspiral->GetLength(),x,y,hdg);
        return 0;
        }
        break;
    case 3:
        QMessageBox::warning(this,"warn","type not supported.");
        break;
    case 4:
        {

        double xtem,ytem;
        double xtem1,ytem1,x1,y1;
        GeometryParamPoly3 * ppoly3 = (GeometryParamPoly3* )pgeo;
        double s = ppoly3->GetLength();
        xtem = ppoly3->GetuA() + ppoly3->GetuB() * s  + ppoly3->GetuC() * s*s  + ppoly3->GetuD() * s*s*s ;
        ytem = ppoly3->GetvA() + ppoly3->GetvB() * s + ppoly3->GetvC() * s*s  + ppoly3->GetvD() * s*s*s ;
        x = xtem*cos(ppoly3->GetHdg()) - ytem * sin(ppoly3->GetHdg()) + ppoly3->GetX();
        y = xtem*sin(ppoly3->GetHdg()) + ytem * cos(ppoly3->GetHdg()) + ppoly3->GetY();
        s = ppoly3->GetLength()*0.99;
        if(s>0)
        {
            xtem1 = ppoly3->GetuA() + ppoly3->GetuB() * s  + ppoly3->GetuC() * s*s  + ppoly3->GetuD() * s*s*s ;
            ytem1 = ppoly3->GetvA() + ppoly3->GetvB() * s + ppoly3->GetvC() * s*s  + ppoly3->GetvD() * s*s*s ;
            x1 = xtem*cos(ppoly3->GetHdg()) - ytem * sin(ppoly3->GetHdg()) + ppoly3->GetX();
            y1 = xtem*sin(ppoly3->GetHdg()) + ytem * cos(ppoly3->GetHdg()) + ppoly3->GetY();
            hdg = geofit::CalcHdg(xtem1,ytem1,x1,y1);
        }
        else
        {
            hdg = 0;
        }
        return 0;

        }
        break;
    default:
        QMessageBox::warning(this,"warn","type not supported.");
        break;
    }


    return -1;
}

void MainWindow::onChangeRoadType(int index)
{
    if(index == 1)
    {
       mpLBRoadType->setVisible(false);
       mpLERoadType->setVisible(false);
    }
    else
    {
        mpLBRoadType->setVisible(true);
        mpLERoadType->setVisible(true);
    }

    if(index == 0)
    {
        mpLBRoadType->setText("Radius:");
        mpLERoadType->setText("6.0");
    }

    if(index == 2)
    {
        mpLBRoadType->setText("Extend:");
        mpLERoadType->setText("3.0");
    }
}

std::vector<geobase> MainWindow::CreateLineGeo(double startx, double starty, double starthdg, double endx, double endy, double endhdg)
{

//    std::vector<geobase> xvectorgeo;
//    geobase xgeobezier;
//    int nbtype;
//    double fabc[3],fxy[2],fblen,fbhdg;
//    geofit x;
//    x.CreateBezier(startx,starty,starthdg,
//                 endx,endy,endhdg,
//                 0.35,0.35,xgeobezier.mfu,xgeobezier.mfv,xgeobezier.mfLen,
//                 nbtype,fabc,&fbhdg,fxy,&fblen);
//    if(nbtype == 2)
//    {
//    xgeobezier.mfHdg = starthdg;
//    xgeobezier.mfX = startx;
//    xgeobezier.mfY = starty;
//    xgeobezier.mnType = 2;
//    }
//    else
//    {
//        xgeobezier.mnType = 0; //Line
//        xgeobezier.mfHdgStart = fbhdg;
//        xgeobezier.mfHdg = fbhdg;
//        xgeobezier.mfX = fxy[0];
//        xgeobezier.mfY = fxy[1];
//        xgeobezier.mfLen = fblen;
//    }
//    xvectorgeo.push_back(xgeobezier);
//    return xvectorgeo;
    geobase linegeo;
    linegeo.mnType = 0;
    linegeo.mfX = startx;
    linegeo.mfY = starty;
    linegeo.mfHdg = geofit::CalcHdg(startx,starty,endx,endy);
    linegeo.mfLen = sqrt(pow(endx - startx,2)+pow(endy - starty,2));
    std::vector<geobase> xvectorgeo;
    xvectorgeo.push_back(linegeo);
    return xvectorgeo;
}

std::vector<geobase> MainWindow::CreateTurnGeo(double startx, double starty, double starthdg, double endx, double endy, double endhdg,double R)
{
    std::vector<geobase>  xvectorgeo;
    xvectorgeo.clear();
    if(starthdg == endhdg)
    {
        std::cout<<"hdg same use line contact"<<std::endl;
        return CreateLineGeo(startx,starty,starthdg,endx,endy,endhdg);
    }
    double  a1,c1,a2,c2;
    double inter_x,inter_y;
    if((starthdg == M_PI/2.0)||(starthdg == 3.0*M_PI/2.0))
    {
        a2 = tan(endhdg) *(-1);
        c2 = 0-a2*endx - endy;
        a1 = 1;
        c1 = startx *(-1);
        inter_x = startx;
        inter_y = 0 - a2*inter_x-c2;
    }
    else
    {
        if((endhdg == M_PI/2.0)||(endhdg == 3.0*M_PI/2.0))
        {
            a1 = tan(starthdg) *(-1);
            c1 = 0-a1*startx - starty;
            a2 = 1;
            c2 = endx *(-1);
            inter_x = endx;
            inter_y = 0 - a1*inter_x-c1;
        }
        else
        {
            a1 = tan(starthdg) *(-1);
            a2 = tan(endhdg) *(-1);
            c1 = 0-a1*startx - starty;
            c2 = 0-a2*endx - endy;
            inter_x = (c1-c2)/(a2-a1);
            inter_y = 0 - a1*inter_x - c1;
        }
    }

    double dis1,dis2;
    dis1 =sqrt(pow(inter_x - startx,2)+pow(inter_y - starty,2));
    dis2 =sqrt(pow(inter_x - endx,2)+pow(inter_y - endy,2));

    if((dis1<1.0)||(dis2<1.0))
    {
        std::cout<<"use line connect."<<std::endl;
        return CreateLineGeo(startx,starty,starthdg,endx,endy,endhdg);

    }

    double hdgdiff = endhdg - starthdg;
    if(hdgdiff >= M_PI)hdgdiff = hdgdiff - 2.0*M_PI;
    if(hdgdiff <= (-M_PI))hdgdiff = hdgdiff + 2.0*M_PI;


    double slen = R*tan(fabs(hdgdiff/2.0));
    if((dis1<slen)||(dis2<slen))
    {
        std::cout<<"radius is big. use line."<<std::endl;
        return CreateLineGeo(startx,starty,starthdg,endx,endy,endhdg);
    }

    double p1_x,p1_y,p2_x,p2_y;
    p1_x = inter_x- slen * cos(starthdg);
    p1_y = inter_y- slen * sin(starthdg);
    p2_x = inter_x+slen*cos(endhdg);
    p2_y = inter_y+slen*sin(endhdg);

    geobase linegeo;
    linegeo.mnType = 0;
    linegeo.mfX = startx;
    linegeo.mfY = starty;
    linegeo.mfHdg = starthdg;
    linegeo.mfLen = sqrt(pow(p1_x - startx,2)+pow(p1_y - starty,2));
    xvectorgeo.push_back(linegeo);

    geobase arcgeo;
    arcgeo.mnType = 1;
    arcgeo.mfX = p1_x;
    arcgeo.mfY = p1_y;
    arcgeo.mfHdg =  starthdg;
    arcgeo.mfHdgStart = starthdg;
    arcgeo.mfLen = R*fabs(hdgdiff);
    arcgeo.mR =  R*(fabs(hdgdiff)/hdgdiff);
    arcgeo.mfEndX = p2_x;
    arcgeo.mfEndY = p2_y;
    xvectorgeo.push_back(arcgeo);

    linegeo.mnType = 0;
    linegeo.mfX = p2_x;
    linegeo.mfY = p2_y;
    linegeo.mfHdg = endhdg;
    linegeo.mfLen = sqrt(pow(p2_x - endx,2)+pow(p2_y - endy,2));
    xvectorgeo.push_back(linegeo);

    return xvectorgeo;



}


std::string MainWindow::getlanetype(Road *p, int nlane, bool bstart)
{
    Lane * plane = 0;
    int i;
    LaneSection * pLS;
    if(bstart)
    {
        pLS = p->GetLaneSection(0);
    }
    else
    {
        pLS = p->GetLaneSection(p->GetLaneSectionCount() -1);
    }
    int nlanecount = pLS->GetLaneCount();
    for(i=0;i<nlanecount;i++)
    {
        if(nlane == pLS->GetLane(i)->GetId())
        {
            plane = pLS->GetLane(i);
            break;
        }
    }
    if(plane == 0)
    {
        std::cout<<"MainWindow::getlanetype　can't find lane : "<<nlane<<std::endl;
        return "driving";
    }
    return plane->GetType();
}

double MainWindow::getlanewidth(Road * p, int nlane,bool  bstart)
{
    Lane * plane = 0;
    double a,b,c,d;
    double s;
    if(bstart)
    {
        s = 0;
    }
    else
    {
        s = p->GetRoadLength();
    }
    int i;
    LaneSection * pLS;
    if(bstart)
    {
        pLS = p->GetLaneSection(0);
    }
    else
    {
        pLS = p->GetLaneSection(p->GetLaneSectionCount() -1);
    }
    int nlanecount = pLS->GetLaneCount();
    for(i=0;i<nlanecount;i++)
    {
        if(nlane == pLS->GetLane(i)->GetId())
        {
            plane = pLS->GetLane(i);
            break;
        }
    }
    if(plane == 0)
    {
        std::cout<<"MainWindow::getlanewidth　can't find lane : "<<nlane<<std::endl;
        return 0;
    }
    if(bstart)
    {
        return plane->GetLaneWidth(0)->GetA();
    }
    else
    {
        s = p->GetRoadLength() - pLS->GetS();
        LaneWidth * pLW = plane->GetLaneWidth(plane->GetLaneWidthCount()-1);
        a = pLW->GetA();b = pLW->GetB();c = pLW->GetC();d = pLW->GetD();
        return a+b*s+c*pow(s,2)+d*pow(s,3);
    }
}

double MainWindow::getoff(Road *p, int nlane, bool bstart)
{
    double off = 0;
    int i;
    if(bstart)
    {
        LaneSection * pLS = p->GetLaneSection(0);
        if(nlane<0)
        {
            if(nlane == -1)return 0;
            else
            {
                for(i=0;i<pLS->GetLaneCount();i++)
                {
                    Lane * pL = pLS->GetLane(i);
                    if((pL->GetId()<0)&&(pL->GetId()>nlane))
                    {
                        off = off + pL->GetLaneWidth(0)->GetA();
                    }
                }
            }
        }
        else
        {
            if(nlane == 1)return 0;
            else
            {
                for(i=0;i<pLS->GetLaneCount();i++)
                {
                    Lane * pL = pLS->GetLane(i);
                    if((pL->GetId()>0)&&(pL->GetId()<nlane))
                    {
                        off = off - pL->GetLaneWidth(0)->GetA();
                    }
                }
            }
        }
    }
    else
    {
        LaneSection * pLS = p->GetLaneSection(p->GetLaneSectionCount()-1);

        if(nlane<0)
        {
            if(nlane == -1)return 0;
            else
            {
                for(i=0;i<pLS->GetLaneCount();i++)
                {
                    Lane * pL = pLS->GetLane(i);
                    if((pL->GetId()<0)&&(pL->GetId()>nlane))
                    {
                        double a,b,c,d;
                        a = pL->GetLaneWidth(pL->GetLaneWidthCount()-1)->GetA();
                        b = pL->GetLaneWidth(pL->GetLaneWidthCount()-1)->GetB();
                        c = pL->GetLaneWidth(pL->GetLaneWidthCount()-1)->GetC();
                        d = pL->GetLaneWidth(pL->GetLaneWidthCount()-1)->GetD();
                        double s = p->GetRoadLength();
                        off = off + a + b*s +c *s*s + d*s*s*s;
                    }
                }
            }
        }
        else
        {
            if(nlane == 1)return 0;
            else
            {
                for(i=0;i<pLS->GetLaneCount();i++)
                {
                    Lane * pL = pLS->GetLane(i);
                    if((pL->GetId()>0)&&(pL->GetId()<nlane))
                    {
                        double a,b,c,d;
                        a = pL->GetLaneWidth(pL->GetLaneWidthCount()-1)->GetA();
                        b = pL->GetLaneWidth(pL->GetLaneWidthCount()-1)->GetB();
                        c = pL->GetLaneWidth(pL->GetLaneWidthCount()-1)->GetC();
                        d = pL->GetLaneWidth(pL->GetLaneWidthCount()-1)->GetD();
                        double s = p->GetRoadLength();
                        off = off - a - b*s -c *s*s - d*s*s*s;
                    }
                }
            }
        }
    }
    return off;
}

std::vector<geobase> MainWindow::CreateUTurnGeo(double startx, double starty, double starthdg, double endx, double endy, double endhdg, double fextend)
{
    std::vector<geobase>  xvectorgeo;


    double p1_x,p1_y, p2_x,p2_y,p1_hdg,p2_hdg;

    p1_x = fextend*cos(starthdg) + startx;
    p1_y = fextend*sin(starthdg) + starty;

    p2_x = fextend*cos(endhdg + M_PI) + endx;
    p2_y = fextend*sin(endhdg + M_PI) + endy;

    p1_hdg = starthdg;
    p2_hdg = endhdg;

    if(starthdg == endhdg)
    {
        std::cout<<" hdg is same, can't create u turn."<<std::endl;
        return xvectorgeo;
    }

    double hdgse = geofit::CalcHdg(p1_x,p1_y,p2_x,p2_y);
    double hdgdiff = hdgse - p1_hdg;
    if(hdgdiff < 0)hdgdiff = hdgdiff  + 2.0*M_PI;

    bool bPA = true; //Positive
    if(hdgdiff >=M_PI)bPA = false;

    double xdiff;
    if(bPA)xdiff = hdgdiff - M_PI/2.0;
    else xdiff = hdgdiff - 3.0*M_PI/2.0;

    double xdis = sqrt(pow(p1_x-p2_x,2)+pow(p1_y-p2_y,2));
    double R = xdis/(2.0*cos(xdiff));

    double x_center,y_center;

    double xhdgtocenter;

    if(bPA)
    {
        xhdgtocenter = p1_hdg + M_PI/2.0;
        if(xhdgtocenter  >= 2.0*M_PI)xhdgtocenter = xhdgtocenter - M_PI*2.0;
    }
    else
    {
        xhdgtocenter = p1_hdg - M_PI/2.0;
        if(xhdgtocenter < 0)xhdgtocenter = xhdgtocenter + 2.0*M_PI;
    }

    x_center = p1_x + R*cos(xhdgtocenter);
    y_center = p1_y + R*sin(xhdgtocenter);

    double xhdgcentertoarc;
    if(bPA)
    {
        xhdgcentertoarc = hdgse - M_PI/2.0;
        if(xhdgcentertoarc <0)xhdgcentertoarc = xhdgcentertoarc + 2.0*M_PI;
    }
    else
    {
        xhdgcentertoarc = hdgse + M_PI/2.0;
        if(xhdgcentertoarc >= M_PI*2.0)xhdgcentertoarc = xhdgcentertoarc - M_PI*2.0;
    }

    double p3_x,p3_y,p3_hdg;
    p3_hdg = hdgse;
    p3_x = x_center + R* cos(xhdgcentertoarc);
    p3_y = y_center + R* sin(xhdgcentertoarc);

//    pgb->AddGeometryArc(s,parc->mfX,parc->mfY,parc->mfHdgStart,parc->mfLen,1.0/parc->mR);
    geobase c2,c3;
    c2.mfX = p1_x;
    c2.mfY = p1_y;
    c2.mfHdgStart = p1_hdg;
    c3.mfX = p3_x;
    c3.mfY = p3_y;
    c3.mfHdgStart = hdgse;
    c2.mnType = 1;
    c3.mnType = 1;
    if(bPA)
    {
        c2.mfLen = hdgdiff * R;
        c2.mR = R;
        c3.mfLen = c2.mfLen;
        c3.mR = R;
    }
    else
    {
        c2.mfLen = (2.0*M_PI - hdgdiff) * R;
        c2.mR = R*(-1);
        c3.mfLen = c2.mfLen;
        c3.mR = R;
    }
    geobase l1,l2;
    if(fextend != 0)
    {
        l1.mfX = startx;
        l1.mfY = starty;
        l1.mfHdg = starthdg;
        l1.mfLen = fextend;
        l1.mnType = 0;
        l2.mfX = p2_x;
        l2.mfY = p2_y;
        l2.mfHdg = p2_hdg;
        l2.mfLen = fextend;
        l2.mnType = 0;
        xvectorgeo.push_back(l1);
    }
    xvectorgeo.push_back(c2);
    xvectorgeo.push_back(c3);
    if(fextend != 0)xvectorgeo.push_back(l2);

    return xvectorgeo;
}

void MainWindow::ChangeXODRRoadID(OpenDrive *pxodr, int index, int newid)
{
    Road * proad;
    int  nsize  = pxodr->GetRoadCount();
    int i;
    if(index<0)return;
    if(index>=nsize)return;
    proad = pxodr->GetRoad(index);
    int  noldid = atoi(proad->GetRoadId().data());
    char strid[255];
    snprintf(strid,255,"%d",newid);
    proad->SetRoadId(strid);
    for(i=0;i<nsize;i++)
    {
        Road * proad2  = pxodr->GetRoad(i);
        if(proad2->GetPredecessor()!= 0)
        {
            RoadLink  * plink = proad2->GetPredecessor();
            if(strncmp(plink->GetElementType().data(),"road",255)== 0)
            {
                if(atoi(plink->GetElementId().data()) == noldid)
                {
                    plink->SetElementId(strid);
                }
            }
        }
        if(proad2->GetSuccessor()!= 0)
        {
            RoadLink  * plink = proad2->GetSuccessor();
            if(strncmp(plink->GetElementType().data(),"road",255)== 0)
            {
                if(atoi(plink->GetElementId().data()) == noldid)
                {
                    plink->SetElementId(strid);
                }
            }
        }
    }

    nsize  = pxodr->GetJunctionCount();
    for(i=0;i<nsize;i++)
    {
        Junction * pjunction = pxodr->GetJunction(i);
        int j;
        for(j=0;j<pjunction->GetJunctionConnectionCount();j++)
        {
            JunctionConnection * pjc = pjunction->GetJunctionConnection(j);
            if(atoi(pjc->GetIncomingRoad().data()) == noldid)
            {
                pjc->SetIncomingRoad(strid);
            }
            if(atoi(pjc->GetConnectingRoad().data()) == noldid)
            {
                pjc->SetConnectingRoad(strid);
            }

        }
    }

}

void MainWindow::ChangeXODRJunctionID(OpenDrive *pxodr, int index, int newid)
{
    Junction * pjunction;
    int  nsize  = pxodr->GetJunctionCount();
    int i;
    if(index<0)return;
    if(index>=nsize)return;
    pjunction = pxodr->GetJunction(index);
    int  noldid = atoi(pjunction->GetId().data());
    char strid[255];
    snprintf(strid,255,"%d",newid);
    pjunction->SetId(strid);
    for(i=0;i<nsize;i++)
    {
        Road * proad2  = pxodr->GetRoad(i);
        if(proad2->GetPredecessor()!= 0)
        {
            RoadLink  * plink = proad2->GetPredecessor();
            if(strncmp(plink->GetElementType().data(),"junction",255)== 0)
            {
                if(atoi(plink->GetElementId().data()) == noldid)
                {
                    plink->SetElementId(strid);
                }
            }
        }
        if(proad2->GetSuccessor()!= 0)
        {
            RoadLink  * plink = proad2->GetSuccessor();
            if(strncmp(plink->GetElementType().data(),"junction",255)== 0)
            {
                if(atoi(plink->GetElementId().data()) == noldid)
                {
                    plink->SetElementId(strid);
                }
            }
        }
    }
}

void MainWindow::MoveXODR(OpenDrive *pxodr, double movex, double movey)
{
    int nsize = pxodr->GetRoadCount();
    int i;
    for(i=0;i<nsize;i++)
    {
        Road * pRoad = pxodr->GetRoad(i);
        int nblockcount = pRoad->GetGeometryBlockCount();
        int j;
        for(j=0;j<nblockcount;j++)
        {
            GeometryBlock * pgb = pRoad->GetGeometryBlock(j);
            if(pgb != 0)
            {
                pgb->GetGeometryAt(0)->SetX(pgb->GetGeometryAt(0)->GetX() + movex);
                pgb->GetGeometryAt(0)->SetY(pgb->GetGeometryAt(0)->GetY() + movey);
            }
        }
    }
}

int MainWindow::FindNewRoadID(OpenDrive *pxodr1, OpenDrive *pxodr2)
{
    int nroadsize1,nroadsize2;
    nroadsize1 = pxodr1->GetRoadCount();
    nroadsize2 = pxodr2->GetRoadCount();
    int i;
    int * pnid = new int[nroadsize1 + nroadsize2];
    std::shared_ptr<int> ppnid;ppnid.reset(pnid);

    for(i=0;i<nroadsize1;i++)
    {
        pnid[i] = atoi(pxodr1->GetRoad(i)->GetRoadId().data());
    }
    for(i=0;i<nroadsize2;i++)
    {
        pnid[i+nroadsize1] = atoi(pxodr2->GetRoad(i)->GetRoadId().data());
    }

    int nstartid  = 40000;
    bool bUsed = true;
    while(bUsed == true)
    {
        bUsed = false;
        for(i=0;i<(nroadsize1 + nroadsize2);i++)
        {
            if(pnid[i] == nstartid)
            {
                bUsed = true;
                break;
            }
        }
        if(bUsed == false)break;
        nstartid++;
    }
    return nstartid;
}

int MainWindow::FindNewJunctionID(OpenDrive *pxodr1, OpenDrive *pxodr2)
{
    int njunctionsize1,njunctionsize2;
    njunctionsize1 = pxodr1->GetRoadCount();
    njunctionsize2 = pxodr2->GetRoadCount();
    int i;
    int * pnid = new int[njunctionsize1 + njunctionsize2];
    std::shared_ptr<int> ppnid;ppnid.reset(pnid);

    for(i=0;i<njunctionsize1;i++)
    {
        pnid[i] = atoi(pxodr1->GetJunction(i)->GetId().data());
    }
    for(i=0;i<njunctionsize2;i++)
    {
        pnid[i+njunctionsize1] = atoi(pxodr2->GetJunction(i)->GetId().data());
    }

    int nstartid  = 50000;
    bool bUsed = true;
    while(bUsed == true)
    {
        bUsed = false;
        for(i=0;i<(njunctionsize1 + njunctionsize2);i++)
        {
            if(pnid[i] == nstartid)
            {
                bUsed = true;
                break;
            }
        }
        if(bUsed == false)break;
        nstartid++;
    }
    return nstartid;
}

void MainWindow::onClickRoadMark()
{
    mbRefresh = true;
    update();
}

void MainWindow::onClickRoadDel()
{
    if(mpCBRoad->count() ==  0)
    {
        QMessageBox::warning(this,"warn","no road to be delete");
        return;
    }
    int nroadid = mpCBRoad->currentText().toInt();

    Road * pRoad = 0;
    int nroadsize = mxodr.GetRoadCount();
    int i;
    int index;
    for(i=0;i<nroadsize;i++)
    {
        if(nroadid == atoi(mxodr.GetRoad(i)->GetRoadId().data()))
        {
            pRoad = mxodr.GetRoad(i);
            index = i;
            break;
        }

    }

    if(pRoad == 0)
    {
        QMessageBox::warning(this,"warn","can't find road");
        return;
    }

    mxodr.DeleteRoad(index);

    nroadsize = mxodr.GetRoadCount();
    for(i=0;i<nroadsize;i++)
    {
        pRoad = mxodr.GetRoad(i);
        RoadLink * ppre,* pnext;
        ppre = pRoad->GetPredecessor();
        pnext = pRoad->GetSuccessor();
        if(ppre != 0)
        {
            if(strncmp(ppre->GetElementType().data(),"road",255) == 0)
            {
                if(atoi(ppre->GetElementId().data()) == nroadid)
                {
                    pRoad->RemovePredecessor();
                    LaneSection * pLS = pRoad->GetLaneSection(0);
                    int nlanecount = pLS->GetLaneCount();
                    int j;
                    for(j=0;j<nlanecount;j++)
                    {
                        Lane * pLane;
                        pLane = pLS->GetLane(j);
                        pLane->RemovePredecessor();
                    }

                }
            }
        }

        if(pnext != 0)
        {
            if(strncmp(pnext->GetElementType().data(),"road",255) == 0)
            {
                if(atoi(pnext->GetElementId().data()) == nroadid)
                {
                    pRoad->RemoveSuccessor();
                    LaneSection * pLS = pRoad->GetLaneSection(pRoad->GetLaneSectionCount()-1);
                    int nlanecount = pLS->GetLaneCount();
                    int j;
                    for(j=0;j<nlanecount;j++)
                    {
                        Lane * pLane;
                        pLane = pLS->GetLane(j);
                        pLane->RemoveSuccessor();
                    }

                }
            }
        }
    }

    //remove juction connection where id = roadid
    int njunctioncount = mxodr.GetJunctionCount();
    Junction * pjunction;
    for(i=0;i<njunctioncount;i++)
    {
        pjunction = mxodr.GetJunction(i);
        int j;
        for(j=0;j<pjunction->GetJunctionConnectionCount();j++)
        {
            JunctionConnection * pjc = pjunction->GetJunctionConnection(j);
            int nfromid = atoi(pjc->GetIncomingRoad().data());
            int ntoid = atoi(pjc->GetConnectingRoad().data());
            if((nfromid == nroadid)||(ntoid == nroadid))
            {
                pjunction->DeleteJunctionConnection(j);
                j--;
            }
        }
    }
    updateCBRoad();
    mbRefresh = true;
    update();
}

void MainWindow::updateCBRoad()
{
    mpCBRoad->clear();
    mpCBRoad1->clear();
    mpCBRoad2->clear();
    mpCBJunctionIncommingRoad->clear();
    mpCBJunctionConnectingRoad->clear();
    mpCBPreNxtRoad->clear();
    int i;
    int nroadcount = mxodr.GetRoadCount();
    for(i=0;i<nroadcount;i++)
    {
        const char * strname = mxodr.GetRoad(i)->GetRoadId().data();
        mpCBRoad->addItem(strname);
        mpCBRoad1->addItem(strname);
        mpCBRoad2->addItem(strname);
        mpCBJunctionIncommingRoad->addItem(strname);
        mpCBJunctionConnectingRoad->addItem(strname);
        mpCBPreNxtRoad->addItem(strname);
    }
}

void MainWindow::onClickCBRoadChange(int index)
{
   Road * pRoad = mxodr.GetRoad(index);
   if(pRoad == 0)
   {
//       QMessageBox::warning(this,"WARN","MainWindow::onClickCBRoadChange road is NULL");
       return;
   }
   int i;

   mpLabelRoadShowPreID->setText("");
   mpLabelRoadShowPreType2->setText("");
   mpLabelRoadShowPreType1->setText("");
   mpCBRoadShowPre->clear();
   mpLabelRoadShowNxtID->setText("");
   mpLabelRoadShowNxtType2->setText("");
   mpLabelRoadShowNxtType1->setText("");
   mpCBRoadShowNext->clear();
   if(pRoad->GetPredecessor() != 0)
   {
       RoadLink * pRL = pRoad->GetPredecessor();
       mpLabelRoadShowPreID->setText(pRL->GetElementId().data());
       mpLabelRoadShowPreType2->setText(pRL->GetContactPoint().data());
       mpLabelRoadShowPreType1->setText(pRL->GetElementType().data());
   }
   if(pRoad->GetSuccessor() != 0)
   {
       RoadLink * pRL = pRoad->GetSuccessor();
       mpLabelRoadShowNxtID->setText(pRL->GetElementId().data());
       mpLabelRoadShowNxtType2->setText(pRL->GetContactPoint().data());
       mpLabelRoadShowNxtType1->setText(pRL->GetElementType().data());
   }

   LaneSection * pLS = pRoad->GetLaneSection(0);
   int nlanecount = pLS->GetLaneCount();
   for(i=0;i<nlanecount;i++)
   {
       char strout[255];
       char strtem[255];

       Lane * pLane = pLS->GetLane(i);
       snprintf(strout,255,"%d type:%s",pLane->GetId(),pLane->GetType().data());
       if(pLane->IsPredecessorSet())
       {
           snprintf(strtem,255," pre:%d",pLane->GetPredecessor());
           strncat(strout,strtem,255);
       }
       mpCBRoadShowPre->addItem(strout);
   }
   pLS = pRoad->GetLaneSection(pRoad->GetLaneSectionCount()-1);
   nlanecount = pLS->GetLaneCount();
   for(i=0;i<nlanecount;i++)
   {
       char strout[255];
       char strtem[255];

       Lane * pLane = pLS->GetLane(i);
       snprintf(strout,255,"%d type:%s",pLane->GetId(),pLane->GetType().data());
       if(pLane->IsSuccessorSet())
       {
           snprintf(strtem,255," suc:%d",pLane->GetSuccessor());
           strncat(strout,strtem,255);
       }
       mpCBRoadShowNext->addItem(strout);
   }
}

void MainWindow::updateJunction()
{
    int i;
    int njunctioncount = mxodr.GetJunctionCount();
    mpCBJunction->clear();
    for(i=0;i<njunctioncount;i++)
    {
        Junction * pjunction = mxodr.GetJunction(i);
        mpCBJunction->addItem(pjunction->GetId().data());

    }
}

void MainWindow::onClickCBJunctionChange(int index)
{
    mpCBJunctionConnection->clear();
    Junction * pjunction = mxodr.GetJunction(index);
    if(pjunction == NULL)
    {
//        QMessageBox::warning(this,"warn","MainWindow::onClickCBJunctionChange can't find junction");
        return;
    }

    int nconnectioncount = pjunction->GetJunctionConnectionCount();
    int i;
    for(i=0;i<nconnectioncount;i++)
    {
        JunctionConnection * pJC = pjunction->GetJunctionConnection(i);
        mpCBJunctionConnection->addItem(pJC->GetId().data());
    }
    mpCBJunctionFromTo->clear();
    mpLabelJunctionConnectingRoad->setText("");
    mpLabelJunctionContactPoint->setText("");
    mpLabelJunctionIncommingRoad->setText("");

    if(pjunction->GetJunctionConnectionCount()>0)
    {
        mpCBJunctionConnection->setCurrentIndex(0);
        onClickCBJunctionConnectionChange(0);
    }
}

void MainWindow::onClickCBJunctionConnectionChange(int index)
{
//    mpCBJunctionIncommingRoad->clear();
//    mpCBJunctionConnectingRoad->clear();
//    mpCBJunctionFromLane->clear();
//    mpCBJunctionToLane->clear();

    Junction * pjunction = mxodr.GetJunction(mpCBJunction->currentIndex());
    if(pjunction == NULL)
    {
        std::cout<<"MainWindow::onClickCBJunctionConnectionChange Junction NULL"<<std::endl;
        return;
    }

    if((index<0)||(index>= pjunction->GetJunctionConnectionCount()))
    {
//        std::cout<<"MainWindow::onClickCBJunctionConnectionChange out range."<<std::endl;
        return;
    }
    JunctionConnection * pJC = pjunction->GetJunctionConnection(index);

    if(pJC == NULL)
    {
        std::cout<<"MainWindow::onClickCBJunctionConnectionChange Junction Connection NULL"<<std::endl;
        return;
    }

    mpLabelJunctionIncommingRoad->setText(pJC->GetIncomingRoad().data());
    mpLabelJunctionContactPoint->setText(pJC->GetContactPoint().data());
    mpLabelJunctionConnectingRoad->setText(pJC->GetConnectingRoad().data());

    unsigned int i;
    unsigned int njunctionlanelinkcount = pJC->GetJunctionLaneLinkCount();
    mpCBJunctionFromTo->clear();
    for(i=0;i<njunctionlanelinkcount;i++)
    {
        char strout[255];
        JunctionLaneLink * pjll = pJC->GetJunctionLaneLink(i);
        snprintf(strout,255,"%d to %d",pjll->GetFrom(),pjll->GetTo());
        mpCBJunctionFromTo->addItem(strout);
    }
}

void MainWindow::onClickCBJunctionIncommingChange(int index)
{
    int i;
    int ncount = mxodr.GetRoadCount();
    if((index < 0)||(index>=ncount))
    {
        return;
    }
    Road * pRoad = mxodr.GetRoad(index);
    if(pRoad == NULL)
    {
        return;
    }
    LaneSection * pLS;
    if(mpCBJunctionContactPoint->currentIndex()  == 0)
    {
        pLS = pRoad->GetLaneSection(0);
    }
    else
    {
        pLS = pRoad->GetLaneSection(pRoad->GetLaneSectionCount()-1);
    }
    int nlanecount = pLS->GetLaneCount();
    mpCBJunctionFromLane->clear();
    for(i=0;i<nlanecount;i++)
    {
        Lane * pLane = pLS->GetLane(i);
        if(pLane->GetId() != 0)
        {
            mpCBJunctionFromLane->addItem(QString::number(pLane->GetId()));
        }
    }
}

void MainWindow::onClickCBJunctionConnectionroadChange(int index)
{
    int i;
    int ncount = mxodr.GetRoadCount();
    if((index < 0)||(index>=ncount))
    {
        return;
    }
    Road * pRoad = mxodr.GetRoad(index);
    if(pRoad == NULL)
    {
        return;
    }
    LaneSection * pLS;
        pLS = pRoad->GetLaneSection(0);
    int nlanecount = pLS->GetLaneCount();
    mpCBJunctionToLane->clear();
    for(i=0;i<nlanecount;i++)
    {
        Lane * pLane = pLS->GetLane(i);
        if(pLane->GetId() != 0)
        {
            mpCBJunctionToLane->addItem(QString::number(pLane->GetId()));
        }
    }
}

void MainWindow::onClickCreateJunction()
{
    mxodr.AddJunction("",QString::number(CreateJunctionID()).toStdString());
    updateJunction();
}

void MainWindow::onClickCreateJunctionLaneLink()
{
    Junction * pJunction= mxodr.GetJunction(mpCBJunction->currentIndex());
    if(pJunction == NULL)
    {
        QMessageBox::warning(this,"warn","can't find junction");
        return ;
    }
    Road * pFromRoad = mxodr.GetRoad(mpCBJunctionIncommingRoad->currentIndex());
    if(pFromRoad == NULL)
    {
        QMessageBox::warning(this,"warn","can't find incomming road.");
        return;
    }
    Road * pToRoad = mxodr.GetRoad(mpCBJunctionConnectingRoad->currentIndex());
    if(pToRoad == NULL)
    {
        QMessageBox::warning(this,"warn","can't find connecting road.");
        return;
    }
    string contacpoint = mpCBJunctionContactPoint->currentText().toStdString();
    JunctionConnection * pJC = 0;
    int njunctioncount = pJunction->GetJunctionConnectionCount();
    int i;
    for(i=0;i<njunctioncount;i++)
    {
        JunctionConnection * pJunCon = pJunction->GetJunctionConnection(i);
        if((pJunCon->GetIncomingRoad() == pFromRoad->GetRoadId())&&(pJunCon->GetContactPoint() == contacpoint)&&(pJunCon->GetConnectingRoad() == pToRoad->GetRoadId()))
        {
            pJC = pJunCon;
            break;
        }
    }
    bool bNewJC = false;
    if(pJC == 0)
    {
        int nnewJCid = pJunction->GetJunctionConnectionCount();
        int j;
        int njccount = pJunction->GetJunctionConnectionCount();
        bool bNotUse = true;
        do
        {
            for(j=0;j<njccount;j++)
            {
                if(nnewJCid == atoi(pJunction->GetJunctionConnection(j)->GetId().data()))
                {
                    bNotUse = false;
                    nnewJCid++;
                    break;
                }
            }
        }while(bNotUse == false);
        unsigned int addindex = pJunction->AddJunctionConnection(QString::number(nnewJCid).toStdString(),
                                               pFromRoad->GetRoadId(),
                                               pToRoad->GetRoadId(),
                                               contacpoint);
        pJC = pJunction->GetJunctionConnection(addindex);
        bNewJC = true;


    }

    int nfrom = mpCBJunctionFromLane->currentText().toInt();
    int nto = mpCBJunctionToLane->currentText().toInt();
    int njlcount = pJC->GetJunctionLaneLinkCount();
    for(i=0;i<njlcount;i++)
    {
        JunctionLaneLink * pjll = pJC->GetJunctionLaneLink(i);
        if((pjll->GetFrom()== nfrom)&&(pjll->GetTo() == nto))
        {
            QMessageBox::warning(this,"warn","this lane link exist.");
            return;
        }
    }
    unsigned int nadd = pJC->AddJunctionLaneLink(mpCBJunctionFromLane->currentText().toInt(),
                             mpCBJunctionToLane->currentText().toInt());

    char strout[255];
    JunctionLaneLink * pjll = pJC->GetJunctionLaneLink(nadd);
    snprintf(strout,255,"%d to %d",pjll->GetFrom(),pjll->GetTo());

    if(bNewJC)
    {
        mpCBJunctionConnection->addItem(pJC->GetId().data());
    }

    if(strncmp(contacpoint.data(),"start",255) == 0)
    {
        if(nfrom*nto<0)
        {
            pFromRoad->SetPredecessor("junction",pJunction->GetId(),contacpoint);
        }
        else
        {
            pFromRoad->SetSuccessor("junction",pJunction->GetId(),contacpoint);
        }
    }
    else
    {
        if(nfrom*nto <0)
        {
            pFromRoad->SetSuccessor("junction",pJunction->GetId(),contacpoint);
        }
        else
        {
            pFromRoad->SetPredecessor("junction",pJunction->GetId(),contacpoint);
        }
    }
    pToRoad->SetRoadJunction(pJunction->GetId());
}

void MainWindow::onClickDeleteJunctionLaneLink()
{
    Junction * pJunction= mxodr.GetJunction(mpCBJunction->currentIndex());
    if(pJunction == NULL)
    {
        QMessageBox::warning(this,"warn","can't find junction");
        return ;
    }

    if(pJunction->GetJunctionConnectionCount() == 0)
    {
        mxodr.DeleteJunction(mpCBJunction->currentIndex());
        updateJunction();
        return;
    }

    JunctionConnection * pJC = pJunction->GetJunctionConnection(mpCBJunctionConnection->currentIndex());
    if(pJC == NULL)
    {
        QMessageBox::warning(this,"warn","can't find junction connetion.");
        return;
    }

    if(pJC->GetJunctionLaneLinkCount() == 0)
    {
        pJunction->DeleteJunctionConnection(mpCBJunctionConnection->currentIndex());
        if(pJunction->GetJunctionConnectionCount() > 0)
            onClickCBJunctionChange(mpCBJunction->currentIndex());
        else
        {
            mxodr.DeleteJunction(mpCBJunction->currentIndex());
            updateJunction();
        }
        return;
    }

//    std::string strincommingroad = mpLabelJunctionIncommingRoad->text().toStdString();
//    std::string strconnectionroad = mpLabelJunctionConnectingRoad->text().toStdString();
//    std::string strcontact = mpLabelJunctionContactPoint->text().toStdString();
    int  nllindex = mpCBJunctionFromTo->currentIndex();
    pJC->DeleteJunctionLaneLink(nllindex);
    onClickCBJunctionConnectionChange(mpCBJunctionConnection->currentIndex());


}

void  MainWindow::onClickPreNxtRoadChange(int index)
{
    mpCBPreNxtRelLane->clear();
    Road * pRoad = mxodr.GetRoad(index);
    if(pRoad == NULL)
    {
        return;
    }
    LaneSection * pLS;
    if(strncmp(mpCBPreNxtConatact->currentText().toStdString().data(),"start",255)==0)
    {
        pLS = pRoad->GetLaneSection(0);
    }
    else
    {
        pLS = pRoad->GetLaneSection(pRoad->GetLaneSectionCount()-1);
    }
    if(pLS == NULL)
    {
        std::cout<<"MainWindow::onClickPreNxtRoadChange can't find lanesection"<<std::endl;
        return;
    }
    int nlanecount = pLS->GetLaneCount();
    int  i;
    for(i=0;i<nlanecount;i++)
    {
        Lane * pLane = pLS->GetLane(i);
        char strout[255];
        snprintf(strout,255,"%d %s",pLane->GetId(),pLane->GetType().data());
        mpCBPreNxtRelLane->addItem(strout);
    }
}

void MainWindow::onClickSetRoadPredecessor()
{
    Road * pRoad = mxodr.GetRoad(mpCBRoad->currentIndex());
    if(pRoad == NULL)
    {
        return;
    }
    Road * pOtherRoad = mxodr.GetRoad(mpCBPreNxtRoad->currentIndex());
    if(pOtherRoad == NULL)
    {
        return;
    }
    std::string strcontact = mpCBPreNxtConatact->currentText().toStdString();
    int curlane = mpCBRoadShowPre->currentIndex();
    int otherlane = mpCBPreNxtRelLane->currentIndex();

    pRoad->SetPredecessor("road",pOtherRoad->GetRoadId(),strcontact);
    LaneSection * pLS = pRoad->GetLaneSection(0);
    Lane * pLane =pLS->GetLane(curlane);
    LaneSection * pLSOther;
    if(strncmp(strcontact.data(),"start",0) == 0)
    {
        pLSOther = pOtherRoad->GetLaneSection(0);
    }
    else
    {
        pLSOther = pOtherRoad->GetLaneSection(pOtherRoad->GetLaneSectionCount()-1);
    }
    Lane * pLaneOther = pLSOther->GetLane(otherlane);
    if(pLaneOther == NULL)
    {
        QMessageBox::warning(this,"warn","no this lane.");
        return;
    }
    pLane->SetPredecessor(pLaneOther->GetId());
    onClickCBRoadChange(mpCBRoad->currentIndex());

}

void MainWindow::onClickSetRoadSuccessor()
{
    Road * pRoad = mxodr.GetRoad(mpCBRoad->currentIndex());
    if(pRoad == NULL)
    {
        return;
    }
    Road * pOtherRoad = mxodr.GetRoad(mpCBPreNxtRoad->currentIndex());
    if(pOtherRoad == NULL)
    {
        return;
    }
    std::string strcontact = mpCBPreNxtConatact->currentText().toStdString();
    int curlane = mpCBRoadShowNext->currentIndex();
    int otherlane = mpCBPreNxtRelLane->currentIndex();

    pRoad->SetSuccessor("road",pOtherRoad->GetRoadId(),strcontact);
    LaneSection * pLS = pRoad->GetLaneSection(pRoad->GetLaneSectionCount()-1);
    Lane * pLane =pLS->GetLane(curlane);
    LaneSection * pLSOther;
    if(strncmp(strcontact.data(),"start",0) == 0)
    {
        pLSOther = pOtherRoad->GetLaneSection(0);
    }
    else
    {
        pLSOther = pOtherRoad->GetLaneSection(pOtherRoad->GetLaneSectionCount()-1);
    }
    Lane * pLaneOther = pLSOther->GetLane(otherlane);
    if(pLaneOther == NULL)
    {
        QMessageBox::warning(this,"warn","no this lane.");
        return;
    }
    pLane->SetSuccessor(pLaneOther->GetId());
    onClickCBRoadChange(mpCBRoad->currentIndex());
}

void MainWindow::onClickDelRoadPredecessor()
{
    Road * pRoad = mxodr.GetRoad(mpCBRoad->currentIndex());
    if(pRoad == NULL)
    {
        return;
    }

    int curlane = mpCBRoadShowPre->currentIndex();

    LaneSection * pLS = pRoad->GetLaneSection(0);
    Lane * pLane =pLS->GetLane(curlane);
    if(pLane == 0)
    {
        return;
    }
    pLane->RemovePredecessor();
    int i;
    int nlanecount = pLS->GetLaneCount();
    bool bAllNotHavePre = true;
    for(i=0;i<nlanecount;i++)
    {
        Lane * pLa= pLS->GetLane(i);
        if(pLa->IsPredecessorSet())
        {
            bAllNotHavePre = false;
            break;
        }
    }
    if(bAllNotHavePre)
    {
        pRoad->RemovePredecessor();
    }
    onClickCBRoadChange(mpCBRoad->currentIndex());
}

void MainWindow::onClickDelRoadSuccessor()
{
    Road * pRoad = mxodr.GetRoad(mpCBRoad->currentIndex());
    if(pRoad == NULL)
    {
        return;
    }
    int curlane = mpCBRoadShowNext->currentIndex();
    LaneSection * pLS = pRoad->GetLaneSection(pRoad->GetLaneSectionCount()-1);
    Lane * pLane =pLS->GetLane(curlane);
    if(pLane == 0)
    {
        return;
    }
    pLane->RemoveSuccessor();
    int i;
    int nlanecount = pLS->GetLaneCount();
    bool bAllNotHaveNxt = true;
    for(i=0;i<nlanecount;i++)
    {
        Lane * pLa= pLS->GetLane(i);
        if(pLa->IsSuccessorSet())
        {
            bAllNotHaveNxt = false;
            break;
        }
    }
    if(bAllNotHaveNxt)
    {
        pRoad->RemoveSuccessor();
    }
    onClickCBRoadChange(mpCBRoad->currentIndex());
}

void MainWindow::onClickAutoConnect()
{
    AutoConnect pAC(&mxodr);
    pAC.Connect();
    updateJunction();
}


void MainWindow::on_actionLoad_triggered()
{
    onClickLoad();
}

void MainWindow::on_actionSave_triggered()
{
    onClickSave();
}

void MainWindow::on_actionAutoConnect_triggered()
{
    onClickAutoConnect();
}

void MainWindow::on_actionSet_Speed_triggered()
{
    SpeedDialog sd(&mxodr,this);
    int res = sd.exec();
}

void MainWindow::closeEvent(QCloseEvent *event)
{
    if(mxodr.GetRoadCount() == 0)
    {
        event->accept(); // 接受退出信号，程序退出
        return;
    }
    QMessageBox::StandardButton button;
    button=QMessageBox::question(this,tr("退出程序"),QString(tr("确认退出程序")),QMessageBox::Yes|QMessageBox::No);
    if(button==QMessageBox::No)
    {
        event->ignore(); // 忽略退出信号，程序继续进行
    }
    else if(button==QMessageBox::Yes)
    {
        event->accept(); // 接受退出信号，程序退出
    }
}