modularization/src/tool/map_lanetoxodr/xvmainwindow2.cpp

#include "xvmainwindow2.h"
#include "ui_xvmainwindow2.h"

#include <QMessageBox>
#include <QFileDialog>

#include <string.h>

#include "xodrfunc.h"
#include "roaddigit.h"
#include "xodrscenfunc.h"


#define VIEW_WIDTH 5000
#define VIEW_HEIGHT 5000

static bool IsNaN(double dat)
{
 qint64 & ref=*(qint64 *)&dat;
 return (ref&0x7FF0000000000000) == 0x7FF0000000000000 && (ref&0xfffffffffffff)!=0;
}


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

    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)));
    connect(myview,SIGNAL(beishuchange(double)),this,SLOT(onbeishuchange(double)));

    myview->setCacheMode(myview->CacheBackground);

    mpscene = new  QGraphicsScene(0,0,VIEW_WIDTH,VIEW_HEIGHT);
    mpscene->setBackgroundBrush(Qt::darkGreen);

    myview->setScene(mpscene);
    mfViewMoveX = VIEW_WIDTH/2.0;
    mfViewMoveY = (-1.0)*VIEW_HEIGHT/2.0;

    connect(&mFileDialog,SIGNAL(accepted()),this,SLOT(onFileOpen()));

    setWindowTitle("ADC OpenDrive Viewer2 (20230201)");
}

XVMainWindow::~XVMainWindow()
{

    delete mpscene;
    delete myview;
    delete ui;
}

void XVMainWindow::resizeEvent(QResizeEvent *event)
{
    qDebug("resize");
    QSize sizemain = ui->centralwidget->size();
    qDebug("size x = %d y=%d",sizemain.width(),sizemain.height());
    mfViewWidth = sizemain.width();
    mfViewHeight = sizemain.height() - 30;
    myview->setGeometry(0,30,sizemain.width(),sizemain.height()-30);
}

void XVMainWindow::on_actionLoad_triggered()
{

    if(mxodr.GetRoadCount() > 0)
    {
        QMessageBox::StandardButton button;
        char strquest[256];
        snprintf(strquest,256,"Do you Want to Load New File ?");
        button=QMessageBox::question(this,"Move",strquest,QMessageBox::Yes|QMessageBox::No);
        if(button==QMessageBox::No)
        {
            return;
        }
        else if(button==QMessageBox::Yes)
        {

        }
    }


#ifndef ANDROID
    QString strpath = QFileDialog::getOpenFileName(this,"Load XODR",".","*.xodr");
    if(strpath.isEmpty())return;
    LoadXODR(strpath);
    OpenDrive * pxodr = &mxodr;
    UpdateScene();
#else
//    QMessageBox::warning(this,"warning","no file dialog.",QMessageBox::YesAll);
//    QString strpath = "/storage/emulated/0/map.xodr";
    mFileDialog.open();

#endif

}

