#include "cdaproc.h"

#include <iostream>
#include <math.h>

static std::string cda_lanetype_sel[9] = {"shoulder","border","driving","stop","none","parking","biking","sidewalk",
                               "median"};

static std::string cda_lanemarkcolor_sel[2] = {"standard","yellow"};

static std::string cda_lanemarktype_sel[7] = {"broken","solid","broken broken","solid solid","broken solid","solid broken","none"};

CDAProc::CDAProc()
{

}

static void OffRotate(double nowx,double nowy,double nowhdg,double rel_x,double rel_y,double rel_hdg,double & a_x,double &  a_y,double & a_hdg)
{
    a_x = nowx + rel_x * cos(nowhdg) - rel_y * sin(nowhdg) ;
    a_y= nowy + rel_x * sin(nowhdg) + rel_y * cos(nowhdg) ;
    a_hdg = rel_hdg+ nowhdg;
    if(a_hdg> 2.0*M_PI)a_hdg = a_hdg - 2.0*M_PI;
}

int CDAProc::ProcIntersectionRoad(OpenDrive * pxodr, iv::map::cdadraw * pcdadraw,int ngeo,int & nroadid,double & nowx,double & nowy, double & nowhdg)
{
    iv::map::cdageo * pgeo = pcdadraw->mutable_mgeos(ngeo);
    double fRoadLen =  pgeo->geolen();
//    double flanewidth = atof(strlanewidth.data());
    std::vector<double> xvectorlanewidth;
    int i;
    for(i=0;i<pcdadraw->mlanes_size();i++)
    {
        xvectorlanewidth.push_back(pcdadraw->mutable_mlanes(i)->lanewidth());
    }
    double flanewidth11 = 0;
    for(i=0;i<(pcdadraw->mlanes_size()-1);i++)
    {
        flanewidth11 = flanewidth11 + xvectorlanewidth[i];
    }
    int nlanecount = pcdadraw->mlanes_size();
    if(nlanecount<=0)
    {
        std::cout<<" no lane "<<std::endl;
        return -1;
    }
    double fdefradius = 6.0;
    double finsectlen = fdefradius*2.0 + 2.0*flanewidth11;
    if(fRoadLen < (finsectlen+10.0))
    {
        fRoadLen = finsectlen + 10.0;
    }
    double flinelen = (fRoadLen -finsectlen)/2.0;
    double flinex[4],fliney[4],flinehdg[4];
    flinex[0] = 0;fliney[0]=0;flinehdg[0] = 0;
    flinex[1] = fRoadLen*(0.5);fliney[1] = fRoadLen*(-0.5);  flinehdg[1] = M_PI/2.0;
    flinex[2] = fRoadLen*0.5 + finsectlen*0.5;fliney[2] = 0;  flinehdg[2] = 0;
    flinex[3] = fRoadLen*(0.5);fliney[3] = finsectlen*0.5;  flinehdg[3] = M_PI/2.0;

    double flinex_c[4],fliney_c[4],flinehdg_c[4];

    double nowx_n,nowy_n,nowhdg_n;
    for(i=0;i<4;i++)
    {
        flinex_c[i] = nowx + flinex[i] * cos(nowhdg) - fliney[i] * sin(nowhdg) ;
        fliney_c[i] = nowy + flinex[i] * sin(nowhdg) + fliney[i] * cos(nowhdg) ;
        flinehdg_c[i] = flinehdg[i] + nowhdg;
        if(flinehdg_c[i]> 2.0*M_PI)flinehdg_c[i] = flinehdg_c[i] - 2.0*M_PI;

        char strroadid[100];
        nroadid++;
        snprintf(strroadid,100,"%d",nroadid);
        pxodr->AddRoad("zl",flinelen,strroadid,"-1");
        Road * pRoad = pxodr->GetLastAddedRoad();
        pRoad->AddGeometryBlock();
        GeometryBlock * pgeob = pRoad->GetLastAddedGeometryBlock();
        pgeob->AddGeometryLine(0,flinex_c[i],fliney_c[i],flinehdg_c[i],flinelen);
        pRoad->AddLaneSection(0);
        LaneSection * pLS = pRoad->GetLaneSection(0);
        pLS->AddLane(0,0,"none",false);
        Lane * pcenterlane = pLS->GetLastAddedLane();
        iv::map::cdalanemarkline * pmark = pcdadraw->mutable_mlanemarkline(0);
        if(pmark == NULL)
            pcenterlane->AddRoadMarkRecord(0,"solid solid","standard","yellow",0.15,"none");
        else
        {
            pcenterlane->AddRoadMarkRecord(0,cda_lanemarktype_sel[pmark->lanemarktype()],"standard",
                    cda_lanemarkcolor_sel[pmark->lanemarkcolor()],pmark->lanemarkwidth(),"none");
            if(pmark->lanemarktype() == 0)
            {
                LaneRoadMark * pLaneRoadMark = pcenterlane->GetLastAddedLaneRoadMark();
                LaneRoadMarkType laneRoadMarkType(cda_lanemarktype_sel[pmark->lanemarktype()],pmark->lanemarkwidth());
                laneRoadMarkType.AddLaneRoadMarkLine(pmark->lanemarklinelength(),pmark->lanemarklinespace(),0,0);
                pLaneRoadMark->SetLaneRoadMarkType(laneRoadMarkType);
            }
        }

        int j;
        for(j=0;j<nlanecount;j++)
        {
            iv::map::cdalane * pcdalane = pcdadraw->mutable_mlanes(j);
            pLS->AddLane(-1,(j+1)*(-1),cda_lanetype_sel[pcdalane->lanetype()],false,true);
            Lane * pnewlane = pLS->GetLastAddedLane();

            pnewlane->AddWidthRecord(0,pcdalane->lanewidth(),0,0,0);
            pmark = pcdadraw->mutable_mlanemarkline(j+1);
            if(pmark != NULL)
            {
                pnewlane->AddRoadMarkRecord(0,cda_lanemarktype_sel[pmark->lanemarktype()],"standard",
                        cda_lanemarkcolor_sel[pmark->lanemarkcolor()],pmark->lanemarkwidth(),"none");
                if(pmark->lanemarktype() == 0)
                {
                    LaneRoadMark * pLaneRoadMark = pnewlane->GetLastAddedLaneRoadMark();
                    LaneRoadMarkType laneRoadMarkType(cda_lanemarktype_sel[pmark->lanemarktype()],pmark->lanemarkwidth());
                    laneRoadMarkType.AddLaneRoadMarkLine(pmark->lanemarklinelength(),pmark->lanemarklinespace(),0,0);
                    pLaneRoadMark->SetLaneRoadMarkType(laneRoadMarkType);
                }
            }
            pLS->AddLane(1,(j+1)*(1),cda_lanetype_sel[pcdalane->lanetype()],false,true);
            pnewlane = pLS->GetLastAddedLane();
            pnewlane->AddWidthRecord(0,pcdalane->lanewidth(),0,0,0);
            if(pmark != NULL)
            {
                pnewlane->AddRoadMarkRecord(0,cda_lanemarktype_sel[pmark->lanemarktype()],"standard",
                        cda_lanemarkcolor_sel[pmark->lanemarkcolor()],pmark->lanemarkwidth(),"none");
                if(pmark->lanemarktype() == 0)
                {
                    LaneRoadMark * pLaneRoadMark = pnewlane->GetLastAddedLaneRoadMark();
                    LaneRoadMarkType laneRoadMarkType(cda_lanemarktype_sel[pmark->lanemarktype()],pmark->lanemarkwidth());
                    laneRoadMarkType.AddLaneRoadMarkLine(pmark->lanemarklinelength(),pmark->lanemarklinespace(),0,0);
                    pLaneRoadMark->SetLaneRoadMarkType(laneRoadMarkType);
                }
            }
        }
        if(i==2)
        {
            pRoad->GetGeometryCoords(pRoad->GetRoadLength(),nowx_n,nowy_n,nowhdg_n);
        }
    }

    double frightlen = fdefradius * M_PI/2.0;
    double frightx[4],frighty[4],frighthdg[4];
    frightx[0] = flinelen;frighty[0] = flanewidth11*(-1);frighthdg[0] = 0;
    frightx[1] = fRoadLen*0.5 + flanewidth11 ;frighty[1] = finsectlen*(-0.5);frighthdg[1] = M_PI/2.0;
    frightx[2] = fRoadLen*0.5 + finsectlen*0.5;frighty[2] = flanewidth11*(1);frighthdg[2] = M_PI;
    frightx[3] = fRoadLen*0.5 - flanewidth11;frighty[3] = finsectlen*0.5;frighthdg[3] = 3.0*M_PI/2.0;

    for(i=0;i<4;i++)
    {
        flinex_c[i] = nowx + frightx[i] * cos(nowhdg) - frighty[i] * sin(nowhdg) ;
        fliney_c[i] = nowy + frightx[i] * sin(nowhdg) + frighty[i] * cos(nowhdg) ;
        flinehdg_c[i] = frighthdg[i] + nowhdg;
        if(flinehdg_c[i]> 2.0*M_PI)flinehdg_c[i] = flinehdg_c[i] - 2.0*M_PI;

        char strroadid[100];
        nroadid++;
        snprintf(strroadid,100,"%d",nroadid);
        pxodr->AddRoad("zl",flinelen,strroadid,"-1");
        Road * pRoad = pxodr->GetLastAddedRoad();
        pRoad->AddGeometryBlock();
        GeometryBlock * pgeob = pRoad->GetLastAddedGeometryBlock();
        pgeob->AddGeometryArc(0,flinex_c[i],fliney_c[i],flinehdg_c[i],frightlen,-1.0/fdefradius);
        pRoad->AddLaneSection(0);
        LaneSection * pLS = pRoad->GetLaneSection(0);
        pLS->AddLane(0,0,"none",false);
//        Lane * pcenterlane = pLS->GetLastAddedLane();
 //       pcenterlane->AddRoadMarkRecord(0,"solid","standard","yellow",0.15,"none");

        iv::map::cdalane * pcdalane = pcdadraw->mutable_mlanes(pcdadraw->mlanes_size()-1);

        pLS->AddLane(-1,-1,cda_lanetype_sel[pcdalane->lanetype()],false,true);
        Lane * pnewlane = pLS->GetLastAddedLane();
        pnewlane->AddWidthRecord(0,pcdadraw->mutable_mlanes(pcdadraw->mlanes_size()-1)->lanewidth(),0,0,0);
    }

    double fleftx[4],flefty[4],flefthdg[4];
    fleftx[0] = flinelen;flefty[0] = 0;flefthdg[0] = 0;
    fleftx[1] = fRoadLen*0.5;flefty[1] = finsectlen*(-0.5);flefthdg[1] = M_PI/2.0;
    fleftx[2] = flinelen+finsectlen;flefty[2] = 0;flefthdg[2] = M_PI;
    fleftx[3] = fRoadLen*0.5;flefty[3] = finsectlen*0.5;flefthdg[3] = 3.0*M_PI/2.0;
    for(i=0;i<4;i++)
    {
        double fleftlinelen = 0.5*finsectlen - fdefradius;
        double fleftarcx = fleftx[i] + fleftlinelen * cos(flefthdg[i]);
        double fleftarcy = flefty[i] + fleftlinelen * sin(flefthdg[i]);
        double fleftarchdg = flefthdg[i];
        double a_x,a_y,a_hdg;
        char strroadid[100];
        nroadid++;
        snprintf(strroadid,100,"%d",nroadid);
        pxodr->AddRoad("zl",fleftlinelen*2.0+fdefradius*M_PI/2.0,strroadid,"-1");
        Road * pRoad = pxodr->GetLastAddedRoad();

        GeometryBlock * pgeob;
        if(fleftlinelen >0.00000000001)
        {
            pRoad->AddGeometryBlock();
            GeometryBlock * pgeob = pRoad->GetLastAddedGeometryBlock();
            OffRotate(nowx,nowy,nowhdg,fleftx[i],flefty[i],flefthdg[i],a_x,a_y,a_hdg);
            pgeob->AddGeometryLine(0,a_x,a_y,a_hdg,fleftlinelen);
        }
        pRoad->AddGeometryBlock();
        pgeob= pRoad->GetLastAddedGeometryBlock();
         OffRotate(nowx,nowy,nowhdg,fleftarcx,fleftarcy,fleftarchdg,a_x,a_y,a_hdg);
        pgeob->AddGeometryArc(fleftlinelen,a_x,a_y,a_hdg,fdefradius*M_PI/2.0,1.0/fdefradius);
        if(fleftlinelen >0.00000000001)
        {
            pRoad->AddGeometryBlock();
            GeometryBlock * pgeob = pRoad->GetLastAddedGeometryBlock();
            OffRotate(nowx,nowy,nowhdg,fleftarcx + fdefradius*cos(flefthdg[i]) + fdefradius*cos(flefthdg[i] + M_PI/2.0) ,
                      fleftarcy + fdefradius*sin(flefthdg[i]) + fdefradius*sin(flefthdg[i] + M_PI/2.0),flefthdg[i] +M_PI/2.0,a_x,a_y,a_hdg);
            pgeob->AddGeometryLine(fleftlinelen + fdefradius*M_PI/2.0,a_x,a_y,a_hdg,fleftlinelen);
        }
        pRoad->AddLaneSection(0);
        LaneSection * pLS = pRoad->GetLaneSection(0);
        pLS->AddLane(0,0,"none",false);
//        Lane * pcenterlane = pLS->GetLastAddedLane();
 //       pcenterlane->AddRoadMarkRecord(0,"solid","standard","yellow",0.15,"none");

        iv::map::cdalane * pcdalane = pcdadraw->mutable_mlanes(0);
        pLS->AddLane(-1,-1,cda_lanetype_sel[pcdalane->lanetype()],false,true);
        Lane * pnewlane = pLS->GetLastAddedLane();
        pnewlane->AddWidthRecord(0,pcdadraw->mutable_mlanes(0)->lanewidth(),0,0,0);
    }

    double finlinex[4],finliney[4],finlinehdg[4];
    finlinex[0] = flinelen;finliney[0] = 0;finlinehdg[0] = 0;
    finlinex[1] = fRoadLen*0.5;finliney[1] = finsectlen*(-0.5);finlinehdg[1] = M_PI/2.0;
    finlinex[2] = flinelen+finsectlen;finliney[2] = 0;finlinehdg[2] = M_PI;
    finlinex[3] = fRoadLen*0.5;finliney[3] = finsectlen*0.5;finlinehdg[3] = 3.0*M_PI/2.0;

    for(i=0;i<4;i++)
    {
        char strroadid[100];
        nroadid++;
        snprintf(strroadid,100,"%d",nroadid);
        pxodr->AddRoad("zl",flinelen,strroadid,"-1");
        Road * pRoad = pxodr->GetLastAddedRoad();
        pRoad->AddGeometryBlock();
        GeometryBlock * pgeob = pRoad->GetLastAddedGeometryBlock();
        double a_x,a_y,a_hdg;
        OffRotate(nowx,nowy,nowhdg,finlinex[i],finliney[i],finlinehdg[i],a_x,a_y,a_hdg);
        pgeob->AddGeometryLine(0,a_x,a_y,a_hdg,finsectlen);
        pRoad->AddLaneSection(0);
        LaneSection * pLS = pRoad->GetLaneSection(0);
        pLS->AddLane(0,0,"none",false);
//        Lane * pcenterlane = pLS->GetLastAddedLane();
//        pcenterlane->AddRoadMarkRecord(0,"solid solid","standard","yellow",0.15,"none");

        int j;
        for(j=0;j<nlanecount;j++)
        {
            iv::map::cdalane * pcdalane = pcdadraw->mutable_mlanes(j);
            pLS->AddLane(-1,(j+1)*(-1),cda_lanetype_sel[pcdalane->lanetype()],false,true);
            Lane * pnewlane = pLS->GetLastAddedLane();
            pnewlane->AddWidthRecord(0,pcdalane->lanewidth(),0,0,0);
            pLS->AddLane(1,(j+1)*(1),cda_lanetype_sel[pcdalane->lanetype()],false,true);
            pnewlane = pLS->GetLastAddedLane();
            pnewlane->AddWidthRecord(0,pcdalane->lanewidth(),0,0,0);
        }
    }

    nowx = nowx_n;
    nowy = nowy_n;
    nowhdg = nowhdg_n;

}

