#include "showxodrinvtk.h"

#include <OpenDrive/OpenDrive.h>
#include <OpenDrive/OpenDriveXmlParser.h>

#include "xodrfunc.h"


static int gnLineIndex = 0;


void DrawLine(std::vector<iv::LanePoint> & xveclastlp,std::vector<iv::LanePoint> & xveclp,pcl::visualization::PCLVisualizer& viewer, double  fz)
{


    unsigned int i,j;
    for(i=0;i<xveclp.size();i++)
    {
        iv::LanePoint xNow = xveclp[i];
        for(j=0;j<xveclastlp.size();j++)
        {
            if(xveclastlp[j].mnlane == xNow.mnlane)
            {
                iv::LanePoint xLast = xveclastlp[j];

                pcl::PointXYZ mass_center(xLast.mfX,xLast.mfY,fz);
                pcl::PointXYZ kinectZ(xNow.mfX,xNow.mfY,fz);

                char strgeoname[256];
                snprintf(strgeoname,256,"xodr%d",gnLineIndex);
                gnLineIndex++;
                if(xNow.mnlane != 0)
                    viewer.addLine(mass_center,kinectZ,1.0f,1.0f,1.0f,strgeoname);
                else
                    viewer.addLine(mass_center,kinectZ,1.0f,1.0f,0.0f,strgeoname);

                break;
            }
        }
    }
}

void ShowRoad(pcl::visualization::PCLVisualizer& viewer,Road * pRoad,double  fz)
{

    if(pRoad->GetLaneSectionCount()<1)
    {
        qDebug("no lane,return;");
        return;
    }



    double fRoadLen = pRoad->GetRoadLength();

    double s = 0;
    double fSpace = 0.1;

    double x,y,fhdg;
    double flasthdg = 0;
    double flastS = 0;
    pRoad->GetGeometryCoords(s,x,y,fhdg);
    std::vector<iv::LanePoint> xveclp = xodrfunc::GetAllLanePoint(pRoad,s,x,y,fhdg);
    std::vector<iv::LanePoint> xveclastlp = xveclp;
    flasthdg = fhdg;
    flastS = s;
    s = s+ fSpace;

    while(s<fRoadLen)
    {
        pRoad->GetGeometryCoords(s,x,y,fhdg);
        xveclp = xodrfunc::GetAllLanePoint(pRoad,s,x,y,fhdg);

        double fhdgdiff =  fhdg - flasthdg;
        double fSDiff = s - flastS;

        if((fabs(fhdgdiff) > 0.01) || (fSDiff>= 10.0))
        {

            //Draw Line
            DrawLine(xveclastlp,xveclp,viewer,fz);
            xveclastlp = xveclp;
            flasthdg = fhdg;
            flastS = s;
        }

        s= s + fSpace;
    }

    if(flastS<fRoadLen)
    {
        pRoad->GetGeometryCoords(fRoadLen,x,y,fhdg);
        xveclp = xodrfunc::GetAllLanePoint(pRoad,s,x,y,fhdg);
        DrawLine(xveclastlp,xveclp,viewer,fz);
    }




}

void ShowXODRINVTK(pcl::visualization::PCLVisualizer& viewer,double & flon,double & flat,bool & bxodr,double  fz)
{

    char strpath[256];
    snprintf(strpath,256,"%s/map/map.xodr",getenv("HOME"));

    OpenDrive xxodr;

    OpenDriveXmlParser xp(&xxodr);
    bxodr = xp.ReadFile(strpath);

    if(bxodr)
    {
        xxodr.GetHeader()->GetLat0Lon0(flat,flon);
    }

    unsigned int i;
    for(i=0;i<xxodr.GetRoadCount();i++)
    {
        ShowRoad(viewer,xxodr.GetRoad(i),fz);
    }


//    pcl::PointXYZ mass_center(0,0,0);
//    pcl::PointXYZ kinectZ(0,0,-100);

//    viewer.addLine(mass_center,kinectZ,1.0f,1.0f,1.0f,"xodr1");

//    pcl::PointXYZ kinectZ2(0,0,100);
//    viewer.addLine(mass_center,kinectZ2,1.0f,1.0f,1.0f,"xodr2");
}