//
// Created by zx on 22-12-1.
//

#ifndef LIO_LIVOX_CPP_MPC_TRAJECTORY_H_
#define LIO_LIVOX_CPP_MPC_TRAJECTORY_H_

#include "pose2d.h"
#include <vector>
#include <Eigen/Core>
#include <Eigen/Dense>

class Trajectory
{
 public:
    Trajectory();
    Trajectory(const std::vector<Pose2d>& points);
    Trajectory(const Trajectory& other)= default;
    Trajectory& operator=(const Trajectory& other)= default;
    Pose2d& operator[](int index);
    Trajectory& operator+(const Trajectory& other);
    unsigned int size()const{return m_pose_vector.size();}
    void clear(){m_pose_vector.clear();}
    void push_point(const Pose2d& point);

    static Trajectory create_trajectory(const Pose2d& start,const Pose2d& end,int point_count=100);
    static Trajectory create_line_trajectory(const Pose2d& start,const Pose2d& end,float dt=0.2);
    static Trajectory path_smooth_slid(const std::vector<Pose2d>& path, int size = 5);

 protected:
    std::vector<Pose2d>             m_pose_vector;
};

/*
 * 根据梯度和方向计算角度,x轴正，逆时针为正
 */
double gradient2theta(double gradient,bool direction=true);

double gen_axis_angle(const Pose2d& point1,const Pose2d& point2);
/*
 * 计算曲线P 在(x1,x2)的线积分
 * 默认以 （x2-x1）/100  为步长
 */
double integral_path(double x1,double x2,Eigen::MatrixXd P);


#endif //LIO_LIVOX_CPP_MPC_TRAJECTORY_H_