// boost::atomic<int> i(0);
int i=0;
boost::mutex mut;
void thread_1(int n);

int main()
{
    int n=100000;   //  n不够大时，i不容易出现不同的情况
    boost::thread th1 = boost::thread(boost::bind(&thread_1,n));
    boost::thread th2 = boost::thread(boost::bind(&thread_1,n));
    th1.join();
    th2.join();
    cout<< i<<endl;
    return 0;
}

void thread_1(int n)
{
    while(n--)
    {
       mut.lock();
        i++;
       mut.unlock();
    }
}

atomic<int> a(10);
assert(a==10);      //安全函数，若表达式不成立结束程序


atomic<long> L;
cout << L <<endl;    //初始值不确定

boost::atomic<bool> b(1);
assert(b != 0);
std::cout << b << std::endl;
b.store(0);//存值
std::cout << b << std::endl;

boost::atomic<int> n1(100);
std::cout << n1.exchange(200) << std::endl;  //交换两个值，并且返回原值100
std::cout << n1 << std::endl;

int a[] = { 1, 3, 5, 2, 9, 6, 8 };
std::sort(std::begin(a), std::end(a) );

vector<int> num_list = { 2,4,6,8,10,12 };
int find_num = 8;    //要查找的元素,类型要与vector中的元素类型一致
std::vector<int>::iterator num = std::find(num_list.begin(), num_list.end(), find_num);  //返回一个迭代器指针

if (num != num_list.end())   //查找成功
{
    cout << "其索引为 " << distance(num_list.begin(), num) << endl;   
}
else
    cout<< "元素 " << find_num << " 不在num_list中" << endl;

std::copy(src.begin(), src.end(), std::back_inserter(dest));

vector<int>  vec;
vec.push_back(1);
vec.push_back(2);
vec.push_back(3);
vec.push_back(4);
vec.push_back(5);

vector<int>  vv;
std::copy(vec.begin(), vec.end(), std::back_inserter(vv) );
for(const auto &it: vv)
    cout << it << "  ";

std::vector<int> vec = {1, 2, 3, 4, 5};
std::fill(vec.begin(), vec.end(), 0); // 将 vec 中的所有元素设置为 0

template< class InputIt >
typename std::iterator_traits<InputIt>::difference_type
    distance( InputIt first, InputIt last );

std::vector<int> v{3, 1, 4, 1, 5, 9};
std::cout << "distance: " << std::distance(v.begin(), v.end()) << std::endl;   // 6

#include <vector>
#include <iostream>
#include <iterator>
#include <algorithm>
 
int main()
{
    std::vector<int> v({1,2,3});
    std::reverse(std::begin(v), std::end(v));
    std::cout << v[0] << v[1] << v[2] << '\n';
 
    int a[] = {4, 5, 6, 7};
    std::reverse(std::begin(a), std::end(a));
    std::cout << a[0] << a[1] << a[2] << a[3] << '\n';
}

vector<int>  vec;
vec.push_back(1);
vec.push_back(2);
vec.push_back(3);
vec.push_back(4);
vec.push_back(5);

for(const auto &it: vec)
{
    cout << it << "  ";
}
cout << endl;
int num = std::accumulate(vec.begin(), vec.end(), 0, [](int input){return 2*input; }  );

int b = 3, c = 12;
qDebug() << std::greater<int>()(b, c);
qDebug() << std::less<int>()(b, c);

int a[]={3,4,1,12,0,8,4};
// 默认从小到大排列
std::sort(a, a+7);
// 从大到小
std::sort(a, a+7, std::greater<int>());
for(int i=0; i < 7; i++)
    cout << a[i] <<"  ";
// 0  1  3  4  4  8  12

vector<int>  vec;
vec.push_back(1);
vec.push_back(2);
vec.push_back(3);
vec.push_back(4);
vec.push_back(5);

for(const auto &it: vec)
{
    cout << it << "  ";
}
cout << endl;
std::sort(vec.begin(), vec.end(),
          [](int a, int b){
            return  (a%2) < (b%2);
            }
          );
for(auto it : vec)
{
    cout << it << "  ";
}

struct Frontier
{
    Frontier(double c):
        cost(c){}

    double cost;
};

bool compareFrontier(Frontier f1, Frontier f2)
{
    return (f1.cost < f2.cost);
}
void showFrontiers(const vector<Frontier>& v)
{
    for(auto f: v)
        cout << f.cost << "  ";
    cout << endl;
}

vector<Frontier> frontiers;
frontiers.push_back(Frontier(1.2) );
frontiers.push_back(Frontier(4.9) );
frontiers.push_back(Frontier(12.7) );
frontiers.push_back(Frontier(0.3) );
frontiers.push_back(Frontier(3.6) );
showFrontiers(frontiers);

std::sort(frontiers.begin(), frontiers.end(),
      [](const Frontier& f1, const Frontier& f2) { return f1.cost < f2.cost; });
// std::sort(frontiers.begin(), frontiers.end(), compareFrontier);

showFrontiers(frontiers);

1.2  4.9  12.7  0.3  3.6  
0.3  1.2  3.6  4.9  12.7  

#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;
}

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;
          }
        }
      }
    }
}