int CDAProc::ProcIntersectionRoad(OpenDrive * pxodr,  std::string strtype,std::string strradius,std::string strroadlen,std::string strlanewidth,
    std::string strlannecount,std::string strlanetype,std::string strlanemarkcolor,std::string strlanemarktype)
{
    double fRoadLen =  atof(strroadlen.data());
    double flanewidth = atof(strlanewidth.data());
    int nlanecount = atoi(strlannecount.data());
    if(flanewidth<=0)flanewidth = 3.5;
    if(nlanecount<=0)nlanecount = 1;
    double fdefradius = 6.0;
    double finsectlen = fdefradius*2.0 + 2.0*(flanewidth*(nlanecount-1));
    if(fRoadLen < (finsectlen+10.0))
    {
        fRoadLen = finsectlen + 10.0;
    }
    double flinelen = (fRoadLen -finsectlen)/2.0;
    double flinex[4],fliney[4],flinehdg[4];
    flinex[0] = 0;fliney[0]=0;flinehdg[0] = 0;
    flinex[1] = fRoadLen*(0.5);fliney[1] = fRoadLen*(-0.5);  flinehdg[1] = M_PI/2.0;
    flinex[2] = fRoadLen*0.5 + finsectlen*0.5;fliney[2] = 0;  flinehdg[2] = 0;
    flinex[3] = fRoadLen*(0.5);fliney[3] = finsectlen*0.5;  flinehdg[3] = M_PI/2.0;

    int i;
    for(i=0;i<4;i++)
    {
        char strroadid[100];
        snprintf(strroadid,100,"%d",i+1);
        pxodr->AddRoad("zl",flinelen,strroadid,"-1");
        Road * pRoad = pxodr->GetLastAddedRoad();
        pRoad->AddGeometryBlock();
        GeometryBlock * pgeob = pRoad->GetLastAddedGeometryBlock();
        pgeob->AddGeometryLine(0,flinex[i],fliney[i],flinehdg[i],flinelen);
        pRoad->AddLaneSection(0);
        LaneSection * pLS = pRoad->GetLaneSection(0);
        pLS->AddLane(0,0,"none",false);
        Lane * pcenterlane = pLS->GetLastAddedLane();
        pcenterlane->AddRoadMarkRecord(0,"solid solid","standard","yellow",0.15,"none");

        int j;
        for(j=0;j<nlanecount;j++)
        {
            pLS->AddLane(-1,(j+1)*(-1),"driving",false,true);
            Lane * pnewlane = pLS->GetLastAddedLane();
            pnewlane->AddWidthRecord(0,flanewidth,0,0,0);
            if(j == (nlanecount -1))
            {
                pnewlane->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"none");
            }
            else
            {
                std::string type = "broken";
                if(strlanemarktype == "实线")type = "solid";
                std::string color = "standard";
                if(strlanemarkcolor == "黄色")color = "yellow";
                pnewlane->AddRoadMarkRecord(0,type,"standard",color,0.15,"none");
  //              pnewlane->AddRoadMarkRecord(0,"broken","standard","standard",0.15,"none");
            }
            pLS->AddLane(1,(j+1)*(1),"driving",false,true);
            pnewlane = pLS->GetLastAddedLane();
            pnewlane->AddWidthRecord(0,flanewidth,0,0,0);
            if(j == (nlanecount -1))
            {
                pnewlane->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"none");
            }
            else
            {
                std::string type = "broken";
                if(strlanemarktype == "实线")type = "solid";
                std::string color = "standard";
                if(strlanemarkcolor == "黄色")color = "yellow";
                pnewlane->AddRoadMarkRecord(0,type,"standard",color,0.15,"none");
  //              pnewlane->AddRoadMarkRecord(0,"broken","standard","standard",0.15,"none");
            }
        }
    }

    double frightlen = fdefradius * M_PI/2.0;
    double frightx[4],frighty[4],frighthdg[4];
    frightx[0] = flinelen;frighty[0] = flanewidth*(nlanecount-1)*(-1);frighthdg[0] = 0;
    frightx[1] = fRoadLen*0.5 + flanewidth*(nlanecount-1) ;frighty[1] = finsectlen*(-0.5);frighthdg[1] = M_PI/2.0;
    frightx[2] = fRoadLen*0.5 + finsectlen*0.5;frighty[2] = flanewidth*(nlanecount-1)*(1);frighthdg[2] = M_PI;
    frightx[3] = fRoadLen*0.5 - flanewidth*(nlanecount-1);frighty[3] = finsectlen*0.5;frighthdg[3] = 3.0*M_PI/2.0;

    for(i=0;i<4;i++)
    {
        char strroadid[100];
        snprintf(strroadid,100,"%d",i+5);
        pxodr->AddRoad("zl",flinelen,strroadid,"-1");
        Road * pRoad = pxodr->GetLastAddedRoad();
        pRoad->AddGeometryBlock();
        GeometryBlock * pgeob = pRoad->GetLastAddedGeometryBlock();
        pgeob->AddGeometryArc(0,frightx[i],frighty[i],frighthdg[i],frightlen,-1.0/fdefradius);
        pRoad->AddLaneSection(0);
        LaneSection * pLS = pRoad->GetLaneSection(0);
        pLS->AddLane(0,0,"none",false);
//        Lane * pcenterlane = pLS->GetLastAddedLane();
 //       pcenterlane->AddRoadMarkRecord(0,"solid","standard","yellow",0.15,"none");

        pLS->AddLane(-1,-1,"driving",false,true);
        Lane * pnewlane = pLS->GetLastAddedLane();
        pnewlane->AddWidthRecord(0,flanewidth,0,0,0);
    }

    double fleftx[4],flefty[4],flefthdg[4];
    fleftx[0] = flinelen;flefty[0] = 0;flefthdg[0] = 0;
    fleftx[1] = fRoadLen*0.5;flefty[1] = finsectlen*(-0.5);flefthdg[1] = M_PI/2.0;
    fleftx[2] = flinelen+finsectlen;flefty[2] = 0;flefthdg[2] = M_PI;
    fleftx[3] = fRoadLen*0.5;flefty[3] = finsectlen*0.5;flefthdg[3] = 3.0*M_PI/2.0;
    for(i=0;i<4;i++)
    {
        double fleftlinelen = 0.5*finsectlen - fdefradius;
        double fleftarcx = fleftx[i] + fleftlinelen * cos(flefthdg[i]);
        double fleftarcy = flefty[i] + fleftlinelen * sin(flefthdg[i]);
        double fleftarchdg = flefthdg[i];
        char strroadid[100];
        snprintf(strroadid,100,"%d",i+9);
        pxodr->AddRoad("zl",fleftlinelen*2.0+fdefradius*M_PI/2.0,strroadid,"-1");
        Road * pRoad = pxodr->GetLastAddedRoad();

        GeometryBlock * pgeob;
        if(fleftlinelen >0.00000000001)
        {
            pRoad->AddGeometryBlock();
            GeometryBlock * pgeob = pRoad->GetLastAddedGeometryBlock();
            pgeob->AddGeometryLine(0,fleftx[i],flefty[i],flefthdg[i],fleftlinelen);
        }
        pRoad->AddGeometryBlock();
        pgeob= pRoad->GetLastAddedGeometryBlock();
        pgeob->AddGeometryArc(fleftlinelen,fleftarcx,fleftarcy,fleftarchdg,fdefradius*M_PI/2.0,1.0/fdefradius);
        if(fleftlinelen >0.00000000001)
        {
            pRoad->AddGeometryBlock();
            GeometryBlock * pgeob = pRoad->GetLastAddedGeometryBlock();
            pgeob->AddGeometryLine(fleftlinelen + fdefradius*M_PI/2.0,fleftarcx + fdefradius*cos(flefthdg[i]) + fdefradius*cos(flefthdg[i] + M_PI/2.0) ,
                                   fleftarcy + fdefradius*sin(flefthdg[i]) + fdefradius*sin(flefthdg[i] + M_PI/2.0),flefthdg[i] +M_PI/2.0,fleftlinelen);
        }
        pRoad->AddLaneSection(0);
        LaneSection * pLS = pRoad->GetLaneSection(0);
        pLS->AddLane(0,0,"none",false);
//        Lane * pcenterlane = pLS->GetLastAddedLane();
 //       pcenterlane->AddRoadMarkRecord(0,"solid","standard","yellow",0.15,"none");

        pLS->AddLane(-1,-1,"driving",false,true);
        Lane * pnewlane = pLS->GetLastAddedLane();
        pnewlane->AddWidthRecord(0,flanewidth,0,0,0);
    }

    double finlinex[4],finliney[4],finlinehdg[4];
    finlinex[0] = flinelen;finliney[0] = 0;finlinehdg[0] = 0;
    finlinex[1] = fRoadLen*0.5;finliney[1] = finsectlen*(-0.5);finlinehdg[1] = M_PI/2.0;
    finlinex[2] = flinelen+finsectlen;finliney[2] = 0;finlinehdg[2] = M_PI;
    finlinex[3] = fRoadLen*0.5;finliney[3] = finsectlen*0.5;finlinehdg[3] = 3.0*M_PI/2.0;

    for(i=0;i<4;i++)
    {
        char strroadid[100];
        snprintf(strroadid,100,"%d",i+13);
        pxodr->AddRoad("zl",flinelen,strroadid,"-1");
        Road * pRoad = pxodr->GetLastAddedRoad();
        pRoad->AddGeometryBlock();
        GeometryBlock * pgeob = pRoad->GetLastAddedGeometryBlock();
        pgeob->AddGeometryLine(0,finlinex[i],finliney[i],finlinehdg[i],finsectlen);
        pRoad->AddLaneSection(0);
        LaneSection * pLS = pRoad->GetLaneSection(0);
        pLS->AddLane(0,0,"none",false);
//        Lane * pcenterlane = pLS->GetLastAddedLane();
//        pcenterlane->AddRoadMarkRecord(0,"solid solid","standard","yellow",0.15,"none");

        int j;
        for(j=0;j<nlanecount;j++)
        {
            pLS->AddLane(-1,(j+1)*(-1),"driving",false,true);
            Lane * pnewlane = pLS->GetLastAddedLane();
            pnewlane->AddWidthRecord(0,flanewidth,0,0,0);
//            if(j == (nlanecount -1))
//            {
//                pnewlane->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"none");
//            }
//            else
//                pnewlane->AddRoadMarkRecord(0,"broken","standard","standard",0.15,"none");
            pLS->AddLane(1,(j+1)*(1),"driving",false,true);
            pnewlane = pLS->GetLastAddedLane();
            pnewlane->AddWidthRecord(0,flanewidth,0,0,0);
//            if(j == (nlanecount -1))
//            {
//                pnewlane->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"none");
//            }
//            else
//                pnewlane->AddRoadMarkRecord(0,"broken","standard","standard",0.15,"none");
        }
    }


    return 0;
}

