#include <iostream>
#include "include/optimal_planner.h"



#include "matplotlibcpp.h"




int main() {
    TebConfig config;
    std::vector<ObstaclePtr> obst_vector;
    ViaPointContainer* via_point = nullptr;
   // via_point->push_back({1,1});
    RobotFootprintModelPtr robot_model = std::make_shared<CircularRobotFootprint>(0.5);

    obst_vector.push_back( std::make_shared<PointObstacle>(0,0.5) );
    obst_vector.push_back( std::make_shared<PointObstacle>(0,0.5) );
    obst_vector.push_back( std::make_shared<PointObstacle>(0,-0.2) );
    obst_vector.push_back(std::make_shared<PointObstacle>(0,-0.5));

    obst_vector.push_back( std::make_shared<LineObstacle>(0,1.5,0,-1.5) ); //90 deg

    obst_vector.push_back( std::make_shared<LineObstacle>(-8,1,8,1) );
    obst_vector.push_back( std::make_shared<LineObstacle>(-8,-1,-2,-1) );
    obst_vector.push_back( std::make_shared<LineObstacle>(2,-1,8,-1) );
    obst_vector.push_back( std::make_shared<LineObstacle>(-2,-1,-2,-2) );
    obst_vector.push_back( std::make_shared<LineObstacle>(2,-1,2,-2) );
    obst_vector.push_back( std::make_shared<LineObstacle>(-2,-2,2,-2) );

    //obst_vector.push_back( std::make_shared<LineObstacle>(1,0,-1,0) ); //180 deg

    // Dynamic obstacles
    Eigen::Vector2d vel (0, 0);
    obst_vector.at(0)->setCentroidVelocity(vel);
    vel = Eigen::Vector2d(0, 0);
    obst_vector.at(1)->setCentroidVelocity(vel);

    std::shared_ptr<PlannerInterface> optimizer(new TebOptimalPlanner(config, &obst_vector, robot_model,via_point));

//    time_t start_time = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
//    optimizer->plan(PoseSE2(-4,0,0),PoseSE2(4,0,0));
//    time_t end_time = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
//
//    std::cout << "time duration = " << end_time - start_time << std::endl;





    TimedElasticBand* teb_ptr_for_visualize;
    int teb_point_num;
    teb_ptr_for_visualize = optimizer->get_teb();
    std::vector<double> x, y;
    std::vector<double> obs_x,obs_y;
    std::vector<double> line_obs_x0,line_obs_y0,line_obs_x1,line_obs_y1,line_obs_x2,line_obs_y2
    ,line_obs_x3,line_obs_y3,line_obs_x4,line_obs_y4,line_obs_x5,line_obs_y5;
    obs_x.push_back(-7);
    obs_x.push_back(-6);
    obs_x.push_back(0);
    obs_x.push_back(0);
    obs_y.push_back(2);
    obs_y.push_back(2);
    obs_y.push_back(2);
    obs_y.push_back(-0.5);

    line_obs_x0.push_back(-8);
    line_obs_x0.push_back(8);
    line_obs_y0.push_back(1);
    line_obs_y0.push_back(1);

    line_obs_x1.push_back(-8);
    line_obs_x1.push_back(-2);
    line_obs_y1.push_back(-1);
    line_obs_y1.push_back(-1);

    line_obs_x2.push_back(2);
    line_obs_x2.push_back(8);
    line_obs_y2.push_back(-1);
    line_obs_y2.push_back(-1);

    line_obs_x3.push_back(-2);
    line_obs_x3.push_back(-2);
    line_obs_y3.push_back(-1);
    line_obs_y3.push_back(-2);

    line_obs_x4.push_back(2);
    line_obs_x4.push_back(2);
    line_obs_y4.push_back(-1);
    line_obs_y4.push_back(-2);

    line_obs_x4.push_back(-2);
    line_obs_x4.push_back(2);
    line_obs_y4.push_back(-2);
    line_obs_y4.push_back(-2);

    for(int num = 0;num < 10;++num){

        if(!x.empty() && !y.empty()){
            x.clear();
            y.clear();
        }

        std::chrono::high_resolution_clock::time_point beginTime = std::chrono::high_resolution_clock::now();
        optimizer->plan(PoseSE2(-4,0,0),PoseSE2(4,0,0), nullptr, false);
        std::chrono::high_resolution_clock::time_point endTime = std::chrono::high_resolution_clock::now();
        std::chrono::milliseconds timeInterval = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - beginTime);
        std::cout << timeInterval.count() << "ms\n" << std::endl;



        teb_point_num = teb_ptr_for_visualize->sizePoses();

        //可视化轨迹点
        for(int i = 0; i < teb_point_num; ++i){

            x.push_back(teb_ptr_for_visualize->Pose(i).x());
            y.push_back(teb_ptr_for_visualize->Pose(i).y());

        }
        matplotlibcpp::plot(x,y,":");
        matplotlibcpp::scatter(obs_x,obs_y,100);
        matplotlibcpp::plot(line_obs_x0,line_obs_y0,"r--");
        matplotlibcpp::plot(line_obs_x1,line_obs_y1,"r--");
        matplotlibcpp::plot(line_obs_x2,line_obs_y2,"r--");
        matplotlibcpp::plot(line_obs_x3,line_obs_y3,"r--");
        matplotlibcpp::plot(line_obs_x4,line_obs_y4,"r--");
        matplotlibcpp::plot(line_obs_x5,line_obs_y5,"r--");
        matplotlibcpp::xlabel("x ");
        matplotlibcpp::ylabel("y ");


        matplotlibcpp::xlim(-8, 8);
        matplotlibcpp::ylim(-5,5);
       // matplotlibcpp::legend();
        matplotlibcpp::pause(2);
    }

    return 0;
}