#ifndef MPC_DIFF_MODEL_HH
#define MPC_DIFF_MODEL_HH 

#include <tf/transform_datatypes.h>

#include <vector>
#include <Eigen/Core>
#include <Eigen/QR>
#include <cppad/cppad.hpp>
#include <cppad/ipopt/solve.hpp>

class MpcDiffModel{

public:
    MpcDiffModel();
    ~MpcDiffModel();

    bool Solve(double ref_velociity, std::vector<tf::Pose> ref_poses,tf::Pose curr_pose, double& v, double& vth);

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 = 10;
    static constexpr double dt=0.1;
    static constexpr double Lf=1;

    // x，y； psi旋转角； v速度值； cte实际路径误差； epsi角度误差； a_left左轮加速度； a_right右轮加速度
    static const size_t x_start = 0;
    static const size_t y_start = x_start + N;
    static const size_t psi_start = y_start + N;
    static const size_t v_left_start = psi_start + N;
    static const size_t v_right_start = v_left_start + N;
    static const size_t cte_start = v_right_start + N;
    static const size_t epsi_start = cte_start + N;

    static const size_t a_left_start = epsi_start + N;
    static const size_t a_right_start = a_left_start + N - 1;

    bool forward = true;

private:

    double last_theta, v_left, v_right, a_left, a_right;
};

#endif /* MPC_DIFF_MODEL_HH */