int CDAProc::ProcArcRoad(OpenDrive * pxodr,  std::string strtype,std::string strradius,std::string strroadlen,std::string strlanewidth,
    std::string strlannecount,std::string strlanetype,std::string strlanemarkcolor,std::string strlanemarktype)
{

    double fRoadLen =  atof(strroadlen.data());
    double fRadius = atof(strradius.data());
    if(fabs(fRadius) < 0.000000000000001)return -5;
    pxodr->AddRoad("wd",fRoadLen,"1","-1");
    Road * pRoad = pxodr->GetLastAddedRoad();


    double fArcLen = fabs(fRadius) * M_PI/2.0;
    bool bHaveLine = false;
    double fxline,fyline,fhdgline,flinelen;
    if(fArcLen>=fRoadLen)
    {
        fArcLen = fRoadLen;
    }
    else
    {
        bHaveLine = true;
        flinelen = fRoadLen - fArcLen;
        fxline = fabs(fRadius);
        fyline = fRadius;
        fhdgline = (fyline/fxline)*(M_PI/2.0);

    }

    pRoad->AddGeometryBlock();
    GeometryBlock * pgeob = pRoad->GetLastAddedGeometryBlock();
    pgeob->AddGeometryArc(0,0,0,0,fArcLen,1.0/fRadius);
    if(bHaveLine == true)
    {
        pRoad->AddGeometryBlock();
        pgeob = pRoad->GetLastAddedGeometryBlock();
        pgeob->AddGeometryLine(fArcLen,fxline,fyline,fhdgline,flinelen);
    }
    pRoad->AddLaneSection(0);
    LaneSection * pLS = pRoad->GetLaneSection(0);
    pLS->AddLane(0,0,"none",false);
    Lane * pcenterlane = pLS->GetLastAddedLane();
    pcenterlane->AddRoadMarkRecord(0,"solid","standard","yellow",0.15,"none");

    int nlanecount = atoi(strlannecount.data());
    double flanewidth = atof(strlanewidth.data());
    int i;
    for(i=0;i<nlanecount;i++)
    {
        pLS->AddLane(-1,(i+1)*(-1),"driving",false,true);
        Lane * pnewlane = pLS->GetLastAddedLane();
        pnewlane->AddWidthRecord(0,flanewidth,0,0,0);
        if(i == (nlanecount -1))
        {
            pnewlane->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"none");
        }
        else
        {
            std::string type = "broken";
            if(strlanemarktype == "实线")type = "solid";
            std::string color = "standard";
            if(strlanemarkcolor == "黄色")color = "yellow";
            pnewlane->AddRoadMarkRecord(0,type,"standard",color,0.15,"none");
//              pnewlane->AddRoadMarkRecord(0,"broken","standard","standard",0.15,"none");
        }
    }
    return 0;
}

