#ifndef MPC_CONTROLLER_NODE_HH
#define MPC_CONTROLLER_NODE_HH 

#include "make_trajectory.h"
#include  "MpcDiffModel.h"
#include "mpc_tools.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>

// 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_controller{

public:
    mpc_controller(ros::NodeHandle nh);
    ~mpc_controller();

    void poseHandler(const turtlesim::Pose::ConstPtr& pose_msg);

    void targetHandler(const geometry_msgs::PoseStamped::ConstPtr& target_pose_msg);

    // Solve the model given an initial state and polynomial coefficients.
    // Return the first actuations.
    std::vector<double> Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs);

    // Evaluate a polynomial.
    double polyeval(Eigen::VectorXd coeffs, double x);

    // Fit a polynomial.
    // Adapted from
    // https://github.com/JuliaMath/Polynomials.jl/blob/master/src/Polynomials.jl#L676-L716
    Eigen::VectorXd polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order);
public:
    // Set the timestep length and duration
    // Currently tuned to predict 1 second worth
    size_t N = 10;
    double dt = 0.1;

    MpcDiffModel mdm;
    MakeTrajectory mt;
    mpc_tools mpc_tools_;
    // // This value assumes the model presented in the classroom is used.
    // //
    // // It was obtained by measuring the radius formed by running the vehicle in the
    // // simulator around in a circle with a constant steering angle and velocity on a
    // // flat terrain.
    // //
    // // Lf was tuned until the the radius formed by the simulating the model
    // // presented in the classroom matched the previous radius.
    // //
    // // This is the length from front to CoG that has a similar radius.
    // const double Lf = 1.0;

    // // Set desired speed for the cost function (i.e. max speed)

private:
    ros::NodeHandle nh_;
    tf::TransformListener tf_listener_;
    tf::TransformBroadcaster tf_broadcaster_;
    ros::Publisher vel_publisher_;
    ros::Publisher global_path_publisher_;
    ros::Publisher local_path_publisher_;
    int path_count;
    ros::Subscriber pose_sub_;
    ros::Subscriber target_sub_;

    tf::Pose current_pose, target_pose;

    double v_left, v_right, last_theta, a_left, a_right, omega;
    pcl::PointXYZ last_pos, curr_pos;
    std::vector<double> ptsx;
    std::vector<double> ptsy;
};

#endif /* MPC_CONTROLLER_NODE_HH */