#include <fstream>

std::ifstream fin(file_name);
if (!fin) {
    printf("file not exist");
    return false;
}
fin.close();

#include <filesystem>
#include <iostream>
namespace fs = std::filesystem;


int main()
{
    std::filesystem::path test_path("/home/user/test.yaml");
    //判断路径是否存在
    std::cout << "is_exist = " << std::filesystem::exists(test_path) << std::endl;

    // 判断路径为绝对路径还是相对路径
    std::cout << "is_abs = " << test_path.is_absolute() << std::endl;
    std::cout << "is_relative = " << test_path.is_relative() << std::endl;
    // 取得不带扩展名的文件名
    std::cout << "stem = " << test_path.stem() << std::endl;
    // 取得扩展名,如果没有,则为空
    std::cout << "extension = " << test_path.extension() << std::endl;
    // 取得文件名
    std::cout << "filename = " << test_path.filename() << std::endl;

    std::cout << "----------------------------------------" << std::endl;

    std::filesystem::path test_dir("/home/user/Pictures");
    //判断是文件还是文件夹
    std::cout << "is_file = " << std::filesystem::is_regular_file(test_path) << std::endl;
    std::cout << "is_dir = " << std::filesystem::is_directory(test_dir) << std::endl;

    //关于路径拼接
    std::filesystem::path new_path = test_dir / "test.txt";
    std::cout << "new_path = " << new_path << std::endl;

    //取得当前绝对工作路径 编译后可执行文件的路径文件夹
    std::filesystem::path workPath = std::filesystem::current_path();
    std::cout << "current exe path = " << workPath << std::endl;

    //递归遍历指定路径下所有文件,文件夹名字也会返回
    std::filesystem::recursive_directory_iterator iterDir(test_dir);
    for (auto &it: iterDir)
    {
        //打印绝对路径
        std::cout << it << std::endl;
        // 不打印路径而是打印文件名
       std::cout << it.path().filename() << std::endl;
    }

    //给一个相对路径,返回绝对路径
    std::filesystem::path path_test("./");
    std::cout << "abs_path = " << std::filesystem::absolute(path_test) << std::endl;


    // 删除文件, 路径不存在不报错
//    std::filesystem::remove(test_dir / "deepfakes"/"yolov7.pdf");

    // 拷贝文件夹, 删除文件夹
//    std::filesystem::copy(test_dir, test_dir.parent_path() / "works", std::filesystem::copy_options::recursive);
//    std::filesystem::remove_all(test_dir.parent_path() / "works");
    return 0;
}

warning: exception of type ‘std::bad_alloc’ will be caught
 catch (bad_alloc e){ cout << e.what() << endl;}
 ^~~~~
 
warning:    by earlier handler for ‘std::exception’
 catch (exception e){ cout <<  e.what() << endl;}

int main()
{  
    try {  
        throw DerivedException();  
    }
    catch (DerivedException& e) { // Handle derived class first  
        std::cerr << "Caught DerivedException: " << e.what() << std::endl;  
    }
    catch (BaseException& e) { // Then handle base class  
        std::cerr << "Caught BaseException: " << e.what() << std::endl;  
    }  
    return 0;  
}

YAML::Node gps_root;
try {
    gps_root = YAML::LoadFile("/home/user/111.yaml");
} catch (YAML::ParserException &ex) {
    std::cerr << "gps.yaml parse failed: " << ex.what() << std::endl;
   exit(-1);
} catch (YAML::BadFile &ex) {
    std::cerr << "gps.yaml load failed: " << ex.what() << std::endl;
   exit(-1);
}

Node LoadFile(const std::string& filename) {
  std::ifstream fin(filename.c_str());
  if (!fin) {
    throw BadFile();
  }
  return Load(fin);
}

Node Load(std::istream& input)
{
	Parser parser(input);
	NodeBuilder builder;
	if (!parser.HandleNextDocument(builder)) {
		return Node();
	}

	return builder.Root();
}

double planner::distance(double x1,double y1,double x2,double y2)
{
    return sqrt( (x1-x2) * (x1-x2) + (y1-y2) * (y1-y2) );
}

