#ifndef MPC_TWO_VEHICLE_MODEL_HH
#define MPC_TWO_VEHICLE_MODEL_HH 

#include <tf/transform_datatypes.h>

#include <vector>
#include <Eigen/Core>
#include <Eigen/QR>
#include <cppad/cppad.hpp>
#include <cppad/ipopt/solve.hpp>
/**
 * mpc 两车模型。每车定义为矩形，双前轮主动，双后轮从动。两车头朝外，中间软性连接。
 * 
 * */
class Mpc_two_vehicle_model{

public:
    // 构造函数
    Mpc_two_vehicle_model();
    // 析构函数
    ~Mpc_two_vehicle_model();
    // 模型解析
    // 参数:
    //      ref_velocity 参考速度，两车具有相同的参考速度
    //      ref_poses 规划的路径
    //      curr_pose 当前位姿
    //      v 下发速度
    //      vth 下发角速度
    bool Solve(double ref_velocity, std::vector<tf::Pose> ref_poses,tf::Pose curr_pose, double& v, double& vth, std::vector<tf::Pose> &pred_poses);

    void set_global_target(tf::Pose target);

private:

    double polyeval(Eigen::VectorXd coeffs, double x);
    Eigen::VectorXd polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order);

public:
    // 假设车辆由两个质点组成
    static const size_t N = 12;
    // static constexpr double Lf=1;

    // x，y； theta旋转角； v速度值； distance_error实际路径误差； epsi角度误差； a加速度
    // N个点，N个旋转角，N个速度值，N个角速度值, N个加速度值
    static const size_t x_start = 0;
    static const size_t y_start = x_start + N;
    static const size_t theta_start = y_start + N;
    static const size_t v_start = theta_start + N;
    static const size_t vth_start = v_start + N;

    static const size_t acc_start = vth_start + N - 1;
    static const size_t acc_vth_start = acc_start + N - 1;

    static double dt;
    static double ref_v;
    static double max_vel;

    bool forward = true;

private:

    double last_velocity, last_velocity_theta;
    tf::Pose m_last_pose;
    tf::Pose m_global_target;
    bool mb_has_global_target;
};

#endif /* MPC_TWO_VEHICLE_MODEL_HH */