#include "ivmapview.h"

#include <QFile>

#define VIEW_WIDTH  1100
#define VIEW_HEIGHT 900

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

#include "pos_def.h"

extern std::vector<iv::pos_def> gvectorpos;
extern std::string gstrmode;

#include <math.h>
//高斯投影由经纬度(Unit:DD)反算大地坐标(含带号，Unit:Metres)
void GaussProjCal(double longitude, double latitude, double *X, double *Y)
{
    int ProjNo = 0; int ZoneWide; ////带宽
    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
    double a, f, e2, ee, NN, T, C, A, M, iPI;
    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
    ZoneWide = 6; ////6度带宽
    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
                                    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
    ProjNo = (int)(longitude / ZoneWide);
    longitude0 = ProjNo * ZoneWide + ZoneWide / 2;
    longitude0 = longitude0 * iPI;
    latitude0 = 0;
    longitude1 = longitude * iPI; //经度转换为弧度
    latitude1 = latitude * iPI; //纬度转换为弧度
    e2 = 2 * f - f * f;
    ee = e2 * (1.0 - e2);
    NN = a / sqrt(1.0 - e2 * sin(latitude1)*sin(latitude1));
    T = tan(latitude1)*tan(latitude1);
    C = ee * cos(latitude1)*cos(latitude1);
    A = (longitude1 - longitude0)*cos(latitude1);
    M = a * ((1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256)*latitude1 - (3 * e2 / 8 + 3 * e2*e2 / 32 + 45 * e2*e2
        *e2 / 1024)*sin(2 * latitude1)
        + (15 * e2*e2 / 256 + 45 * e2*e2*e2 / 1024)*sin(4 * latitude1) - (35 * e2*e2*e2 / 3072)*sin(6 * latitude1));
    xval = NN * (A + (1 - T + C)*A*A*A / 6 + (5 - 18 * T + T * T + 72 * C - 58 * ee)*A*A*A*A*A / 120);
    yval = M + NN * tan(latitude1)*(A*A / 2 + (5 - T + 9 * C + 4 * C*C)*A*A*A*A / 24
        + (61 - 58 * T + T * T + 600 * C - 330 * ee)*A*A*A*A*A*A / 720);
    X0 = 1000000L * (ProjNo + 1) + 500000L;
    Y0 = 0;
    xval = xval + X0; yval = yval + Y0;
    *X = xval;
    *Y = yval;
}


static bool LoadXODR(std::string strpath,OpenDrive & xxodr)
{
    OpenDriveXmlParser xp(&xxodr);
    xp.ReadFile(strpath);



    return  true;

}

static int getmnfac(OpenDrive & mxodr,double & fmovex,double & fmovey)
{
    int nrtn = 1;

    int i;
    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);
    bool bHaveRealRoad = false;
    for(i=0;i<mxodr.GetRoadCount();i++)
    {
        int j;

        Road * pRoad = mxodr.GetRoad(i);
        if(pRoad->GetRoadLength()<0.1)
        {
            continue;
        }
        bHaveRealRoad = true;
        for(j=0;j<mxodr.GetRoad(i)->GetGeometryBlockCount();j++)
        {
            GeometryBlock * pgeob = pRoad->GetGeometryBlock(j);
            double x,y;

            RoadGeometry * pg;
            pg = pgeob->GetGeometryAt(0);

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

            if(x>fxmax)fxmax = x;
            if(x<fxmin)fxmin = x;
            if(y>fymax)fymax = y;
            if(y<fymin)fymin = y;

        }

        double x,y,hdg;

        int nco = pRoad->GetGeometryCoords(pRoad->GetRoadLength()-0.1,x,y,hdg);
        if(nco >= 0)
        {
            if(x>fxmax)fxmax = x;
            if(x<fxmin)fxmin = x;
            if(y>fymax)fymax = y;
            if(y<fymin)fymin = y;
        }
    }

    if(bHaveRealRoad == false)
    {
 //       std::cout<<"No Real Road."<<std::endl;
        return 1.0;
    }

    fmovex = 0;
    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;
    }

    double fabsxmax = fabs(fxmax);
    if(fabsxmax < fabs(fxmin))fabsxmax = fabs(fxmin);
    double fabsymax = fabs(fymax);
    if(fabsymax < fabs(fymin))fabsymax = fabs(fymin);
    int nfacx=1;
    if(fabsxmax!= 0)
    {
        nfacx = (VIEW_WIDTH/2)/(fabsxmax*1.2);
    }
    int nfacy = 1;
    if(fabsymax != 0)
    {
        nfacy = (VIEW_HEIGHT/2)/(fabsymax*1.2);
    }
    if(nfacx<1)nfacx = 1;
    if(nfacy<1)nfacy = 1;



    if(nfacx < nfacy)nrtn = nfacx;
    else nrtn = nfacy;

    if(nrtn<=1)
    {
        nrtn = 1;
        return nrtn;
    }

    if((nrtn >(VIEW_WIDTH/1600))&&(VIEW_WIDTH>1600))
    {
        nrtn = nrtn/(VIEW_WIDTH/1600);
    }



    return nrtn;
}