int CDAProc::ProcLineRoad(OpenDrive * pxodr,  std::string strtype,std::string strradius,std::string strroadlen,std::string strlanewidth,
    std::string strlannecount,std::string strlanetype,std::string strlanemarkcolor,std::string strlanemarktype)
{

    pxodr->AddRoad("zl",atof(strroadlen.data()),"1","-1");
    Road * pRoad = pxodr->GetLastAddedRoad();
    pRoad->AddGeometryBlock();
    GeometryBlock * pgeob = pRoad->GetLastAddedGeometryBlock();
    pgeob->AddGeometryLine(0,0,0,0,atof(strroadlen.data()));
    pRoad->AddLaneSection(0);
    LaneSection * pLS = pRoad->GetLaneSection(0);
    pLS->AddLane(0,0,"none",false);
    Lane * pcenterlane = pLS->GetLastAddedLane();
    pcenterlane->AddRoadMarkRecord(0,"solid","standard","yellow",0.15,"none");

    int nlanecount = atoi(strlannecount.data());
    double flanewidth = atof(strlanewidth.data());
    int i;
    for(i=0;i<nlanecount;i++)
    {
        pLS->AddLane(-1,(i+1)*(-1),"driving",false,true);
        Lane * pnewlane = pLS->GetLastAddedLane();
        pnewlane->AddWidthRecord(0,flanewidth,0,0,0);
        if(i == (nlanecount -1))
        {
            pnewlane->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"none");
        }
        else
        {
            std::string type = "broken";
            if(strlanemarktype == "实线")type = "solid";
            std::string color = "standard";
            if(strlanemarkcolor == "黄色")color = "yellow";
            pnewlane->AddRoadMarkRecord(0,type,"standard",color,0.15,"none");
//              pnewlane->AddRoadMarkRecord(0,"broken","standard","standard",0.15,"none");
        }
    }
    return 0;
}

