#ifndef MPC_TWO_VEHICLE_CONTROLLER_NODE_HH
#define MPC_TWO_VEHICLE_CONTROLLER_NODE_HH 

#include "mpc_two_vehicle_model.h"
#include "mpc_particle_model.h"
#include "trajectory_tool.h"

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Path.h>

#include <turtlesim/Pose.h>

#include <vector>
#include <Eigen/Core>
#include <Eigen/QR>
#include <cppad/cppad.hpp>
#include <cppad/ipopt/solve.hpp>
#include <pcl/point_types.h>
#include <chrono>
#include <mutex>

// For converting back and forth between radians and degrees.
constexpr double pi() { return M_PI; }
double deg2rad(double x) { return x * pi() / 180; }
double rad2deg(double x) { return x * 180 / pi(); }

/**
 * 双车模型控制器
 * 
 * */
class mpc_two_vehicle_controller{

public:
    mpc_two_vehicle_controller(ros::NodeHandle nh);
    ~mpc_two_vehicle_controller();
    // 小乌龟位姿回调，筛选局部路径，mpc优化生成v与vth，下发到小乌龟
    void poseHandler(const turtlesim::Pose::ConstPtr& pose_msg);
    // 目标设置回调，rviz上设置目标后，更新全局路径
    void targetHandler(const geometry_msgs::PoseStamped::ConstPtr& target_pose_msg);
    // 筛选局部路径
    bool filterPath(tf::Pose pose,std::vector<tf::Pose> poses, std::vector<tf::Pose>& out, int numOfPoints, bool forward = true);
    
public:
    size_t N = 10;
    double dt = 0.1;

private:
    Trajectory_tool m_traj_tool;
    // Mpc_two_vehicle_model m_mpc_model;
    Mpc_particle_model m_mpc_model;
    std::mutex m_edge_mutex; // 读写端点位姿互斥锁
    Eigen::Isometry2d m_base;
    Eigen::Isometry2d m_target;
    trajectory_type m_trajectory;

    nav_msgs::Path m_global_path;
    nav_msgs::Path m_local_path;
    int m_path_publish_count;

    ros::NodeHandle nh_;
    tf::TransformBroadcaster tf_broadcaster_;
    ros::Publisher vel_publisher_;
    ros::Publisher global_path_publisher_;
    ros::Publisher local_path_publisher_;
    ros::Subscriber pose_sub_;
    ros::Subscriber target_sub_;

    // 从话题获取到的当前位姿
    tf::Pose m_current_pose, m_target_pose;

    double velocity, omega, m_ref_vel, m_last_theta;
    pcl::PointXYZ last_pos, curr_pos;
    std::vector<double> ptsx;
    std::vector<double> ptsy;
};

#endif /* MPC_TWO_VEHICLE_CONTROLLER_NODE_HH */