ivmapview::ivmapview()
{
    image = new QImage(VIEW_WIDTH, VIEW_HEIGHT, QImage::Format_RGB32);//画布的初始化大小设为300*300，使用32位颜色
 //   mimagepaint = new QImage(VIEW_WIDTH, VIEW_HEIGHT, QImage::Format_RGB32);
    painter = new QPainter(image);
    painter->end();
    std::vector<iv::ObstacleBasic> * pobs = new std::vector<iv::ObstacleBasic>();
    mobs.reset(pobs);
    mnavigation_data.clear();



#ifdef Android
    LoadXODR("/storage/emulated/0//map.xodr",mxodr);
#else
    if(gstrmode == "false")
        LoadXODR("./map.xodr",mxodr);
    else
    {
        char * strhome = getenv("HOME");
        char strmappath[256];
        snprintf(strmappath,256,"%s/map/map.xodr",strhome);
        LoadXODR(strmappath,mxodr);
    }
#endif


//    qDebug("run hear.");

    double fmovex,fmovey;
    mnfac = getmnfac(mxodr,fmovex,fmovey);




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

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

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

//    mnfac = 100;
//    mnfac = 20;

    unsigned short int revMajor,revMinor;
    std::string name,date;
    float version;
    double north,south,east,west,lat0,lon0,hdg0;
    if(mxodr.GetHeader() != 0)
    {
        mxodr.GetHeader()->GetAllParams(revMajor,revMinor,name,version,date,north,south,east,west,lat0,lon0,hdg0);
        glat0 = lat0;
        glon0 = lon0;
    }
    GaussProjCal(glon0,glat0,&mx0,&my0);
}

void ivmapview::run()
{
    while(!QThread::isInterruptionRequested())
    {
        bool bNeedUpdate = false;
        if(mnMapMode == 0 )
        {
        qint64 xTime = QDateTime::currentMSecsSinceEpoch();
        if(mnavigation_data.size()> 0)
        {
            if((xTime - mnTimeGPSUpdate)<3000)
            {
                bNeedUpdate = true;
            }
        }

        if((xTime - mnTimeLidarOBS)<3000)
        {
            bNeedUpdate = true;
        }

        if((xTime-mnTimeRADAR)<3000)
        {
            bNeedUpdate = true;
        }

        if(bNeedUpdate)paint();
        msleep(100);
        }
        else
        {
            mmutexxodrfile.lock();
            if(mbNeedReload)
            {
                mxodr.Clear();
                LoadXODR("./map.xodr",mxodr);
                mbNeedReload = false;
            }
            mmutexxodrfile.unlock();
            paintxodr();
            msleep(100);
            std::cout<<"paint."<<std::endl;
        }
    }
}


