#include <adc_planner/dubins_planner.h>
#include <adc_tools/transfer.h>
#include <adc_tools/parameter_status.h>
#include <common/constants.h>

#include <math.h>

std::vector<iv::GPSData> iv::decition::DubinsPlanner::gpsMap;

iv::decition::DubinsPlanner::DubinsPlanner(){
    this->planner_id = 2;
    this->planner_name = "Dubins";
}

iv::decition::DubinsPlanner::~DubinsPlanner(){
}

/**
 * @brief iv::decition::LaneChangePlanner::getPath
 * 采用的车道变换的局部路径生成方法。计算原始地图路径偏移offset距离之后的路径。
 *
 * @param now_gps_ins          实时gps信息
 * @param gpsMapLine           地图数据点
 * @param PathPoint            地图路线中距离车辆位置最近的一个点的序号
 * @param offset               在车辆避障中，地图源路线所在车道 与 即将去往的车道 之间的距离差值。可以为负值。
 * @param lidarGridPtr         激光雷达信息网格
 * @return                     返回一条车辆坐标系下的路径
 */
std::vector<iv::Point2D> iv::decition::DubinsPlanner::getPath(GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint, double offset, iv::LidarGridPtr lidarGridPtr){
    std::vector<iv::Point2D> trace;

    double L = 2.6;
    double R = L/sin(10.0*M_PI/180.0);
    std::cout<<" R = "<<R<<std::endl;
//    return a.exec();
//    double q0[] = { 0,0,1.57};
//    double q1[] = { 4,4,3.142 };
    gpsMap.clear();
    double q0[] = { now_gps_ins.gps_x, now_gps_ins.gps_y, now_gps_ins.ins_heading_angle};
    double q1[] = { gpsMapLine[PathPoint]->gps_x, gpsMapLine[PathPoint]->gps_y ,gpsMapLine[PathPoint]->ins_heading_angle };
    double turning_radius = 10.0;
    DubinsPath path;
    dubins_shortest_path( &path, q0, q1, turning_radius);
    dubins_path_sample_many( &path, 0.1,  printConfiguration, NULL);


}

int  iv::decition::DubinsPlanner::printConfiguration(double q[3], double x, void* user_data) {
    GPSData gps;
    gps->roadMode=5;
    gps->gps_x=q[0];
    gps->gps_y=q[1];
    gps->ins_heading_angle=q[3];
    gpsMap.push_back(gps);
    return 0;
}