#ifndef __GEN_TRAJECTORY__
#define __GEN_TRAJECTORY__
/*
 * 三次曲线获取轨迹，两点坐标+两点梯度 求曲线参数
 */
#include <tf/tf.h>
#include "tf2_msgs/TFMessage.h"
#include <boost/thread.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <vector>

double get_pose_angle_z(tf::Transform pos);
double get_gradient_by_tf(tf::Transform pos);
tf::Transform get_tf(double x,double y,double gradient);
tf::Transform transform_tf(tf::Transform pos,tf::Transform tf);
tf::Pose Rotate_pose(tf::Pose pose,double angle);


class MakeTrajectory
{
public:
    MakeTrajectory();
    bool TransformTrajectory(tf::Transform tf);
    void SetLRTransform(tf::Transform letf,tf::Transform right);
    tf::Transform operator[](int n);
    int Size(){return trajectory_.size();}
    virtual bool Make(tf::Transform t0,tf::Transform t1, int extend_num=0);
    bool IsPoseInTrajectory(tf::Transform pose);
protected:
    //计算当前x点的 角度变化率
    double threta_gradient(double x);
    double  gen_axis_angle();
public:
    std::vector<tf::Transform>      trajectory_;                //雷达轨迹
protected:
    tf::Transform                   position0_;
    tf::Transform                   position1_;

    tf::Transform                   left_tf_;
    tf::Transform                   right_tf_;
    Eigen::MatrixXd                 line_param_;                //曲线参数

    boost::mutex	                mutex_trajectory_;
};

#endif