void ivmapview::paintxodr()
{
    QTime x;
    x.start();
    //    qDebug("painter.");


    qint64 xTime = QDateTime::currentMSecsSinceEpoch();
    std::vector<iv::MAP_GPS_INS> xnavigation_data;
    iv::gps::gpsimu xgpsimu;
    mMutexMap.lock();
    xnavigation_data = mnavigation_data;
    mMutexMap.unlock();

     mMutexPaint.lock();

    painter->begin(image);

    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;
    for(i=0;i<mxodr.GetRoadCount();i++)
    {
        int j;
        Road * pRoad = mxodr.GetRoad(i);
        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;
            GeometryPoly3 *ppoly;
            double rel_x,rel_y,rel_hdg;
            pg = pgeob->GetGeometryAt(0);

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

            switch (pg->GetGeomType()) {
            case 0:
                x = x + mfViewMoveX;
                y = y + mfViewMoveY;
                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;
                    x = x + mfViewMoveX;
                    y = y + mfViewMoveY;
                    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);
                    }
                    x_draw = x_draw + mfViewMoveX;
                    y_draw = y_draw + mfViewMoveY;
                    painter->drawPoint(x_draw * mnfac ,y_draw * mnfac *(-1));
                }
            }
                break;
            case 3:
            {
                painter->setPen(Qt::red);
                ppoly = (GeometryPoly3 *)pg;
                x = pg->GetX();
                y = pg->GetY();
                double A,B,C,D;
                A = ppoly->GetA();
                B = ppoly->GetB();
                C = ppoly->GetC();
                D = ppoly->GetD();
                const double steplim = 0.1;
                double du = steplim;
                double u = 0;
                double v = 0;
                double oldx,oldy;
                oldx = x;
                oldy = y;
                double xstart,ystart;
                xstart = x;
                ystart = y;
                double hdgstart = ppoly->GetHdg();
                double flen = 0;
                while(flen < ppoly->GetLength())
                {
                    //                        double fdis = 0;
                    //                        v = A + B*u + C*u*u + D*u*u*u;
                    //                        x = xstart + u*cos(hdgstart) - v*sin(hdgstart);
                    //                        y = ystart + u*sin(hdgstart) + v*cos(hdgstart);
                    //                        fdis = sqrt(pow(x- oldx,2)+pow(y-oldy,2));
                    //                        oldx = x;
                    //                        oldy = y;
                    //                        if(fdis>(steplim*2.0))du = du/2.0;
                    //                        flen = flen + fdis;
                    //                        u = u + du;
                    double fHdg;
                    ppoly->GetCoords(flen,x,y,fHdg);
                    //                       std::cout<<" x: "<<x<<" y:"<<y<<std::endl;
                    x = x + mfViewMoveX;
                    y = y + mfViewMoveY;
                    painter->drawPoint((int)(x*mnfac),(int)(y*(-1.0*mnfac)));
                    flen = flen + steplim;
                }
//                painter->setPen(Qt::blue);
            }
                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 fhdg;
                    ppp3->GetCoords(ppp3->GetS() + s,x,y,fhdg);
//                    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();
                    x = x + mfViewMoveX;
                    y = y + mfViewMoveY;
                    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)));

        }
    }


    QPen pen;
    pen.setWidth(3);
    pen.setColor(Qt::green);
    painter->setPen(pen);

    double x0,y0;
    GaussProjCal(glon0,glat0,&x0,&y0);
    int nMapSize = xnavigation_data.size();
    for(i=0;i<nMapSize;i++)
    {
        double x,y;
//        GaussProjCal(xnavigation_data.at(i).gps_lng,xnavigation_data.at(i).gps_lat,&x,&y);
        x = xnavigation_data.at(i).gps_x;
        y = xnavigation_data.at(i).gps_y;
        x = x-x0;
        y= y-y0;

        x = x + mfViewMoveX;
        y = y + mfViewMoveY;
        painter->drawPoint((int)(x*mnfac),(int)(y*(-1.0*mnfac)));
    }

    painter->setPen(Qt::blue);


    if((xTime - mnTimeGPSUpdate)<3000)
    {
        mMutexGPS.lock();
        xgpsimu.CopyFrom(mgpsimu);
        mMutexGPS.unlock();

        painter->setPen(Qt::green);
        QPoint xP[4];

        double x[4],y[4];
        double x0[4],y0[4];
        double xu = mnMarkSize*3;
        x0[0] = -xu;y0[0] = -xu;
        x0[1] = xu*2; y0[1] = 0;
        x0[2] = -xu;y0[2] =xu;
        x0[3] = 0; y0[3] = 0;

        double xnow,ynow,hdgnow;
        hdgnow = (90- xgpsimu.heading()) * M_PI/180.0;
        GaussProjCal(xgpsimu.lon(),xgpsimu.lat(),&xnow,&ynow);
        double xz,yz;
        GaussProjCal(glon0,glat0,&xz,&yz);
        xnow = xnow - xz;
        ynow = ynow - yz;

        double hr= hdgnow;
        for(i=0;i<4;i++)
        {
            x[i] = x0[i]*cos(hr) +y0[i]*sin(hr);
            y[i] = y0[i]*cos(hr) - x0[i]*sin(hr);
            xP[i].setX((xnow + mfViewMoveX)*mnfac  + x[i]);
            xP[i].setY((ynow + mfViewMoveY)*mnfac*(-1) + y[i]);
        }

        //        painter->drawRect(mfManualX*mnfac-5,mfManualY*mnfac*(-1)-5,10,10);

        painter->drawPolygon(xP,4);
        painter->setPen(Qt::black);
    }

    int nstationsize = gvectorpos.size();

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

        double xpos,ypos;
        GaussProjCal(gvectorpos[i].mflon,gvectorpos[i].mflat,&xpos,&ypos);
        double xz,yz;
        GaussProjCal(glon0,glat0,&xz,&yz);

        xpos = xpos - xz;
        ypos = ypos - yz;


        painter->setPen(Qt::red);
        painter->setBrush(Qt::red)
