#include "odtolanelet.h"

#include <vector>

#include "gaussprojector.h"



namespace iv
{





}

odtolanelet::odtolanelet(std::string strxodrfile)
{
    mstrxodrfile = strxodrfile;
}


int odtolanelet::ConvertToLanelet(std::string strlanelet2file)
{


    OpenDrive * pxodr = new OpenDrive();  //because add to xodr,so don't delete
    OpenDriveXmlParser x(pxodr);
    if(!x.ReadFile(mstrxodrfile))
    {
        std::cout<<"Can't  load xodr file."<<std::endl;
        return -1;
    }

    lanelet::LaneletMapPtr laneletMap  = std::make_shared<lanelet::LaneletMap>();
    unsigned int nroadnum = pxodr->GetRoadCount();
    unsigned int i;
    for(i=0;i<nroadnum;i++)
    {
        Road * pRoad = pxodr->GetRoad(i);
        lanelet::Lanelet let;
        if(Road2Lanelet(pRoad,laneletMap) == 0)
        {

        }

    }

    lanelet::Origin origin2({39.1364713, 117.0866293, 0});

    lanelet::write(strlanelet2file,*laneletMap,GaussProjector(origin2));

    std::cout<<"convert complete."<<std::endl;
//    lanelet::write(strlanelet2file,*laneletMap,origin2);

    return 0;
}

int odtolanelet::Road2Lanelet(Road * pRoad,lanelet::LaneletMapPtr & laneletMap)
{


    if(pRoad == NULL)
    {
        std::cout<<"odtolanelet::Road2Lanelet Road is NULL. "<<std::endl;
        return -1;
    }

    std::shared_ptr<iv::RoadSample> pSample_ptr = std::shared_ptr<iv::RoadSample>(new iv::RoadSample(pRoad));
 //   iv::RoadSample * pSample = new iv::RoadSample(pRoad);

    std::vector<iv::RoadPoint> * pvectorRoadPoint = pSample_ptr->GetRoadPointVector();

    unsigned int nRPSize = static_cast<unsigned int>(pvectorRoadPoint->size());

    unsigned int i;

    std::vector<lanelet::LineString3d> xvectorlinestring;
    std::vector<lanelet::Point3d> xvectorPoint;
    std::vector<lanelet::Point3d> xvectorLastPoint;

    unsigned int ncenterlanepos = 0;

    for(i=0;i<nRPSize;i++)
    {
        bool bNewLet = false;
        if(i == 0)
        {
            bNewLet = true;
        }
        else
        {
            if(pvectorRoadPoint->at(i).nSecNum != pvectorRoadPoint->at(i-1).nSecNum)
            {
                //Create LaneLet

                linetolet(laneletMap,xvectorlinestring,ncenterlanepos);
                bNewLet = true;
            }
        }

        if(bNewLet)
        {
            xvectorlinestring.clear();

            unsigned int nlanesize = static_cast<unsigned int>(pvectorRoadPoint->at(i).mvectorLanePoint.size());

            unsigned int j;
            for(j=0;j<nlanesize;j++)
            {
                lanelet::LineString3d xline;
                xline.setId(mid);mid++;
                odtypetolettype(pvectorRoadPoint->at(i).mvectorLanePoint[j],xline);
                xvectorlinestring.push_back(xline);
                if(pvectorRoadPoint->at(i).mvectorLanePoint[j].nLane == 0)
                {
                    ncenterlanepos = j;
                }
            }

            if(i== 0)
            {
                LanePointToPoint3D(pvectorRoadPoint->at(i).mvectorLanePoint,xvectorPoint);
                unsigned int nlanesize = static_cast<unsigned int>(pvectorRoadPoint->at(i).mvectorLanePoint.size());

                unsigned int j;
                for(j=0;j<nlanesize;j++)xvectorlinestring[j].push_back(xvectorPoint[j]);
            }
            else
            {
                LanePointToPoint3DWithLast(pvectorRoadPoint->at(i).mvectorLanePoint,xvectorPoint,xvectorLastPoint);
                unsigned int nlanesize = static_cast<unsigned int>(pvectorRoadPoint->at(i).mvectorLanePoint.size());

                unsigned int j;
                for(j=0;j<nlanesize;j++)xvectorlinestring[j].push_back(xvectorPoint[j]);
            }

        }
        else
        {
            LanePointToPoint3D(pvectorRoadPoint->at(i).mvectorLanePoint,xvectorPoint);
            unsigned int nlanesize = static_cast<unsigned int>(pvectorRoadPoint->at(i).mvectorLanePoint.size());

            unsigned int j;
            for(j=0;j<nlanesize;j++)xvectorlinestring[j].push_back(xvectorPoint[j]);
        }

        xvectorLastPoint = xvectorPoint;
//        pvectorRoadPoint->at(i).mvectorLanePoint
    }

    if(xvectorlinestring.size()>0)
    {
        linetolet(laneletMap,xvectorlinestring,ncenterlanepos);
    }

    return  0;

}

