#ifndef TRAJECTORY_TOOL_HH
#define TRAJECTORY_TOOL_HH

#include <mutex>
#include <Eigen/Core>
#include <Eigen/Geometry>
// #include <opencv2/core.hpp>
// 使用eigen中向量构成路径类型
typedef std::vector<Eigen::Isometry2d, Eigen::aligned_allocator<Eigen::Isometry2d>> trajectory_type;
/**
 * 路径功能类
 * 用于根据起始点与朝向、目标点与朝向生成路径
 * */
class Trajectory_tool
{
public:
    // 构造函数
    Trajectory_tool();
    // 析构函数
    ~Trajectory_tool();
    // 生成路径
    bool create_trajectory(Eigen::Isometry2d base, Eigen::Isometry2d target, trajectory_type &trajectory, int extend_num=0);
    // 获取已生成的路径
    bool get_trajectory(trajectory_type &trajectory);
    // 获取下标对应的路径上的点
    Eigen::Isometry2d operator[](int trajectory_index);

private:
    // 处理90度附近斜率无限大的情况,角度(sin,cos)近似为89.5度(0.99995, 0.01)或90.5度(0.99995,-0.01)
    void max_gradient_limit(Eigen::Matrix2d &rotation);

private:
    // 路径访问互斥锁
    std::mutex m_mutex_trajectory;
    // 当前生成路径
    trajectory_type m_trajectory;

};

#endif // !TRAJECTORY_TOOL_HH