;        if(i != mnStationIndex)
            painter->drawEllipse(QPoint((xpos + mfViewMoveX)*mnfac ,(ypos + mfViewMoveX)*mnfac*(-1)),mnMarkSize,mnMarkSize);
        else
            painter->drawEllipse(QPoint((xpos + mfViewMoveX)*mnfac ,(ypos + mfViewMoveX)*mnfac*(-1)),2*mnMarkSize,2*mnMarkSize);

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




    painter->end();

    mMutexPaint.unlock();
    mbImageUpdate = true;

    qDebug("time is %d",x.elapsed());
//    std::cout<<" draw map use: "<<(QDateTime::currentMSecsSinceEpoch() - xTime)<<std::endl;

}

void ivmapview::paint()
{

    qint64 xTime = QDateTime::currentMSecsSinceEpoch();
    std::vector<iv::MAP_GPS_INS> xnavigation_data;
    iv::gps::gpsimu xgpsimu;
    mMutexMap.lock();
    xnavigation_data = mnavigation_data;
    mMutexMap.unlock();




    mMutexPaint.lock();
    painter->begin(image);
    image->fill(QColor(208,208,208));//对画布进行填充
    painter->setRenderHint(QPainter::Antialiasing, true);//设置反锯齿模式，好看一点

    int pointx = 450, pointy = 700;//确定坐标轴起点坐标，这里定义(35,280)

    QPen pen, penPoint;

    //先绘制车位置点及框图
    static const QPointF points1[2] = { QPointF(300, 700), QPointF(600, 700) };
    static const QPointF points2[2] = { QPointF(450, 0), QPointF(450, 900) };

    penPoint.setColor(Qt::red);
    penPoint.setWidth(2);
    painter->setPen(penPoint);
    painter->drawPoint(pointx, pointy);
    painter->drawPolyline(points1, 2);
    painter->drawPolyline(points2, 2);

//    std::cout<<"map size is "<<xnavigation_data.size()<<std::endl;
    if((xnavigation_data.size()>0)&&((xTime - mnTimeGPSUpdate)<3000))
    {
        mMutexGPS.lock();
        xgpsimu.CopyFrom(mgpsimu);
        mMutexGPS.unlock();



        double * x0, * y0, * lng, * x1, * y1, * x2, * y2;
        std::shared_ptr<double> ptrx0,ptry0,ptrlng,ptrx1,ptry1,ptrx2,ptry2;
        int nmapsize = xnavigation_data.size();
        x0 = new double[nmapsize];
        y0 = new double[nmapsize];
        lng = new double[nmapsize];
        x1 = new double[nmapsize];
        y1 = new double[nmapsize];
        x2 = new double[nmapsize];
        y2 = new double[nmapsize];
        ptrx0.reset(x0);
        ptry0.reset(y0);
        ptrlng.reset(lng);
        ptrx1.reset(x1);
        ptry1.reset(y1);
        ptrx2.reset(x2);
        ptry2.reset(y2);

        double xx = 0, yy = 0;
        double sumx = 0, sumy = 0;//, ave_x = 0, ave_y = 0;
        int sizeN = xnavigation_data.size();
        //int max_x_pos = 0, min_x_pos = 0, max_y_pos = 0, min_y_pos = 0;
        int x_max = 0, y_max = 0;//数组里的最大值
        int x_min = 0x3f3f3f3f, y_min = 0x3f3f3f3f;//inf为 #define inf 0x3f3f3f3f
        double k1, k2;


  //      qDebug("y: %f ",xnavigation_data[1].gps_y);
        //路径点的预处理
        for (int i = 0; i < sizeN; i++)
        {
            x0[i] = xnavigation_data[i].gps_x;
            y0[i] = xnavigation_data[i].gps_y;
            lng[i] = xnavigation_data[i].ins_heading_angle;
            sumx = sumx + xnavigation_data[i].gps_x;
            sumy = sumy + xnavigation_data[i].gps_y;
            if (x0[i] > x_max) {
                x_max = x0[i];
                //max_x_pos = i;
            }
            if (x0[i] < x_min) {
                x_min = x0[i];
                //min_x_pos = i;
            }

            if (y0[i] > y_max) {
                y_max = y0[i];
                //max_y_pos = i;
            }
            if (y0[i] < y_min) {
                y_min = y0[i];
                //min_y_pos = i;
            }
        }
        //ave_x = sumx / sizeN;
        //ave_y = sumy / sizeN;

        //获取到实时 GPS信息，并做路径点的显示更新
        double gps_x = 0.0,gps_y = 0.0;
//        if(DataUI->mInfo.gps_lng!=0.0&&DataUI->mInfo.gps_lat != 0.0)
//        {
//            GaussProjCal(DataUI->mInfo.gps_lng, DataUI->mInfo.gps_lat, &gps_x, &gps_y);
//        }
//        if (gps_x == 0)
//        {
//            painter->drawText(rect(), Qt::AlignLeft, QStringLiteral("等待车辆实时GPS位置信息"));
//        }
//        else
//        {
//            x0[0] = gps_x;
//            y0[0] = gps_y;
//            lng[0] = DataUI->mInfo.gps_inshead;
//        }
        GaussProjCal(xgpsimu.lon(), xgpsimu.lat(), &gps_x, &gps_y);
 //       qDebug("now lon:%f lat:%f %f %f ",xgpsimu.lon(),xgpsimu.lat(),xnavigation_data[0].gps_lng,xnavigation_data[0].gps_lat);
        x0[0] = gps_x;
        y0[0] = gps_y;
        lng[0] = xgpsimu.heading();

//        qDebug("%f %f %f %f",xnavigation_data[1].gps_x, xnavigation_data[1].gps_y,xnavigation_data[0].gps_lng,xnavigation_data[0].gps_lat);
        //根据标定原点的选取，对路径点进行转化
        for (int i = 1; i < sizeN; i++)
        {
            x0[i] = x0[i] - x0[0];
            y0[i] = y0[i] - y0[0];
            xx = x0[i];
            yy = y0[i];
            x0[i] = xx * cos(lng[0] * M_PI / 180) - yy * sin(lng[0] * M_PI / 180);
            y0[i] = xx * sin(lng[0] * M_PI / 180) + yy * cos(lng[0] * M_PI / 180);
            k1 = sin((90 + (lng[i] - lng[0])) * M_PI / 180);
            k2 = cos((90 + (lng[i] - lng[0])) * M_PI / 180);
            x1[i] = x0[i] + k1 * 5;
            y1[i] = y0[i] + k2 * 5;
            x2[i] = x0[i] - k1 * 5;
            y2[i] = y0[i] - k2 * 5;
        }



        double kx = (double)(600) / (abs(y_max - y_min));//x轴的系数
        double ky = (double)(600) / (abs(y_max - y_min));//y方向的比例系数

        //距离车正前方10m处画一条线
        static const QPointF points3[2] = { QPointF(0, 700 - 10 * ky), QPointF(900, 700 - 10 * ky) };
        painter->drawPolyline(points3, 2);

        //绘制路径点
        penPoint.setColor(Qt::black);
        penPoint.setWidth(1);

        for (int i = 1; i < sizeN - 1; i++)
        {
            painter->setPen(penPoint);//蓝色的笔，用于标记各个点
            painter->drawPoint(pointx + x0[i] * kx, pointy - y0[i] * ky);
            painter->drawPoint(pointx + x1[i] * kx, pointy - y1[i] * ky);
            painter->drawPoint(pointx + x2[i] * kx, pointy - y2[i] * ky);
        }
        painter->drawPoint(pointx + x0[sizeN - 1] * kx, pointy - y0[sizeN - 1] * ky);//绘制最后一个点

        penPoint.setColor(Qt::red);
        penPoint.setWidth(2);

        painter->drawPoint(pointx + x0[0] * kx, pointy - y0[0] * ky);



    }


    if(abs(xTime - mnTimeLidarOBS)<3000)
    {
        std::shared_ptr<std::vector<iv::ObstacleBasic>> xobs;
        mMutexOBS.lock();
        xobs = mobs;
        mMutexOBS.unlock();
        painter->setPen(QPen(Qt::red, 2));
        int nobssize = xobs->size();
        for (unsigned int i = 0; i <nobssize; i++)
        {
            painter->drawPoint(xobs->at(i).nomal_x*10+450,-xobs->at(i).nomal_y*10+700);
  //          painter->drawPoint(mlidarpoint.lidar_point[x]* 10 + 450, -mlidarpoint.lidar_point[x+5000]* 10 + 700);
        }
    }

    if(abs(xTime-mnTimeFusion)<3000)
    {
  //      qDebug("update show fusion.");
        iv::fusion::fusionobjectarray xfusion;
        mMutexFusion.lock();
        xfusion.CopyFrom(mfusion);
        mMutexFusion.unlock();

        painter->setPen(QColor(255,0,0));

        for(int a = 0; a < xfusion.obj_size(); a++)
        {
            for(int b = 0; b < xfusion.obj(a).nomal_centroid_size(); b++)
            {
                painter->drawPoint((xfusion.obj(a).nomal_centroid(b).nomal_x())*10 + 450, -(xfusion.obj(a).nomal_centroid(b).nomal_y() + 0)*10 + 700);
            }
        }

    }

    if(abs(xTime-mnTimeRADAR)<3000)
    {
        iv::radar::radarobjectarray xradarobj;
        mMutexRadar.lock();
        xradarobj.CopyFrom(mradarobj);
        mMutexRadar.unlock();

        painter->setPen(QPen(Qt::black, 2));
        painter->setBrush(Qt::yellow);
        QFont esr_font("Microsoft YaHei", 10, 75); //第一个属性是字体（微软雅黑），第二个是大小，第三个是加粗（权重是75）
        painter->setFont(esr_font);
        char coordinate_ear[20];
        for (unsigned int i = 0 ; i < xradarobj.obj_size(); i++)
        {
            iv::radar::radarobject * pobj = xradarobj.mutable_obj(i);

            if(pobj->bvalid())
            {
            painter->drawEllipse(pobj->x() * 10 + 450, -pobj->y() * 10 + 700, 10, 10);
            sprintf(coordinate_ear, "(%d, %d)", (int)pobj->x(), (int)pobj->y());
            painter->drawText(pobj->x() * 10 + 450, -pobj->y() * 10 + 700, QString(coordinate_ear));
            }

        }


    }

    QPixmap pix;
    //pix.load("car.png");
    pix.load(":/new/pic/car.png");
    painter->drawPixmap(435,667,30,67,pix);


    painter->end();
    mMutexPaint.unlock();
    mbImageUpdate = true;
    /*
    iv::vision::rawpic pic;
    mMutex.lock();
    pic.CopyFrom(mrawpic);
    mMutex.unlock();

    cv::Mat mat(pic.height(),pic.width(),pic.mattype());

    if(pic.type() == 1)
        memcpy(mat.data,pic.picdata().data(),mat.rows*mat.cols*mat.elemSize());
    else
    {

//        qDebug("jpg");
        std::vector<unsigned char> buff(pic.picdata().data(),pic.picdata().data()+pic.picdata().size());
        mat = cv::imdecode(buff,CV_LOAD_IMAGE_COLOR);
    }

    cv::cvtColor(mat, mat, CV_BGR2RGB);
    QImage image2 = QImage((uchar*)(mat.data), mat.cols, mat.rows,  QImage::Format_RGB888);

    mMutexPaint.lock();

//    delete mimagepaint;
//    mimagepaint = new  QImage(mat.cols, mat.rows,  QImage::Format_RGB888);
    *mimagepaint = image2.copy();
    //  *mimagepaint = image2;
 //   mimagepaint = new QImage(image2);
    mMutexPaint.unlock();




    mat.release();
    */
}