void odtolanelet::odtypetolettype(iv::LanePoint & xLanePoint, lanelet::LineString3d & xlinestring)
{
    if(xLanePoint.mstrroadmarkcolor.size()>0)
    {
        xlinestring.setAttribute("color",lanelet::Attribute(xLanePoint.mstrroadmarkcolor));
    }
    if(xLanePoint.mstrroadmarktype == "none")
    {
        xlinestring.setAttribute("type",lanelet::Attribute("virtual"));
        return;
    }
    if(xLanePoint.mstrroadmarktype == "solid")
    {
        xlinestring.setAttribute("type",lanelet::Attribute("line_thin"));
        xlinestring.setAttribute("subtype",lanelet::Attribute("solid"));
        return;
    }
    if(xLanePoint.mstrroadmarktype == "broken")
    {
        xlinestring.setAttribute("type",lanelet::Attribute("line_thin"));
        xlinestring.setAttribute("subtype",lanelet::Attribute("solid"));
        return;
    }
    if(xLanePoint.mstrroadmarktype == "broken")
    {
        xlinestring.setAttribute("type",lanelet::Attribute("line_thin"));
        xlinestring.setAttribute("subtype",lanelet::Attribute("dashed"));
        return;
    }
    if(xLanePoint.mstrroadmarktype == "solid solid")
    {
        xlinestring.setAttribute("type",lanelet::Attribute("line_thin"));
        xlinestring.setAttribute("subtype",lanelet::Attribute("solid_solid"));
        return;
    }
    if(xLanePoint.mstrroadmarktype == "solid broken")
    {
        xlinestring.setAttribute("type",lanelet::Attribute("line_thin"));
        xlinestring.setAttribute("subtype",lanelet::Attribute("solid_dashed"));
        return;
    }
    if(xLanePoint.mstrroadmarktype == "broken solid")
    {
        xlinestring.setAttribute("type",lanelet::Attribute("line_thin"));
        xlinestring.setAttribute("subtype",lanelet::Attribute("dashed_solid"));
        return;
    }
    if(xLanePoint.mstrroadmarktype == "curb")
    {
        xlinestring.setAttribute("type",lanelet::Attribute("curbstone"));
        xlinestring.setAttribute("subtype",lanelet::Attribute("high"));
        return;
    }
    if(xLanePoint.mstrroadmarktype == "edge")
    {
        xlinestring.setAttribute("type",lanelet::Attribute("road_border"));
        return;
    }
    xlinestring.setAttribute("type",lanelet::Attribute("virtual"));
    return;
}

void odtolanelet::LanePointToPoint3D(std::vector<iv::LanePoint> & xvectorLanePoint, std::vector<lanelet::Point3d> & xvectorPoint3D)
{
    xvectorPoint3D.clear();
    unsigned int i;
    unsigned int nLPsize = static_cast<unsigned int>(xvectorLanePoint.size());
    for(i=0;i<nLPsize;i++)
    {
        lanelet::Point3d xPoint(mid,xvectorLanePoint[i].mx,xvectorLanePoint[i].my,xvectorLanePoint[i].mz);
        mid++;
        xvectorPoint3D.push_back(xPoint);
    }
}

void odtolanelet::LanePointToPoint3DWithLast(std::vector<iv::LanePoint> & xvectorLanePoint, std::vector<lanelet::Point3d> & xvectorPoint3D,
                                std::vector<lanelet::Point3d> & xvectorPoint3DLast)
{
    xvectorPoint3D.clear();
    unsigned int i;
    unsigned int nLPsize = static_cast<unsigned int>(xvectorLanePoint.size());
    for(i=0;i<nLPsize;i++)
    {
        unsigned int j;
        unsigned int nLastSize = static_cast<unsigned int>(xvectorPoint3DLast.size());
        bool bUseOld = false;
        for(j=0;j<nLastSize;j++)
        {
            if((fabs(xvectorLanePoint[i].mx - xvectorPoint3DLast[j].x())<0.001)&(fabs(xvectorLanePoint[i].my - xvectorPoint3DLast[j].y())<0.001))
            {
                bUseOld = true;
                xvectorPoint3D.push_back(xvectorPoint3DLast[j]);
                break;
            }
        }
        if(bUseOld == false)
        {
            lanelet::Point3d xPoint(mid,xvectorLanePoint[i].mx,xvectorLanePoint[i].my,xvectorLanePoint[i].mz);
            mid++;
            xvectorPoint3D.push_back(xPoint);
        }

    }
}

void odtolanelet::linetolet(lanelet::LaneletMapPtr & laneletMap,std::vector<lanelet::LineString3d> & xvectorlinestring, unsigned int ncenterlane)
{
    unsigned int i;
    unsigned int nsize = static_cast<unsigned int>(xvectorlinestring.size()) ;
    for(i=0;i<ncenterlane;i++)
    {
        lanelet::LineString3d xleft;
        lanelet::LineString3d xright ;
        InvertLine(xleft,xvectorlinestring[i+1]);
        InvertLine(xright,xvectorlinestring[i]);
        lanelet::Lanelet xlet(mid,xleft,xright);
        mid++;
        laneletMap->add(xlet);
    }
    for(i=ncenterlane;i<(nsize-1);i++)
    {
        lanelet::Lanelet xlet(mid,xvectorlinestring[i],xvectorlinestring[i+1]);
        mid++;
        laneletMap->add(xlet);
    }
}

int odtolanelet::InvertLine(lanelet::LineString3d & xinvert,lanelet::LineString3d & xraw)
{
    xinvert.setId(mid);mid++;
    if(xraw.hasAttribute("color"))xinvert.setAttribute("color",xraw.attribute("color"));
    if(xraw.hasAttribute("subtype"))xinvert.setAttribute("subtype",xraw.attribute("subtype"));
    if(xraw.hasAttribute("type"))xinvert.setAttribute("type",xraw.attribute("type"));
    xinvert.clear();
    unsigned int npointssize = static_cast<unsigned int >(xraw.size());
    unsigned int i;
    for(i=0;i<npointssize;i++)
    {
        xinvert.push_back(xraw[npointssize-1-i]);
    }
    return 0;
}