#include "dialogroadoptimize.h"
#include "ui_dialogroadoptimize.h"

#include <math.h>
#include <memory>

#include "geofit.h"

DialogRoadOptimize::DialogRoadOptimize(Road * pRoad,OpenDrive * pxodr,QWidget *parent) :
    QDialog(parent),
    ui(new Ui::DialogRoadOptimize)
{
    ui->setupUi(this);

    ui->lineEdit_disthresh->setText("0.1");
    ui->lineEdit_hdgthresh->setText("0.01");
    ui->lineEdit_arcbezel->setText("3.0");
    ui->lineEdit_linebezel->setText("10.0");
    ui->lineEdit_roadoffthresh->setText("0.5");

    mpRoad = pRoad;
    mpxodr = pxodr;

    std::vector<double> fvectordis,fvectorhdg;
    int nrtn = CheckRoadQuality(pRoad,fvectordis,fvectorhdg);

    const int noutbufsize = 100000;
    std::shared_ptr<char> pstrout_ptr = std::shared_ptr<char>(new char[noutbufsize]);
    char strline[1000];
    snprintf(pstrout_ptr.get(),noutbufsize,"Check point:%d\n",nrtn);
    if(nrtn > 0)
    {
        int i;
        for(i=0;i<nrtn;i++)
        {
            RoadGeometry * pg = pRoad->GetGeometryBlock(i)->GetGeometryAt(0);
            snprintf(strline,1000,"%d  distance:%f   hdgdiff:%f geo type:%d s:%f length %f\n",
                     i,fvectordis[i],fvectorhdg[i],pg->GetGeomType(),pg->GetS(),pg->GetLength());
            strncat(pstrout_ptr.get(),strline,noutbufsize);
        }
    }

    ui->plainTextEdit->setPlainText(pstrout_ptr.get());

    setWindowTitle("Road Optimize");
}

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

int DialogRoadOptimize::CheckRoadQuality(Road *pRoad, std::vector<double> & fvectordis,
                                         std::vector<double> & fvectorhdg)
{
    if(pRoad->GetGeometryBlockCount() <=1)
    {
        return 0;
    }

    int nrtn = 0;

    unsigned int i;
    for(i=0;i<(pRoad->GetGeometryBlockCount()-1);i++)
    {
        GeometryBlock * pgeob1 =  pRoad->GetGeometryBlock(i);
        GeometryBlock * pgeob2 =  pRoad->GetGeometryBlock(i+1);
        double x1,y1,hdg1;
        double x2,y2,hdg2;
        pgeob1->GetGeometryAt(0)->GetCoords(pgeob1->GetGeometryAt(0)->GetS() + pgeob1->GetGeometryAt(0)->GetLength(),
                                            x1,y1,hdg1);
        pgeob2->GetGeometryAt(0)->GetCoords(pgeob2->GetGeometryAt(0)->GetS() ,
                                            x2,y2,hdg2);

        double fdis = sqrt(pow(x2-x1,2)+pow(y2-y1,2));
        double fhdgdiff = hdg2 - hdg1;
        while (fhdgdiff>M_PI)fhdgdiff = fhdgdiff -2.0*M_PI;
        while(fhdgdiff<=(-M_PI))fhdgdiff = fhdgdiff + 2.0*M_PI;
        fvectordis.push_back(fdis);
        fvectorhdg.push_back(fhdgdiff);
        nrtn++;

    }
    return nrtn;
}