QImage ivmapview::GetImage()
{

    mMutexPaint.lock();
//    QImage imagertn(*mimagepaint);
//    QImage imagertn(mimagepaint->width(), mimagepaint->height(), QImage::Format_RGB32);
    QImage imagertn = image->copy();
    mMutexPaint.unlock();
    mbImageUpdate = false;
    return imagertn;
}

bool ivmapview::IsHaveNew()
{
    return mbImageUpdate;
}

int ivmapview::GetType()
{
    return 2;
}

void ivmapview::setmap(std::vector<iv::MAP_GPS_INS> xnavigation_data)
{
    unsigned int i;
    for(i=0;i<xnavigation_data.size();i++)
    {
        double x,y;
        GaussProjCal(xnavigation_data[i].gps_lng,xnavigation_data[i].gps_lat,&x,&y);
        xnavigation_data[i].gps_x = x;
        xnavigation_data[i].gps_y = y;
 //       qDebug("x %f y %f ",xnavigation_data[i].gps_lng,xnavigation_data[i].gps_lat);
    }
    mMutexMap.lock();
    mnavigation_data = xnavigation_data;
    mnTimeGPSUpdate = QDateTime::currentMSecsSinceEpoch();
    qDebug("map updata.");
    mMutexMap.unlock();
}