void XVMainWindow::LoadXODR(QString strpath)
{
    mxodr.Clear();
    OpenDrive * pxodr = &mxodr;  //because add to xodr,so don't delete
    OpenDriveXmlParser x(pxodr);
    if(!x.ReadFile(strpath.toStdString()))
    {
        QMessageBox::warning(this,"warn","Can't  load xodr file.");
        return;
    }

    mvectorRoadSample.clear();

    int nroadnum = static_cast<int>(pxodr->GetRoadCount()) ;
    int i;
    double froadlen = 0;
    double fxmin,fxmax,fymin,fymax;
    fxmin = std::numeric_limits<double>::max() *(1.0);
    fxmax = std::numeric_limits<double>::max()*(-1.0);
    fymin = std::numeric_limits<double>::max() *(1.0);
    fymax = std::numeric_limits<double>::max()*(-1.0);
    for(i=0;i<nroadnum;i++)
    {
        Road * pRoad = pxodr->GetRoad(i);
        if(IsNaN(pRoad->GetRoadLength()))
        {
            pxodr->DeleteRoad(i);
            i--;
            nroadnum--;
            qDebug("delete road %s because length is NaN",pRoad->GetRoadId().data());
        }
        else
        {
            froadlen = froadlen + pRoad->GetRoadLength();

            iv::RoadSample xSample(pRoad);
            mvectorRoadSample.push_back(xSample);

            if(pRoad->GetGeometryBlockCount()>0)
            {
                double fx,fy;
                fx = pRoad->GetGeometryBlock(0)->GetGeometryAt(0)->GetX();
                fy = pRoad->GetGeometryBlock(0)->GetGeometryAt(0)->GetY();
                if(fx>fxmax)fxmax = fx;
                if(fx<fxmin)fxmin = fx;
                if(fy>fymax)fymax = fy;
                if(fy<fymin)fymin = fy;
            }
        }
    }

    double fmovex = 0;
    double fmovey = 0;
    if(((fxmax>1000)&&(fxmin>1000))||((fxmax<-1000)&&(fxmin<-1000)))
    {
        fmovex = (fxmax + fxmin)/2.0;
    }

    if(((fymax>1000)&&(fymin>1000))||((fymax<-1000)&&(fymin<-1000)))
    {
        fmovey = (fymax + fymin)/2.0;
    }

    mfViewMoveX = mfViewMoveX - fmovex;
    mfViewMoveY = mfViewMoveY - fmovey;

    qDebug("view move is %f",mfViewMoveX,mfViewMoveY);

    char strout[256];
    snprintf(strout,256,"Road count is %d. Total Len is %f",mxodr.GetRoadCount(),froadlen);
    QMessageBox::information(this,"Info",strout,QMessageBox::YesAll);
}

void XVMainWindow::UpdateScene()
{
    int i;
    int nsize = mvectorviewitem.size();
    for(i=0;i<nsize;i++)
    {
        mpscene->removeItem(mvectorviewitem.at(i));
        delete mvectorviewitem.at(i);
    }
    mvectorviewitem.clear();

    nsize = static_cast<int>(mvectorRoadSample.size()) ;

    for(i=0;i<nsize;i++)
    {
        std::vector<QGraphicsPathItem *> xvectorlanepath = xodrscenfunc::GetRoadViewItem(&(mvectorRoadSample[i]));
        int j;
        int ncount = xvectorlanepath.size();
        for(j=0;j<ncount;j++)
        {
            QGraphicsPathItem * pitem = xvectorlanepath[j];
//            pitem->setPos(mfViewMoveX +VIEW_WIDTH/2,-mfViewMoveY+VIEW_HEIGHT/2);
            pitem->setPos(mfViewMoveX,-mfViewMoveY);

            mpscene->addItem(pitem);
            mvectorviewitem.push_back(pitem);
        }
    }

    return;

    nsize = mxodr.GetRoadCount();


    std::vector<RoadDigit> xvectorrd;
    for(i=0;i<nsize;i++)
    {
        RoadDigit xrd(mxodr.GetRoad(i),5.0);  //Space Must <= Broken dot dis
        xvectorrd.push_back(xrd);
    }
    for(i=0;i<nsize;i++)
    {
        std::vector<QGraphicsPathItem *> xvectorlanepath = xodrscenfunc::GetRoadLaneItem(&(xvectorrd[i]));
        int j;
        int ncount = xvectorlanepath.size();
        for(j=0;j<ncount;j++)
        {
            QGraphicsPathItem * pitem = xvectorlanepath[j];
//            pitem->setPos(mfViewMoveX +VIEW_WIDTH/2,-mfViewMoveY+VIEW_HEIGHT/2);
            pitem->setPos(mfViewMoveX,-mfViewMoveY);
            mpscene->addItem(pitem);
            mvectorviewitem.push_back(pitem);
        }
    }

    for(i=0;i<nsize;i++)
    {
        std::vector<QGraphicsPathItem *> xvectormarkpath = xodrscenfunc::GetRoadMarkItem(&(xvectorrd[i]));
        int j;
        int ncount = xvectormarkpath.size();
        for(j=0;j<ncount;j++)
        {
            QGraphicsPathItem * pitem = xvectormarkpath[j];
 //           pitem->setPos(mfViewMoveX +VIEW_WIDTH/2,-mfViewMoveY+VIEW_HEIGHT/2);
            pitem->setPos(mfViewMoveX,-mfViewMoveY);
            mpscene->addItem(pitem);
            mvectorviewitem.push_back(pitem);
        }
    }
}


