#ifndef MPC_TOOLS_HH
#define MPC_TOOLS_HH 

#include <vector>
#include <Eigen/Core>
#include <Eigen/QR>
#include <tf/transform_datatypes.h>


// For converting back and forth between radians and degrees.


class mpc_tools{

public:
    mpc_tools();
    ~mpc_tools();

    Eigen::VectorXd polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order);

    double polyeval(Eigen::VectorXd coeffs, double x);

    bool filterPath(tf::Pose pose,std::vector<tf::Pose> poses, std::vector<tf::Pose>& out, int numOfPoints, bool forward = true);


};

#endif /* MPC_TOOLS_HH */