void ivmapview::setgps(iv::gps::gpsimu *pxgpsimu)
{
    mMutexGPS.lock();
    mgpsimu.CopyFrom(*pxgpsimu);
    mnTimeGPSUpdate = QDateTime::currentMSecsSinceEpoch();
    mMutexGPS.unlock();
}

void ivmapview::setobs(std::shared_ptr<std::vector<iv::ObstacleBasic> > xobs)
{
    mMutexOBS.lock();
    mobs = xobs;
    mMutexOBS.unlock();
    mnTimeLidarOBS = QDateTime::currentMSecsSinceEpoch();
}

void ivmapview::setradar(iv::radar::radarobjectarray *pradarobj)
{
    mMutexRadar.lock();
    mradarobj.CopyFrom(*pradarobj);
    mMutexRadar.unlock();
    mnTimeRADAR = QDateTime::currentMSecsSinceEpoch();
}

void ivmapview::setfusion(iv::fusion::fusionobjectarray &xfusion)
{
//    qDebug("set fusion.");
    mMutexFusion.lock();
    mfusion.CopyFrom(xfusion);
    mMutexFusion.unlock();
    mnTimeFusion = QDateTime::currentMSecsSinceEpoch();
}

void ivmapview::setMapMode(int nMode)
{
    mnMapMode = nMode;
}