void XVMainWindow::on_actionZoom_In_triggered()
{
    myview->zoomIn();
}

void XVMainWindow::on_actionZoom_Out_triggered()
{
    myview->zoomOut();
}

void XVMainWindow::on_actionZoom_One_triggered()
{
    myview->zoomone();
}

void XVMainWindow::onClickXY(double x, double y)
{
    double selx,sely;
    double lon,lat;


//    qDebug("  x is %f y is %f",x,y);
//    selx = (x  - (1.0/mfbeishu) * VIEW_WIDTH/2);
//    sely = (y  - (1.0/mfbeishu) *VIEW_HEIGHT/2) * (-1);


//    qDebug("beishu is %f ",mfbeishu);
//    selx = (x  - (1.0/mfbeishu) *(mfViewWidth/2) + (mfViewWidth/2)) ;
//    sely = (y  - (1.0/mfbeishu) *(mfViewHeight/2) + (mfViewHeight/2)) * (-1);

    selx = x - VIEW_WIDTH/2;
    sely = (y - VIEW_HEIGHT/2)*(-1);


    mfselx = selx *1.0/mfbeishu ;
    mfsely = sely *1.0/mfbeishu;


//    selx = x;
//    sely = y;
//    mfselx = selx;
//    mfsely = sely;

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

    double rel_x,rel_y;

    rel_x = selx - mfViewMoveX + VIEW_WIDTH/2 ;
    rel_y = sely - mfViewMoveY - VIEW_HEIGHT/2;

    qDebug("selx is %f sely is %f ",rel_x,rel_y);

    unsigned int nRoadCount = static_cast<unsigned int>(mvectorRoadSample.size());
    unsigned int i;


    int64_t time1 = std::chrono::system_clock::now().time_since_epoch().count();
    bool bFind = false;
    char strout[1000];
    for(i=0;i<nRoadCount;i++)
    {
        double s,t;
        int nlane;
        std::string strlanetype;
        if(mvectorRoadSample[i].PointInRoad(rel_x,rel_y,s,t,nlane,strlanetype))
        {
            std::cout<<" In Road"<<mvectorRoadSample[i].GetRoadID()<<" s:"<<s<<" t:"<<t<<" nlane:"<<nlane<<std::endl;

            if(bFind == false)
            {
                snprintf(strout,1000,"x:%7.3f y:%7.3f |Road:%s s:%f t:%f nlane:%d %s",
                         rel_x,rel_y,mvectorRoadSample[i].GetRoadID().data(),s,t,nlane,strlanetype.data());
     //       mpLabel_Status->setText(strout);
                bFind = true;
            }
            else
            {
                char strtem[1000];
                snprintf(strtem,1000,"| %s %d",mvectorRoadSample[i].GetRoadID().data(),nlane);
                strncat(strout,strtem,1000);
            }
        }
    }

    if(bFind == false)
    {
        snprintf(strout,1000,"x:%7.3f y:%7.3f",rel_x,rel_y);
    }

    ui->statusbar->showMessage(strout,10000);
    int64_t time2 = std::chrono::system_clock::now().time_since_epoch().count();
    std::cout<<" find use time: "<<(time2 - time1)<<std::endl;

    return;


    Road * pRoad = 0;
    GeometryBlock * pgeob;
    double fdis,nearx,neary,hdg;
    double fs;
    int nlane;
    if(xodrfunc::GetNearPoint(rel_x,rel_y,&mxodr,&pRoad,&pgeob,fdis,nearx,neary,hdg,50,&fs,&nlane) == 0)
    {
        qDebug("s:%f dis is %f nlane is %d",fs,fdis,nlane);
        char strout[1000];
        snprintf(strout,1000,"Road:%s s:%f dis:%f nlane:%d",pRoad->GetRoadId().data(),fs,fdis,nlane);
 //       mpLabel_Status->setText(strout);
        ui->statusbar->showMessage(strout,10000);

    }
    else
    {
        char strout[1000];
        snprintf(strout,1000,"Click x:%f y:%f",rel_x,rel_y);
        ui->statusbar->showMessage(strout,10000);
        qDebug("not found near road.");
    }
}