void DialogRoadOptimize::on_pushButton_Optimize_clicked()
{

    double fdisthresh = ui->lineEdit_disthresh->text().toDouble();
    double fhdgthresh = ui->lineEdit_hdgthresh->text().toDouble();
    double flinebezel = ui->lineEdit_linebezel->text().toDouble();
    double farcbezel = ui->lineEdit_arcbezel->text().toDouble();
    double froadoffthresh = ui->lineEdit_roadoffthresh->text().toDouble();

    if(fdisthresh<0.00001)
    {
        fdisthresh = 0.00001;
        std::cout<<"fdisthresh change to "<<fdisthresh<<std::endl;
    }
    if(fhdgthresh<0.001)
    {
        fhdgthresh = 0.001;
        std::cout<<"fhdgthesh change to "<<fhdgthresh<<std::endl;
    }
    if(flinebezel<1.0)
    {
        flinebezel = 1.0;
        std::cout<<"flinebezel change to "<<flinebezel<<std::endl;
    }
    if(farcbezel<1.0)
    {
        farcbezel = 1.0;
        std::cout<<"farcbezel change to "<<farcbezel<<std::endl;
    }
    if(froadoffthresh<0.05)
    {
        froadoffthresh = 0.05;
        std::cout<<"froadoffthresh change to "<<froadoffthresh<<std::endl;
    }

    std::vector<double> fvectordis,fvectorhdg;
    Road * pRoad = mpRoad;
    int nrtn = CheckRoadQuality(pRoad,fvectordis,fvectorhdg);
    if(nrtn == 0)
    {
        QMessageBox::information(this,"Info","Not Need Optimize",QMessageBox::YesAll);
        return;
    }

    int nmsgbufsize = 100000;
    std::shared_ptr<char> strmsg_ptr = std::shared_ptr<char>(new char[nmsgbufsize]);
    snprintf(strmsg_ptr.get(),nmsgbufsize,"Change pos: \n");

    int nchange = 0;
    Road * pnewroad = mpRoad;
    int i;
    int nowgeobpos = 0;
    GeometryBlock * pgeobnow = pnewroad->GetGeometryBlock(0);
    std::vector<GeometryBlock > * pvectorgeo = pnewroad->GetGeometryBlockVector();
    for(i=0;i<nrtn;i++)
    {
        if((fvectordis[i]>fdisthresh)||(fabs(fvectorhdg[i])>fhdgthresh))
        {
            pgeobnow = pnewroad->GetGeometryBlock(nowgeobpos);
            RoadGeometry * pgeo = pgeobnow->GetGeometryAt(0);
            int ngeotype = pgeo->GetGeomType();
            std::cout<<"geo type : "<<ngeotype<<std::endl;
            if((pgeo->GetGeomType() ==0) ||(pgeo->GetGeomType() == 2))
            {
                double fthresh = flinebezel;
                if(ngeotype == 2)fthresh = farcbezel;
                if(pgeo->GetLength()>(fthresh+1.0))
                {
                    double xkeeplen = pgeo->GetLength() - fthresh;
                    double x_start,y_start,hdg_start,x_end,y_end,hdg_end;
                    pgeo->GetCoords(pgeo->GetS()+xkeeplen,x_start,y_start,hdg_start);
                    pnewroad->GetGeometryBlock(nowgeobpos+1)->GetGeometryAt(0)->GetCoords(pgeo->GetS() + pgeo->GetLength(),
                                                                                 x_end,y_end,hdg_end);
                    std::vector<geobase> xvectorgeo = geofit::CreateBezierGeo(x_start,y_start,
                                                                              hdg_start,x_end,y_end,hdg_end);


                    GeometryBlock xgb;
                    qDebug("be len: %f ",xvectorgeo[0].mfLen);
                    xgb.AddGeometryParamPoly3(pgeo->GetS() + xkeeplen,x_start,y_start,hdg_start,xvectorgeo[0].mfLen,
                                              xvectorgeo[0].mfu[0],xvectorgeo[0].mfu[1],
                            xvectorgeo[0].mfu[2],xvectorgeo[0].mfu[3],
                            xvectorgeo[0].mfv[0],xvectorgeo[0].mfv[1],
                            xvectorgeo[0].mfv[2],xvectorgeo[0].mfv[3],false);
                    double fdisgeo = GetGeo1toGeo2MaxDis(&xgb,&(pvectorgeo->at(nowgeobpos)));

                    if(fdisgeo<= froadoffthresh)
                    {
                        pgeo->SetLength(xkeeplen);
                        pvectorgeo->insert(pvectorgeo->begin() + nowgeobpos+1,xgb);
                        nowgeobpos = nowgeobpos+2;
                        char strout[1000];
                        snprintf(strout,1000,"geo %d to geo %d. Insert a bezel.geo off:%f.\n",i,i+1,fdisgeo);
                        strncat(strmsg_ptr.get(),strout,nmsgbufsize);
                        nchange++;
                    }
                    else
                    {
                        char strout[1000];
                        snprintf(strout,1000,"geo %d to geo %d. Optimize off %f exceed thresh.\n",i,i+1,fdisgeo);
                        strncat(strmsg_ptr.get(),strout,nmsgbufsize);
                        nchange++;
                        nowgeobpos++;
                    }
                }
                else
                {
                    double x_start,y_start,hdg_start,x_end,y_end,hdg_end;
                    pgeo->GetCoords(pgeo->GetS(),x_start,y_start,hdg_start);
                    pnewroad->GetGeometryBlock(nowgeobpos+1)->GetGeometryAt(0)->GetCoords(pgeo->GetS() + pgeo->GetLength(),
                                                                                 x_end,y_end,hdg_end);
                    std::vector<geobase> xvectorgeo = geofit::CreateBezierGeo(x_start,y_start,
                                                                              hdg_start,x_end,y_end,hdg_end);

                    qDebug("be len: %f ",xvectorgeo[0].mfLen);
                    GeometryBlock xgb;
                    xgb.AddGeometryParamPoly3(pgeo->GetS(),x_start,y_start,hdg_start,xvectorgeo[0].mfLen,
                                              xvectorgeo[0].mfu[0],xvectorgeo[0].mfu[1],
                            xvectorgeo[0].mfu[2],xvectorgeo[0].mfu[3],
                            xvectorgeo[0].mfv[0],xvectorgeo[0].mfv[1],
                            xvectorgeo[0].mfv[2],xvectorgeo[0].mfv[3],false);

                    double fdisgeo = GetGeo1toGeo2MaxDis(&xgb,&(pvectorgeo->at(nowgeobpos)));
                    if(fdisgeo <= froadoffthresh)
                    {
                        pvectorgeo->erase(pvectorgeo->begin()+nowgeobpos);
                        pvectorgeo->insert(pvectorgeo->begin()+nowgeobpos,xgb);
                        nowgeobpos = nowgeobpos+1;
                        char strout[1000];
                        snprintf(strout,1000,"geo %d to geo %d. geo %d replace by a bezel. geo off:%f.\n",i,i+1,i,fdisgeo);
                        strncat(strmsg_ptr.get(),strout,nmsgbufsize);
                        nchange++;
                    }
                    else
                    {
                        char strout[1000];
                        snprintf(strout,1000,"geo %d to geo %d. Optimize off %f exceed thresh.\n",i,i+1,fdisgeo);
                        strncat(strmsg_ptr.get(),strout,nmsgbufsize);
                        nchange++;
                        nowgeobpos++;
                    }
     //               break;



                }

                unsigned int j;
                for(j=1;j<pvectorgeo->size();j++)
                {
                    pvectorgeo->at(j).GetGeometryAt(0)->SetS(pvectorgeo->at(j-1).GetGeometryAt(0)->GetS()
                                                             +pvectorgeo->at(j-1).GetGeometryAt(0)->GetLength());
                }

            }
            else
            {
                nowgeobpos++;
            }

        }
        else
        {
            char strout[1000];
            snprintf(strout,1000,"geo %d to geo %d. Not Need Optimize.\n",i,i+1);
            strncat(strmsg_ptr.get(),strout,nmsgbufsize);
            nowgeobpos++;
        }
    }

    if(nchange > 0)
    {
        RoadGeometry * pgeolast = pvectorgeo->at(pvectorgeo->size()-1).GetGeometryAt(0);
        pnewroad->SetRoadLength(pgeolast->GetS() + pgeolast->GetLength());
    }

    char strout[1000];
    snprintf(strout,1000,"Optimize %d position.",nchange);
    strncat(strmsg_ptr.get(),strout,nmsgbufsize);

    QMessageBox::information(this,"Optimize Result",strmsg_ptr.get(),QMessageBox::YesAll);



}