int CDAProc::ProcRoad(OpenDrive * pxodr,  std::string strtype,std::string strradius,std::string strroadlen,std::string strlanewidth,
                      std::string strlannecount,std::string strlanetype,std::string strlanemarkcolor,std::string strlanemarktype)
{
    if(atof(strroadlen.data())<0.00000001)
    {
        return -2;
    }
    if(atof(strlanewidth.data())<0.00000001)
    {
        return -3;
    }
    if(atoi(strlannecount.data())<1)
    {
        return -4;
    }
    if(strtype == "直路" )
    {
        std::cout<<" is zl . "<<std::endl;
        return ProcLineRoad(pxodr,strtype,strradius,strroadlen,strlanewidth,strlannecount,strlanetype,strlanemarkcolor,strlanemarktype);
    }
    if(strtype == "弯路" )
    {
        std::cout<<" is wl . "<<std::endl;
        return ProcArcRoad(pxodr,strtype,strradius,strroadlen,strlanewidth,strlannecount,strlanetype,strlanemarkcolor,strlanemarktype);
    }
    if(strtype == "路口" )
    {
        std::cout<<" is lk . "<<std::endl;
        return ProcIntersectionRoad(pxodr,strtype,strradius,strroadlen,strlanewidth,strlannecount,strlanetype,strlanemarkcolor,strlanemarktype);
    }
    return -1;
}