bool planner::isAroundFree(unsigned int mx, unsigned int my)
{
    if(mx <= 1 || my <= 1 || mx >= this->costmap_->getSizeInCellsX()-1 || my >= this->costmap_->getSizeInCellsY()-1)
      return false;
    int x,y;
    for(int i=-1;i<=1;i++)
    {
      for(int j=-1;j<=1;j++)
      {
        x = static_cast<int>(mx) + i;
        y = static_cast<int>(my) + j;
        if(this->costmap_->getCost(static_cast<unsigned int>(x),static_cast<unsigned int>(y)) != costmap_2d::FREE_SPACE)
          return false;
      }
    }
    return true;
}

// tolerance：搜索半径,单位米。
// in：输入的点
// out：搜索到的可行点
void planner::getNearFreePoint(const geometry_msgs::PoseStamped in,
                                       geometry_msgs::PoseStamped& out,
                                       double tolerance)
{
    out = in;
    unsigned int grid_size = static_cast<unsigned int>(tolerance/costmap_->getResolution() + 0.5);
    if(grid_size<1)
    {
      out = in;
      return;
    }

    unsigned int mx0,my0;
    if(costmap_->worldToMap(in.pose.position.x,in.pose.position.y,mx0,my0))
    {
      if(this->isAroundFree(mx0,my0))
        return;
      unsigned int minx,maxx,miny,maxy;
      double wx = 0.0,wy = 0.0;
      double min_move_cost = 10000000.0;
      minx = mx0-grid_size>0?mx0-grid_size:0;
      maxx = mx0+grid_size<costmap_->getSizeInCellsX()?mx0+grid_size:costmap_->getSizeInCellsX();
      miny = my0-grid_size>0?my0-grid_size:0;
      maxy = my0+grid_size<costmap_->getSizeInCellsY()?my0+grid_size:costmap_->getSizeInCellsY();
      for(unsigned int i=minx;i<=maxx;i++)
      {
        for(unsigned int j=miny;j<=maxy;j++)
        {
          costmap_->mapToWorld(i,j,wx,wy);
          double current_move_cost = this->distance(in.pose.position.x,in.pose.position.y,wx,wy);
          if(!this->isAroundFree(i,j) || current_move_cost > tolerance)
            continue;
          if(min_move_cost > current_move_cost)
          {
            min_move_cost = current_move_cost;
            out.pose.position.x = wx;
            out.pose.position.y = wy;
          }
        }
      }
    }
}

syntax = "proto3";
// 包名:在生成对应的C++文件时，将被替换为名称空间，在代码中会有体现
package Test; 

message Pose {
    float x = 1;
    float y = 2;
    float yaw = 3;
}

Test::Pose*  pose_msg = new Test::Pose();
pose_msg->set_x(1.5);
pose_msg->set_y(2.7);
pose_msg->set_yaw(0);

int main()
{
    int num = 0;
    int count = 0;
    std::chrono::steady_clock::time_point  start, end;
    double duration;

    while(1)
    {
        std::srand(time(0));
        count = rand() % 1000000;
        if(count > 680000)
        {
            if(num==0)
                start = std::chrono::steady_clock::now();

            num++;
            end = std::chrono::steady_clock::now();
            duration = std::chrono::duration<double>(end - start).count();
            cout << "count: " << count << endl;
            cout << "time elapsed " << static_cast<int>(duration) << endl;
        }
        else
        {
            duration = 0;
            num = 0;
            cout << endl;
        }
    }
    return 0;
}

/*
 * @tparam T point type
 * @param first Given the starting point of a line segment
 * @param last The endpoint of a given line segment
 * @param third Given point
 * @return T Distance from point to line
 */
template <typename T>
static T PointToLineDistance(const Point<T>& first, const Point<T>& last, const Point<T>& third) {
    float dis_suqare =
        ((first.y - last.y) * third.x + (last.x - first.x) * third.y +
         (first.x * last.y - last.x * first.y)) *
        ((first.y - last.y) * third.x + (last.x - first.x) * third.y +
         (first.x * last.y - last.x * first.y)) /
        ((last.x - first.x) * (last.x - first.x) + (last.y - first.y) * (last.y - first.y));
    return std::sqrt(dis_suqare);
}