double DialogRoadOptimize::GetGeo1toGeo2MaxDis(GeometryBlock *pgeob1, GeometryBlock *pgeob2)
{
    RoadGeometry * prg1,*prg2;
    prg1 = pgeob1->GetGeometryAt(0);
    prg2 = pgeob2->GetGeometryAt(0);
    double fstep = 0.1;
    int nCount = (int)(prg1->GetLength()/fstep);
    if(nCount == 0)
    {
        return sqrt(pow(prg1->GetX() - prg2->GetX(),2)+pow(prg1->GetY() - prg2->GetY(),2));
    }
    int i;

    double rg1_s = prg1->GetS();
    double rg2_s = prg2->GetS();
//    double rg1_len = prg1->GetLength();
    double rg2_len = prg2->GetLength();
    double fdismax = 0;
    double fsmax = 0;
    for(i=0;i<nCount;i++)
    {
        double s = fstep * i;
        double x1,y1,hdg1;
        double fstep2 = 0.5;
        prg1->GetCoords(rg1_s+s,x1,y1,hdg1);
        int nCount2 =(int)( rg2_len/fstep2);
        double snear = 0;
        double sdis = 1000.0;
        if(nCount2 == 0)
        {
            snear = 0;
        }
        else
        {
            int j;
            for(j=0;j<nCount2;j++)
            {
                double x2,y2,hdg2;
                double s2 = j*fstep2;
                prg2->GetCoords(rg2_s +s2,x2,y2,hdg2);
                double fdis = sqrt(pow(x2-x1,2)+pow(y2-y1,2));
                if(fdis <sdis)
                {
                    snear = s2;
                    sdis = fdis;
                }
            }
        }
        double snear_low = snear - fstep2;
        double snear_high = snear  + fstep2;
        if(snear_low<0)snear_low = 0;
        if(snear_high > rg2_len)snear_high = rg2_len;
        double s3 = snear_low;
        while(s3<=snear_high)
        {
            double x3,y3,hdg3;
            prg2->GetCoords(rg2_s+s3,x3,y3,hdg3);
            double fdis = sqrt(pow(x3-x1,2)+pow(y3-y1,2));
            if(fdis < sdis)
            {
                snear = s3;
                sdis = fdis;
            }
            s3 = s3 +0.01;
        }

        if(sdis > fdismax)
        {
            fdismax = sdis;
            fsmax = snear;
        }
    }

//    std::cout<<"dismax : "<<fdismax<<std::endl;
    return fdismax;
}