void XVMainWindow::on_actionSet_Move_triggered()
{
    QMessageBox::StandardButton button;
    char strquest[256];
    snprintf(strquest,256,"Want to Move Center to x:%f y:%f ?",-(mfViewMoveX - mfselx),-(mfViewMoveY-mfsely));
    button=QMessageBox::question(this,"Move",strquest,QMessageBox::Yes|QMessageBox::No);
    if(button==QMessageBox::No)
    {
        return;
    }
    else if(button==QMessageBox::Yes)
    {

    }

    mfViewMoveX = mfViewMoveX - mfselx;
    mfViewMoveY = mfViewMoveY - mfsely;

    int nsize = mvectorviewitem.size();
    int i;
    for(i=0;i<nsize;i++)
    {
        mpscene->removeItem(mvectorviewitem.at(i));
    }
    for(i=0;i<nsize;i++)
    {
//        mvectorviewitem.at(i)->setPos(mfViewMoveX +VIEW_WIDTH/2,-mfViewMoveY+VIEW_HEIGHT/2);
        mvectorviewitem.at(i)->setPos(mfViewMoveX ,-mfViewMoveY);
        mpscene->addItem(mvectorviewitem.at(i));
    }

    myview->zoomIn();
    myview->zoomOut();
}

void XVMainWindow::on_actionReset_Move_triggered()
{
    mfViewMoveX = 0;
    mfViewMoveY = 0;
    int nsize = mvectorviewitem.size();
    int i;

    for(i=0;i<nsize;i++)
    {
        mpscene->removeItem(mvectorviewitem.at(i));
    }

    for(i=0;i<nsize;i++)
    {
        mvectorviewitem.at(i)->setPos(mfViewMoveX ,-mfViewMoveY);
 //       mvectorviewitem.at(i)->setPos(mfViewMoveX +VIEW_WIDTH/2,-mfViewMoveY+VIEW_HEIGHT/2);
        mpscene->addItem(mvectorviewitem.at(i));
    }


    myview->zoomIn();
    myview->zoomOut();
}

void XVMainWindow::paintEvent(QPaintEvent * event)
{
    if(mbRefresh)
    {
        myview->setScene(mpscene);
        mbRefresh = false;
    }
}

void XVMainWindow::onFileOpen()
{
    QString strpath = mFileDialog.currentFile();
    if(strpath.isEmpty())return;
    LoadXODR(strpath);
    OpenDrive * pxodr = &mxodr;
    UpdateScene();
}

void XVMainWindow::on_actionHelp_triggered()
{
    QString helpinfo = tr("Load:加载文件（后缀名为.xodr）\nZoom In:放大\nZomm Out:缩小\nZoom One:恢复默认视图\nSet Move:移动显示中心\n"
                          "Reset Move:恢复默认显示中心\n\ntips: 在屏幕上双击选择道路（查看道路id)或者选择点（用来移动中心点 \n      "
                          "在屏幕上可以移动查看区域");
    QMessageBox::information(this,"Help",helpinfo,QMessageBox::Yes);
}

void XVMainWindow::onbeishuchange(double fbeishu)
{
    mfbeishu = fbeishu;
}