#include "ivif_localplan.h"

#include "adc_tools/transfer.h"

ivif_localplan::ivif_localplan()
{

}

ivif_localplan::~ivif_localplan()
{

}

std::vector<iv::Point2D> ivif_localplan::GetLocalPlan(iv::GPS_INS now_gps_ins,int nPathPointIndex ,double fSpeed,std::vector<iv::GPSData> gpsMapLine,
                                                  iv::LidarGridPtr lidarGridPtr)
{
    (void)nPathPointIndex;
    (void)gpsMapLine;
    (void)lidarGridPtr;
    std::vector<iv::Point2D> xvectorlocal;
    xvectorlocal.clear();

    double fPreviewDistance = 100.0;


    if(nPathPointIndex<0)
    {
        return xvectorlocal;
    }

    int nsize = static_cast<int>( gpsMapLine.size());
    int i;

    iv::Point2D ptnow = iv::decition::Coordinate_Transfer(gpsMapLine[static_cast<unsigned int>(nPathPointIndex)]->gps_x,gpsMapLine[static_cast<unsigned int>(nPathPointIndex)]->gps_y, now_gps_ins);
    xvectorlocal.push_back(ptnow);
    double s = 0;

    for(i=(nPathPointIndex+1);i<nsize;i++)
    {
        iv::Point2D pt = iv::decition::Coordinate_Transfer(gpsMapLine[static_cast<unsigned int>(i)]->gps_x,gpsMapLine[static_cast<unsigned int>(i)]->gps_y, now_gps_ins);
        s = s + sqrt(pow(pt.y - xvectorlocal[xvectorlocal.size() -1].y,2)+pow(pt.x - xvectorlocal[xvectorlocal.size()-1].x,2));
        if(s<fPreviewDistance)
            xvectorlocal.push_back(pt);
        else
            break;

    }

    return xvectorlocal;
}