void ivmapview::setStationIndex(int nStationIndex)
{
    mnStationIndex = nStationIndex;
}

void ivmapview::SetXodrRaw(const char * strdata,const unsigned int ndatasize)
{

    bool bSave = false;
    QFile xFile;
#ifdef ANDROID
    xFile.setFileName("/storage/emulated/0//map.xodr");
#else
    xFile.setFileName("./map.xodr");
#endif
    if(xFile.open(QIODevice::ReadOnly))
    {
        if(xFile.size() != ndatasize)
        {
            bSave = true;
            std::cout<<" old size: "<<xFile.size()<<" data size: "<<ndatasize<<std::endl;
        }
        else
        {
            QByteArray ba = xFile.readAll();
            unsigned int i;
            char * pold = ba.data();
            for(i=0;i<ndatasize;i++)
            {
                if(pold[i] != strdata[i])
                {
                    bSave = true;
                    break;
                }
            }
            if(bSave == false)
                std::cout<<" not need update . size is same.info is same."<<std::endl;
            else
                std::cout<<"  size is same. but data info is diff."<<std::endl;
        }
        xFile.close();
    }
    else
    {
        if(!xFile.exists())
        {
            bSave = true;
        }
    }

    if(bSave == false)
    {
        return;
    }


    mmutexxodrfile.lock();
    if(xFile.open(QIODevice::ReadWrite))
    {
        xFile.resize(0);
        xFile.write(strdata,ndatasize);
        xFile.close();
        mbNeedReload = true;
    }

    mmutexxodrfile.unlock();
}