int CDAProc::Proc(std::string strxlsxpath,OpenDrive * pxodr)
{

    void * pexcel = ServiceExcelAPI.Openxlsx(strxlsxpath);
    std::string strtype;
    std::string strradius;
    std::string strroadlen;
    std::string strlanewidth;
    std::string strlannecount;
    std::string strlanetype;
    std::string strlanemarkcolor;
    std::string strlanemarktype;
    ServiceExcelAPI.getcellvalue(pexcel,119,4,strtype);
    ServiceExcelAPI.getcellvalue(pexcel,120,4,strradius);
    ServiceExcelAPI.getcellvalue(pexcel,121,4,strroadlen);
    ServiceExcelAPI.getcellvalue(pexcel,122,4,strlanewidth);
    ServiceExcelAPI.getcellvalue(pexcel,123,4,strlannecount);
//    ServiceExcelAPI.getcellvalue(pexcel,124,4,strlanetype);
    ServiceExcelAPI.getcellvalue(pexcel,124,4,strlanemarkcolor);
    ServiceExcelAPI.getcellvalue(pexcel,125,4,strlanemarktype);
    std::cout<<"type : "<<strtype<<std::endl;
    ServiceExcelAPI.Closexlsx(pexcel);

    return ProcRoad(pxodr,strtype,strradius,strroadlen,strlanewidth,strlannecount,strlanemarktype,strlanemarkcolor,strlanemarktype);
}

#include <QDateTime>
#include <OpenDrive/OpenDriveXmlWriter.h>
int CDAProc::ProcRoads(std::string strxlsxpath,std::string stroutpath)
{
    void * pexcel = ServiceExcelAPI.Openxlsx(strxlsxpath);
    std::string strtype;
    std::string strradius;
    std::string strroadlen;
    std::string strlanewidth;
    std::string strlannecount;
    std::string strlanetype;
    std::string strlanemarkcolor;
    std::string strlanemarktype;
    std::string strname;
    bool bComplete = false;
    int i = 4;
    int nr = 0;
    while(bComplete == false)
    {
        strtype = "";
        strname = "";
        ServiceExcelAPI.getcellvalue(pexcel,26,i,strname);
        ServiceExcelAPI.getcellvalue(pexcel,119,i,strtype);
        ServiceExcelAPI.getcellvalue(pexcel,120,i,strradius);
        ServiceExcelAPI.getcellvalue(pexcel,121,i,strroadlen);
        ServiceExcelAPI.getcellvalue(pexcel,122,i,strlanewidth);
        ServiceExcelAPI.getcellvalue(pexcel,123,i,strlannecount);
        //    ServiceExcelAPI.getcellvalue(pexcel,124,4,strlanetype);
        ServiceExcelAPI.getcellvalue(pexcel,124,i,strlanemarkcolor);
        ServiceExcelAPI.getcellvalue(pexcel,125,i,strlanemarktype);
        if((strtype == "")||(strname == ""))
        {
            bComplete = true;
        }
        else
        {
            std::cout<<" name : "<<strname<<" type: "<<strtype<<std::endl;
            OpenDrive xxodr;
            int nrtn = ProcRoad(&xxodr,strtype,strradius,strroadlen,strlanewidth,strlannecount,strlanemarktype,strlanemarkcolor,strlanemarktype);
            if(nrtn == 0)
            {
                if(xxodr.GetHeader() == NULL)
                {
                    xxodr.SetHeader(1,1,"adcmap",1.1,QDateTime::currentDateTime().toString("yyyy-MM-dd").toLatin1().data(),0,0,0,0,39,117,0);
                    xxodr.GetHeader()->SetVendor("adc");
                }
                else
                {
                    xxodr.GetHeader()->SetVendor("adc");
                }
                OpenDriveXmlWriter x(&xxodr);
                std::string  strfilepath = stroutpath + "/"+ strname+".xodr";
                x.WriteFile(strfilepath);
            }
            else
            {
                nr = nr + nrtn;
                std::cout<<" Convert row "<<i<<" name: "<<strname<<" fail. fail code : "<<nrtn<<std::endl;
            }
        }
        i++;
    }
    ServiceExcelAPI.Closexlsx(pexcel);
    return nr;
}

int CDAProc::ProcCDA(iv::map::cdadraw & xcdadraw,OpenDrive * pxodr)
{
    if(xcdadraw.mgeos_size() == 0)
    {
        std::cout<<" no geo. "<<std::endl;
        return -1;
    }
    int i;
    double xnow,ynow,hdgnow;
    xnow = 0;
    ynow = 0;
    hdgnow = 0;
    int nroadid = 0;
    for(i=0;i<xcdadraw.mgeos_size();i++)
    {
        iv::map::cdageo * pgeo = xcdadraw.mutable_mgeos(i);
        if(pgeo->geotype() == 0)
        {
            nroadid++;
            char strroadid[256];
            snprintf(strroadid,256,"%d",nroadid);
            pxodr->AddRoad("zl",pgeo->geolen(),strroadid,"-1");
            Road * pRoad = pxodr->GetLastAddedRoad();
            pRoad->AddGeometryBlock();
            GeometryBlock * pgeob = pRoad->GetLastAddedGeometryBlock();
            pgeob->AddGeometryLine(0,xnow,ynow,hdgnow,pgeo->geolen());
            pRoad->GetGeometryCoords(pgeo->geolen(),xnow,ynow,hdgnow);

            pRoad->AddLaneSection(0);
            LaneSection * pLS = pRoad->GetLaneSection(0);
            pLS->AddLane(0,0,"none",false);
            Lane * pcenterlane = pLS->GetLastAddedLane();
            iv::map::cdalanemarkline * pmark = xcdadraw.mutable_mlanemarkline(0);
            if(pmark == NULL)
                pcenterlane->AddRoadMarkRecord(0,"solid solid","standard","yellow",0.15,"none");
            else
            {
                pcenterlane->AddRoadMarkRecord(0,cda_lanemarktype_sel[pmark->lanemarktype()],"standard",
                        cda_lanemarkcolor_sel[pmark->lanemarkcolor()],pmark->lanemarkwidth(),"none");
                if(pmark->lanemarktype() == 0)
                {
                    LaneRoadMark * pLaneRoadMark = pcenterlane->GetLastAddedLaneRoadMark();
                    LaneRoadMarkType laneRoadMarkType(cda_lanemarktype_sel[pmark->lanemarktype()],pmark->lanemarkwidth());
                    laneRoadMarkType.AddLaneRoadMarkLine(pmark->lanemarklinelength(),pmark->lanemarklinespace(),0,0);
                    pLaneRoadMark->SetLaneRoadMarkType(laneRoadMarkType);
                }
            }

            int j;
            iv::map::cdadraw * pcdadraw = &xcdadraw;
            int nlanecount = pcdadraw->mlanes_size();
            for(j=0;j<nlanecount;j++)
            {
                iv::map::cdalane * pcdalane = pcdadraw->mutable_mlanes(j);
                pLS->AddLane(-1,(j+1)*(-1),cda_lanetype_sel[pcdalane->lanetype()],false,true);
                Lane * pnewlane = pLS->GetLastAddedLane();

                pnewlane->AddWidthRecord(0,pcdalane->lanewidth(),0,0,0);
                pmark = xcdadraw.mutable_mlanemarkline(j+1);
                if(pmark != NULL)
                {
                    pnewlane->AddRoadMarkRecord(0,cda_lanemarktype_sel[pmark->lanemarktype()],"standard",
                            cda_lanemarkcolor_sel[pmark->lanemarkcolor()],pmark->lanemarkwidth(),"none");
                    if(pmark->lanemarktype() == 0)
                    {
                        LaneRoadMark * pLaneRoadMark = pnewlane->GetLastAddedLaneRoadMark();
                        LaneRoadMarkType laneRoadMarkType(cda_lanemarktype_sel[pmark->lanemarktype()],pmark->lanemarkwidth());
                        laneRoadMarkType.AddLaneRoadMarkLine(pmark->lanemarklinelength(),pmark->lanemarklinespace(),0,0);
                        pLaneRoadMark->SetLaneRoadMarkType(laneRoadMarkType);
                    }
                }

                pLS->AddLane(1,(j+1)*(1),cda_lanetype_sel[pcdalane->lanetype()],false,true);
                pnewlane = pLS->GetLastAddedLane();
                pnewlane->AddWidthRecord(0,pcdalane->lanewidth(),0,0,0);
                pnewlane->AddRoadMarkRecord(0,cda_lanemarktype_sel[pmark->lanemarktype()],"standard",
                        cda_lanemarkcolor_sel[pmark->lanemarkcolor()],pmark->lanemarkwidth(),"none");
                if(pmark->lanemarktype() == 0)
                {
                    LaneRoadMark * pLaneRoadMark = pnewlane->GetLastAddedLaneRoadMark();
                    LaneRoadMarkType laneRoadMarkType(cda_lanemarktype_sel[pmark->lanemarktype()],pmark->lanemarkwidth());
                    laneRoadMarkType.AddLaneRoadMarkLine(pmark->lanemarklinelength(),pmark->lanemarklinespace(),0,0);
                    pLaneRoadMark->SetLaneRoadMarkType(laneRoadMarkType);
                }
            }
        }
        if(pgeo->geotype() == 1)
        {
            nroadid++;
            char strroadid[256];
            snprintf(strroadid,256,"%d",nroadid);
            pxodr->AddRoad("wd",pgeo->geolen(),strroadid,"-1");
            Road * pRoad = pxodr->GetLastAddedRoad();
            pRoad->AddGeometryBlock();
            GeometryBlock * pgeob = pRoad->GetLastAddedGeometryBlock();
            pgeob->AddGeometryArc(0,xnow,ynow,hdgnow,pgeo->geolen(),1.0/pgeo->georadius());
            pRoad->GetGeometryCoords(pgeo->geolen(),xnow,ynow,hdgnow);

            pRoad->AddLaneSection(0);
            LaneSection * pLS = pRoad->GetLaneSection(0);
            pLS->AddLane(0,0,"none",false);
            Lane * pcenterlane = pLS->GetLastAddedLane();
            iv::map::cdalanemarkline * pmark = xcdadraw.mutable_mlanemarkline(0);
            if(pmark == NULL)
                pcenterlane->AddRoadMarkRecord(0,"solid solid","standard","yellow",0.15,"none");
            else
            {
                pcenterlane->AddRoadMarkRecord(0,cda_lanemarktype_sel[pmark->lanemarktype()],"standard",
                        cda_lanemarkcolor_sel[pmark->lanemarkcolor()],pmark->lanemarkwidth(),"none");
                if(pmark->lanemarktype() == 0)
                {
                    LaneRoadMark * pLaneRoadMark = pcenterlane->GetLastAddedLaneRoadMark();
                    LaneRoadMarkType laneRoadMarkType(cda_lanemarktype_sel[pmark->lanemarktype()],pmark->lanemarkwidth());
                    laneRoadMarkType.AddLaneRoadMarkLine(pmark->lanemarklinelength(),pmark->lanemarklinespace(),0,0);
                    pLaneRoadMark->SetLaneRoadMarkType(laneRoadMarkType);
                }
            }

            int j;
            iv::map::cdadraw * pcdadraw = &xcdadraw;
            int nlanecount = pcdadraw->mlanes_size();
            for(j=0;j<nlanecount;j++)
            {
                iv::map::cdalane * pcdalane = pcdadraw->mutable_mlanes(j);
                pLS->AddLane(-1,(j+1)*(-1),cda_lanetype_sel[pcdalane->lanetype()],false,true);
                Lane * pnewlane = pLS->GetLastAddedLane();

                pnewlane->AddWidthRecord(0,pcdalane->lanewidth(),0,0,0);
                pmark = xcdadraw.mutable_mlanemarkline(j+1);
                if(pmark != NULL)
                {
                    pnewlane->AddRoadMarkRecord(0,cda_lanemarktype_sel[pmark->lanemarktype()],"standard",
                            cda_lanemarkcolor_sel[pmark->lanemarkcolor()],pmark->lanemarkwidth(),"none");
                    if(pmark->lanemarktype() == 0)
                    {
                        LaneRoadMark * pLaneRoadMark = pnewlane->GetLastAddedLaneRoadMark();
                        LaneRoadMarkType laneRoadMarkType(cda_lanemarktype_sel[pmark->lanemarktype()],pmark->lanemarkwidth());
                        laneRoadMarkType.AddLaneRoadMarkLine(pmark->lanemarklinelength(),pmark->lanemarklinespace(),0,0);
                        pLaneRoadMark->SetLaneRoadMarkType(laneRoadMarkType);
                    }
                }
                pLS->AddLane(1,(j+1)*(1),cda_lanetype_sel[pcdalane->lanetype()],false,true);
                pnewlane = pLS->GetLastAddedLane();
                pnewlane->AddWidthRecord(0,pcdalane->lanewidth(),0,0,0);
                pmark = xcdadraw.mutable_mlanemarkline(j+1);
                if(pmark != NULL)
                {
                    pnewlane->AddRoadMarkRecord(0,cda_lanemarktype_sel[pmark->lanemarktype()],"standard",
                            cda_lanemarkcolor_sel[pmark->lanemarkcolor()],pmark->lanemarkwidth(),"none");
                    if(pmark->lanemarktype() == 0)
                    {
                        LaneRoadMark * pLaneRoadMark = pnewlane->GetLastAddedLaneRoadMark();
                        LaneRoadMarkType laneRoadMarkType(cda_lanemarktype_sel[pmark->lanemarktype()],pmark->lanemarkwidth());
                        laneRoadMarkType.AddLaneRoadMarkLine(pmark->lanemarklinelength(),pmark->lanemarklinespace(),0,0);
                        pLaneRoadMark->SetLaneRoadMarkType(laneRoadMarkType);
                    }
                }
            }
        }
        if(pgeo->geotype() == 2)
        {
            ProcIntersectionRoad(pxodr,&xcdadraw,i,nroadid,xnow,ynow,hdgnow);
        }

    }

//    for(i=0;i<pxodr->GetRoadCount();i++)
//    {
//        Road * pRoad = pxodr->GetRoad(i);
//        pRoad->AddLaneSection(0);
//        LaneSection * pLS = pRoad->GetLaneSection(0);
//        pLS->AddLane(0,0,"none",false);
//        Lane * pcenterlane = pLS->GetLastAddedLane();
//        pcenterlane->AddRoadMarkRecord(0,"solid","standard","yellow",0.15,"none");

//        int i;
//        for(i=0;i<2;i++)
//        {
//            pLS->AddLane(-1,(i+1)*(-1),"driving",false,true);
//            Lane * pnewlane = pLS->GetLastAddedLane();
//            pnewlane->AddWidthRecord(0,3.5,0,0,0);

//                pnewlane->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"none");
//        }
//    }
    return 0;
}