#include  "mpc_controller_node.h"
using CppAD::AD;

// Set the timestep length and duration
// Currently tuned to predict 1 second worth
size_t N = 10;
double dt = 0.1;

// 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;

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

bool forward = true;

double ref_v = 2.0;
// The solver takes all the state variables and actuator
// variables in a singular vector. Thus, we should to establish
// when one variable starts and another ends to make our lifes easier.
// x，y； psi旋转角； v速度值； cte实际路径误差； epsi角度误差； a_left左轮加速度； a_right右轮加速度
size_t x_start = 0;
size_t y_start = x_start + N;
size_t psi_start = y_start + N;
size_t v_left_start = psi_start + N;
size_t v_right_start = v_left_start + N;
size_t cte_start = v_right_start + N;
size_t epsi_start = cte_start + N;
// size_t delta_start = epsi_start + N;
size_t a_left_start = epsi_start + N;
size_t a_right_start = a_left_start + N - 1;

class FG_eval {
 public:
  // Fitted polynomial coefficients
  Eigen::VectorXd coeffs;
  FG_eval(Eigen::VectorXd coeffs) { this->coeffs = coeffs; std::cout<<"node FG"<<std::endl; }

  typedef CPPAD_TESTVECTOR(AD<double>) ADvector;
  void operator()(ADvector& fg, const ADvector& vars) {
    // Implementing MPC below
    // `fg` a vector of the cost constraints, `vars` is a vector of variable values (state & actuators)
    // The cost is stored is the first element of `fg`.
    // Any additions to the cost should be added to `fg[0]`.
    fg[0] = 0;
    
    // Reference State Cost
    // Below defines the cost related the reference state and
    // any anything you think may be beneficial.
    
    // Weights for how "important" each cost is - can be tuned
    const int cte_cost_weight = 8000;
    const int epsi_cost_weight = 1000;
    const int v_cost_weight = 2;
    // const int delta_cost_weight = 10;
    const int a_cost_weight = 7;
    // const int delta_change_cost_weight = 100;
    const int a_change_cost_weight = 7;

    // std::cout<<"fg0: "<<fg[0]<<std::endl;
    // Cost for CTE, psi error and velocity
    for (int t = 0; t < N; t++) {
      fg[0] += cte_cost_weight * CppAD::pow(vars[cte_start + t], 2);
      fg[0] += epsi_cost_weight * CppAD::pow(vars[epsi_start + t], 2);
      fg[0] += v_cost_weight * CppAD::pow(vars[v_left_start + t] - ref_v, 2);
      fg[0] += v_cost_weight * CppAD::pow(vars[v_right_start + t] - ref_v, 2);
      // std::cout<<"v l r : "<<vars[v_left_start + t]<<", "<<vars[v_right_start + t]<<std::endl;
    }
    
    // Costs for steering (delta) and acceleration (a)
    for (int t = 0; t < N-1; t++) {
      // fg[0] += delta_cost_weight * CppAD::pow(vars[delta_start + t], 2);
      fg[0] += a_cost_weight * CppAD::pow(vars[a_left_start + t], 2);
      fg[0] += a_cost_weight * CppAD::pow(vars[a_right_start + t], 2);
      // std::cout<<"a l r : "<<vars[a_left_start + t]<<", "<<vars[a_right_start + t]<<std::endl;
    }
    // std::cout<<"fg0: "<<fg[0]<<std::endl;
    
    // Costs related to the change in steering and acceleration (makes the ride smoother)
    for (int t = 0; t < N-2; t++) {
      // fg[0] += delta_change_cost_weight * pow(vars[delta_start + t + 1] - vars[delta_start + t], 2);
      fg[0] += a_change_cost_weight * pow(vars[a_left_start + t + 1] - vars[a_left_start + t], 2);
      fg[0] += a_change_cost_weight * pow(vars[a_right_start + t + 1] - vars[a_right_start + t], 2);
    }
    // std::cout<<"fg0: "<<fg[0]<<std::endl;
    
    // Setup Model Constraints

    // Initial constraints
    // We add 1 to each of the starting indices due to cost being located at index 0 of `fg`.
    // This bumps up the position of all the other values.
    fg[1 + x_start] = vars[x_start];
    fg[1 + y_start] = vars[y_start];
    fg[1 + psi_start] = vars[psi_start];
    fg[1 + v_left_start] = vars[v_left_start];
    fg[1 + v_right_start] = vars[v_right_start];
    fg[1 + cte_start] = vars[cte_start];
    fg[1 + epsi_start] = vars[epsi_start];
    // std::cout << "112,,,fg,vars: " <<1 + a_left_start<<", "<<1 + a_right_start<< std::endl;
    // fg[1 + a_left_start] = vars[a_left_start];
    // std::cout << "113,,,fg,vars: " <<1 + a_left_start<<", "<<1 + a_right_start<< std::endl;
    // fg[1 + a_right_start] = vars[a_right_start];
    // std::cout << "114,,,fg,vars: " <<1 + a_left_start<<", "<<1 + a_right_start<< std::endl;
    
    // The rest of the constraints
    for (int t = 1; t < N; t++) {
      // State at time t + 1
      AD<double> x1 = vars[x_start + t];
      AD<double> y1 = vars[y_start + t];
      AD<double> psi1 = vars[psi_start + t];
      AD<double> v_left1 = vars[v_left_start + t];
      AD<double> v_right1 = vars[v_right_start + t];
      AD<double> cte1 = vars[cte_start + t];
      AD<double> epsi1 = vars[epsi_start + t];
      
      // State at time t
      AD<double> x0 = vars[x_start + t - 1];
      AD<double> y0 = vars[y_start + t - 1];
      AD<double> psi0 = vars[psi_start + t - 1];
      AD<double> v_left0 = vars[v_left_start + t - 1];
      AD<double> v_right0 = vars[v_right_start + t - 1];
      AD<double> cte0 = vars[cte_start + t - 1];
      AD<double> epsi0 = vars[epsi_start + t - 1];
      
      // Actuator constraints at time t only
      AD<double> a_left0 = vars[a_left_start + t - 1];
      AD<double> a_right0 = vars[a_right_start + t - 1];
      
      AD<double> f0 = coeffs[0] + coeffs[1] * x0 + coeffs[2] * pow(x0, 2) + coeffs[3] * pow(x0, 3);
      AD<double> psi_des0 = CppAD::atan(coeffs[1] + 2*coeffs[2]*x0 + 3*coeffs[3]*pow(x0,2));
      
      // Setting up the rest of the model constraints
      AD<double> v0 = (v_left0+v_right0)/2.0;
      AD<double> v_diff0 = v_right0 - v_left0;
      fg[1 + x_start + t] = x1 - (x0 + v0 * CppAD::cos(psi0) * dt);
      fg[1 + y_start + t] = y1 - (y0 + v0 * CppAD::sin(psi0) * dt);
      fg[1 + psi_start + t] = psi1 - (psi0 - v_diff0 / Lf * dt);
      // fg[1 + v_start + t] = v1 - (v0 + a0 * dt);
      fg[1 + v_left_start + t] = v_left1 - (v_left0 + a_left0 * dt);
      fg[1 + v_right_start + t] = v_right1 - (v_right0 + a_right0 * dt);
      fg[1 + cte_start + t] = cte1 - ((f0-y0) + (v0 * CppAD::sin(epsi0) * dt));// 预测路径误差 - (y方向与路径差 + 当前角度误差带来的y方向移动)
      fg[1 + epsi_start + t] = epsi1 - ((psi0 - psi_des0) - v_diff0 / Lf * dt);// 预测角度误差 - (当前与目标角度差 - 当前角度偏转或补偿)
    }    
  }
};

mpc_controller::mpc_controller(ros::NodeHandle nh):nh_(nh)
{
  omega = 0;
  v_left = 0;
  v_right = 0;
  a_left = 0;
  a_right = 0;
  last_theta = 0;
  path_count = 0;
  last_pos = pcl::PointXYZ(0, 0, 0);
  curr_pos = pcl::PointXYZ(0, 0, 0);

  // //创建全局路径
  // pcl::PointXYZ base_point(0,0,0);
  // pcl::PointXYZ target_point(10,0,0);
  // for (int i = 0; i < 100; i++)
  // {
  //   double x = i * (target_point.x - base_point.x) / 100.0 + base_point.x;
  //   double y = i * (target_point.y - base_point.y) / 100.0 + base_point.y;
  //   ptsx.push_back(x);
  //   ptsy.push_back(y);
  // }

  for (int i = -30; i < 100; i++)
  {
    double x = i * (15.0 / 100);
    double y = -0.005 * x * x * x + 0.0525 * x * x + 0.001 * x+1;
    ptsx.push_back(x);
    ptsy.push_back(y);
    // std::cout<<"x,y: "<<x<<", "<<y<<std::endl;
  }

  //发布与订阅
  vel_publisher_ = nh_.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);
  global_path_publisher_ = nh_.advertise<nav_msgs::Path>("global_path",2);
  local_path_publisher_ = nh_.advertise<nav_msgs::Path>("local_path",2);
  pose_sub_ = nh_.subscribe<turtlesim::Pose>("/turtle1/pose", 1, &mpc_controller::poseHandler, this);
  target_sub_ = nh_.subscribe<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1, &mpc_controller::targetHandler, this);
}

mpc_controller::~mpc_controller()
{
}

void mpc_controller::targetHandler(const geometry_msgs::PoseStamped::ConstPtr &target_pose_msg)
{
  tf::poseMsgToTF(target_pose_msg->pose, target_pose);
  mt.Make(current_pose, target_pose, 10);
  //发布全局路径
  nav_msgs::Path path;
  path.header.frame_id = "map";
  path.header.seq = ++path_count;
  path.header.stamp = ros::Time::now();
  for (size_t i = 0; i < mt.trajectory_.size(); i++)
  {
    geometry_msgs::PoseStamped temp;
    tf::poseTFToMsg(mt.trajectory_[i], temp.pose);
    temp.header = path.header;
    // temp.pose.position.x = ptsx[i];
    // temp.pose.position.y = ptsy[i];
    // double theta = atan2(ptsy[i], ptsx[i]);
    // tf::quaternionTFToMsg(tf::createQuaternionFromRPY(0.0, 0.0, theta), temp.pose.orientation);
    path.poses.push_back(temp);
  }
  global_path_publisher_.publish(path);

}

void mpc_controller::poseHandler(const turtlesim::Pose::ConstPtr &pose_msg)
{
  current_pose.setOrigin(tf::Vector3(pose_msg->x, pose_msg->y,0));
  current_pose.setRotation(tf::Quaternion(pose_msg->theta,0,0));

  std::chrono::time_point<std::chrono::system_clock> start = std::chrono::system_clock::now();

  std::cout<<"-------------"<<std::endl;
  // //发布全局路径
  // nav_msgs::Path path;
  // path.header.frame_id = "map";
  // path.header.seq = ++path_count;
  // path.header.stamp = ros::Time::now();
  // for (size_t i = 0; i < ptsx.size(); i++)
  // {
  //   geometry_msgs::PoseStamped temp;
  //   temp.header = path.header;
  //   temp.pose.position.x = ptsx[i];
  //   temp.pose.position.y = ptsy[i];
  //   double theta = atan2(ptsy[i], ptsx[i]);
  //   // if(ptsx[i]*ptsy[i]<0)
  //   //   theta = theta-M_PI;
  //   // std::cout<<"x,y,theta: "<<ptsx[i]<<", "<<ptsy[i]<<", "<<theta<<std::endl;
  //   tf::quaternionTFToMsg(tf::createQuaternionFromRPY(0.0, 0.0, theta),temp.pose.orientation);
  //   // temp.pose.orientation.w = cos(theta/2.0);
  //   // temp.pose.orientation.z = sin(theta/2.0);
  //   path.poses.push_back(temp);
  // }
  // global_path_publisher_.publish(path);

  //发布tf
  tf::Transform transform;
  transform.setOrigin(tf::Vector3(pose_msg->x, pose_msg->y, 0.0));
  transform.setRotation(tf::Quaternion(0, 0, sin(pose_msg->theta/2.0), cos(pose_msg->theta/2.0)));
  tf::StampedTransform transform_msg(transform, ros::Time::now(), "map", "base_link");
  tf_broadcaster_.sendTransform(transform_msg);
  
  {
      // curr_pos = pcl::PointXYZ(pose_msg->x, pose_msg->y, 0);
    // double v = pose_msg->linear_velocity;
    // omega = pose_msg->angular_velocity;
    // // std::cout<<"---------------"<<std::endl;
    // double theta_diff = pose_msg->theta - last_theta;//omega*dt;
    // // std::cout<<"linear velo: "<<v<<", theta_diff: "<<theta_diff<<std::endl;
    // v_left = v - (theta_diff*Lf)/(2*dt);
    // v_right = v + (theta_diff*Lf)/(2*dt);

    // // Need Eigen vectors for polyfit
    // // 筛选前方6点
    // Eigen::VectorXd ptsx_car(N);
    // Eigen::VectorXd ptsy_car(N);
    // int count=0;
    // // Transform the points to the vehicle's orientation
    // // 平移加反向旋转到小车坐标系
    // for (int i = 0; i < ptsx.size(); i++)
    // {
    //   double x = ptsx[i] - curr_pos.x;
    //   double y = ptsy[i] - curr_pos.y;
    //   double trans_x = x * cos(-pose_msg->theta) - y * sin(-pose_msg->theta);
    //   double trans_y = x * sin(-pose_msg->theta) + y * cos(-pose_msg->theta);
    //   double dist = sqrt(pow(trans_x,2)+pow(trans_y,2));
    //   if (trans_x>=0 && count < N)
    //   {
    //     ptsx_car[count] = trans_x;
    //     ptsy_car[count] = trans_y;
    //     // std::cout << "path " << count << " : " << ptsx_car[count] << ", " << ptsy_car[count] << std::endl;
    //     count++;
    //   }
    //   // if(ptsx_car[i]>0)
    //   //   std::cout<<"path "<<i<<" : "<<ptsx_car[i]<<", "<<ptsy_car[i]<<std::endl;
    // }

    // if (count < N)
    // {
    //   ref_v /= (N - count);
    // }

    // if (pose_msg->x > 10.5 && forward)
    // {
    //   ref_v = -3.0;
    //   forward = false;
    // }
    // else if(pose_msg->x < 2 && !forward)
    // {
    //   ref_v = 3.0;
    //   forward = true;
    // }
    // std::cout<<ref_v<<", "<<forward<<std::endl;

    // auto coeffs = polyfit(ptsx_car, ptsy_car, 3);
    // // if(std::isnan(coeffs[0])||std::isnan(coeffs[1])||std::isnan(coeffs[2])||std::isnan(coeffs[3]))
    // //   return;
    // double cte = polyeval(coeffs, 0);
    // double etheta = atan(coeffs[1]);
    // // std::cout<<"****************"<<std::endl;
    // // // std::cout<<"coeffs: "<<coeffs<<std::endl;
    // // std::cout<<"cte: "<<cte<<std::endl;
    // // std::cout<<"etheta: "<<etheta<<std::endl;
    // // std::cout<<"theta_diff: "<<theta_diff<<std::endl;
    // // std::cout<<"****************"<<std::endl;

    // // Predict state after latency
    // // x, y and psi are all zero after transformation above
    // double pred_px = 0.0 + v * dt; // Since psi is zero, cos(0) = 1, can leave out
    // const double pred_py = 0.0;    // Since sin(0) = 0, y stays as 0 (y + v * 0 * dt)
    // double pred_theta = 0.0 + theta_diff;
    // double pred_v_left = v_left + a_left* dt;
    // double pred_v_right = v_right + a_right* dt;
    // double pred_cte = cte + v * sin(etheta) * dt;
    // double pred_etheta = etheta - theta_diff;

    // // Feed in the predicted state values
    // Eigen::VectorXd state(7);
    // state << pred_px, pred_py, pred_theta, pred_v_left, pred_v_right, pred_cte, pred_etheta;
    // // std::cout<<state<<std::endl;

    // // auto vars = Solve(state, coeffs);
    // // if(vars.size()==0){
    // //   // std::cout<<"empty, return"<<std::endl;
    // //   return;
    // // }

    // // //更新速度并发布cmd_vel
    // // a_left = vars[0];
    // // a_right = vars[1];
    // // // std::cout<<"v_curr: "<<v_left<<", "<<v_right<<std::endl;
    // // v_left += a_left*dt;
    // // v_right += a_right*dt;
    // // // omega += (a_left-a_right)*dt;
    // // // std::cout<<"a: "<<a_left<<", "<<a_right<<std::endl;
    // // std::cout<<"v: "<<v_left<<", "<<v_right<<std::endl;

  }

  if(mt.trajectory_.size()<1)
    return;
  std::chrono::time_point<std::chrono::system_clock> end = std::chrono::system_clock::now();
  // std::cout<<"part1: "<<std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()<<std::endl;

  // 筛选前方6点
  Eigen::VectorXd ptsx_car(mt.trajectory_.size());
  Eigen::VectorXd ptsy_car(mt.trajectory_.size());
  int count=0;
  // Transform the points to the vehicle's orientation
  // 平移加反向旋转到小车坐标系
  // std::cout<<"current pose:"<<current_pose.getOrigin().getX()<<", "<<current_pose.getOrigin().getY()<<std::endl;
  for (int i = 0; i <mt.trajectory_.size() ; i++)
  {
    double x = mt.trajectory_[i].getOrigin().getX() - current_pose.getOrigin().getX();
    double y = mt.trajectory_[i].getOrigin().getY() - current_pose.getOrigin().getY();
    double trans_x = x * cos(-pose_msg->theta) - y * sin(-pose_msg->theta);
    double trans_y = x * sin(-pose_msg->theta) + y * cos(-pose_msg->theta);
    // std::cout<<"global path"<<i<<": "<<mt.trajectory_[i].getOrigin().getX()<<" "<<mt.trajectory_[i].getOrigin().getY()<<std::endl;

    // if (count < N)
    // {
      ptsx_car[i] = trans_x;
      ptsy_car[i] = trans_y;
      // std::cout << "path " << count << " : " << ptsx_car[count] << ", " << ptsy_car[count] << std::endl;
    //   count++;
    // }
    // if(ptsx_car[i]>0)
    //   std::cout<<"path "<<i<<" : "<<ptsx_car[i]<<", "<<ptsy_car[i]<<std::endl;
  }

  // std::cout<<"thetas: "<<current_pose.getRotation().getX()<<", "<<current_pose.getRotation().getY()<<", "<<current_pose.getRotation().getZ()<<", "<<current_pose.getRotation().getW()<<std::endl;
  tf::Vector3 orient(cos(pose_msg->theta), sin(pose_msg->theta), 0.0);
  tf::Vector3 target_direction = target_pose.getOrigin() - current_pose.getOrigin();
  // std::cout<<"orient: "<<orient.getX()<<" "<<orient.getY()<<std::endl;
  // std::cout<<"target direction: "<<target_direction.getX()<<", "<<target_direction.getY()<<std::endl;
  // std::cout<<"multiplication result: "<<target_direction.dot(orient)<<std::endl;

  // 终点附近限制速度
  if(target_direction.length()<1)
  {
    if (target_direction.length() < 0.02)
    {
      ref_v = 0;
    }
    ref_v *= target_direction.length2();
    ref_v = fmin(ref_v, 0.1);
    ref_v = fmax(ref_v, 0.02);
  }
  else
  {
    ref_v = 2.0;
  }

  // 调整速度方向
  if (target_direction.dot(orient) > 0)
  {
    ref_v = ref_v < 0 ? -ref_v : ref_v;
  }
  else
  {
    ref_v = ref_v > 0 ? -ref_v : ref_v;
  }
  std::cout<<"ref_v: "<<ref_v<<std::endl;

  // std::cout<<"send filtered poses"<<std::endl;
  std::vector<tf::Pose> poses;
  for (size_t i = 0; i < ptsx_car.size(); i++)
  {
    poses.push_back(tf::Pose(tf::Quaternion(), tf::Vector3(ptsx_car[i],ptsy_car[i],0)));
    // std::cout<<"x,y: "<<ptsx_car[i]<<", "<<ptsy_car[i]<<std::endl;
  }
  tf::Pose pose;
  double velo = pose_msg->linear_velocity;
  double velo_angle = pose_msg->angular_velocity;
  pose.setOrigin(tf::Vector3(pose_msg->x, pose_msg->y, 0.0));
  pose.setRotation(tf::Quaternion(0, 0, sin(pose_msg->theta/2.0), cos(pose_msg->theta/2.0)));
  std::cout<<"start solve"<<std::endl;
  if (mdm.Solve(ref_v, poses, pose, velo, velo_angle))
  {
    std::cout << "solve complete" << std::endl;

    geometry_msgs::Twist velocity;
    velocity.linear.x = velo; //(v_left+v_right)/2.0;
    velocity.linear.y = 0;
    velocity.linear.z = 0;
    velocity.angular.x = 0;
    velocity.angular.y = 0;
    velocity.angular.z = velo_angle; //- (v_right-v_left)/Lf;//turtlesim uses left hand angle, if angle less than 0, it means turn left.
    // velocity.angular.z = omega;
    vel_publisher_.publish(velocity);
  }
  std::chrono::time_point<std::chrono::system_clock> end1 = std::chrono::system_clock::now();
  // std::cout<<"part2: "<<std::chrono::duration_cast<std::chrono::milliseconds>(end1-end).count()<<std::endl;

  //测试tools
  std::vector<tf::Pose> local_poses;
  mpc_tools_.filterPath(current_pose, mt.trajectory_, local_poses, N, false);
  //发布局部路径
  nav_msgs::Path local_path;
  local_path.header.seq = path_count;
  local_path.header.stamp = ros::Time::now();
  local_path.header.frame_id = "map";
  for (size_t i = 2; i < local_poses.size(); i++)
  {
    geometry_msgs::PoseStamped temp;
    temp.header = local_path.header;
    // temp.pose.position.x = ptsx_car[i];
    // temp.pose.position.y = ptsy_car[i];
    // double theta = atan2(ptsy_car[i], ptsx_car[i]);
    // tf::quaternionTFToMsg(tf::createQuaternionFromRPY(0.0, 0.0, theta),temp.pose.orientation);
    // std::cout<<"local_poses"<<i<<" "<<local_poses[i].getOrigin().getX()<<", "<<local_poses[i].getOrigin().getY()<<std::endl;
    local_poses[i].setRotation(local_poses[i].getRotation().normalize());
    tf::poseTFToMsg(local_poses[i], temp.pose);
    local_path.poses.push_back(temp);
  }
  local_path_publisher_.publish(local_path);

  last_theta = pose_msg->theta;
  last_pos = curr_pos;
}

// Evaluate a polynomial.求y
double mpc_controller::polyeval(Eigen::VectorXd coeffs, double x) {
  double result = 0.0;
  for (int i = 0; i < coeffs.size(); i++) {
    result += coeffs[i] * pow(x, i);
  }
  return result;
}

// Fit a polynomial.求参数
Eigen::VectorXd mpc_controller::polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order) {
  assert(xvals.size() == yvals.size());
  assert(order >= 1 && order <= xvals.size() - 1);
  Eigen::MatrixXd A(xvals.size(), order + 1);

  for (int i = 0; i < xvals.size(); i++) {
    A(i, 0) = 1.0;
  }

  for (int j = 0; j < xvals.size(); j++) {
    for (int i = 0; i < order; i++) {
      A(j, i + 1) = A(j, i) * xvals(j);
    }
  }

  auto Q = A.householderQr();
  auto result = Q.solve(yvals);
  return result;
}

std::vector<double> mpc_controller::Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs) {
  bool ok = true;
  typedef CPPAD_TESTVECTOR(double) Dvector;
  // State vector holds all current values neede for vars below
  double x = state[0];
  double y = state[1];
  double psi = state[2];
  double v_left = state[3];
  double v_right = state[4];
  double cte = state[5];
  double epsi = state[6];

  // Setting the number of model variables (includes both states and inputs).
  // N * state vector size + (N - 1) * 2 actuators (For steering & acceleration)
  size_t n_vars = N * 7 + (N - 1) * 2;
  // Setting the number of constraints
  size_t n_constraints = N * 7;

  // Initial value of the independent variables.
  // SHOULD BE 0 besides initial state.
  Dvector vars(n_vars);
  for (int i = 0; i < n_vars; i++) {
    vars[i] = 0.0;
  }

  Dvector vars_lowerbound(n_vars);
  Dvector vars_upperbound(n_vars);
  // Sets lower and upper limits for variables.
  // Set all non-actuators upper and lowerlimits
  // to the max negative and positive values.
  for (int i = 0; i < a_left_start; i++) {
    vars_lowerbound[i] = -1.0e6;
    vars_upperbound[i] = 1.0e6;
  }
  
  // // The upper and lower limits of delta are set to -25 and 25
  // // degrees (values in radians).
  // for (int i = delta_start; i < a_start; i++) {
  //   vars_lowerbound[i] = -0.436332;
  //   vars_upperbound[i] = 0.436332;
  // }
  
  // Acceleration/decceleration upper and lower limits.
  for (int i = a_left_start; i < n_vars; i++) {
    vars_lowerbound[i] = -5.5;
    vars_upperbound[i] = 5.5;
  }

  // Lower and upper limits for the constraints
  // Should be 0 besides initial state.
  Dvector constraints_lowerbound(n_constraints);
  Dvector constraints_upperbound(n_constraints);
  for (int i = 0; i < n_constraints; i++) {
    constraints_lowerbound[i] = 0;
    constraints_upperbound[i] = 0;
  }
  
  // Start lower and upper limits at current values
  constraints_lowerbound[x_start] = x;
  constraints_lowerbound[y_start] = y;
  constraints_lowerbound[psi_start] = psi;
  constraints_lowerbound[v_left_start] = v_left;
  constraints_lowerbound[v_right_start] = v_right;
  constraints_lowerbound[cte_start] = cte;
  constraints_lowerbound[epsi_start] = epsi;
  
  constraints_upperbound[x_start] = x;
  constraints_upperbound[y_start] = y;
  constraints_upperbound[psi_start] = psi;
  constraints_upperbound[v_left_start] = v_left;
  constraints_upperbound[v_right_start] = v_right;
  constraints_upperbound[cte_start] = cte;
  constraints_upperbound[epsi_start] = epsi;

  // object that computes objective and constraints
  FG_eval fg_eval(coeffs);
  //
  // NOTE: You don't have to worry about these options
  //
  // options for IPOPT solver
  std::string options;
  // Uncomment this if you'd like more print information
  options += "Integer print_level  0\n";
  // NOTE: Setting sparse to true allows the solver to take advantage
  // of sparse routines, this makes the computation MUCH FASTER. If you
  // can uncomment 1 of these and see if it makes a difference or not but
  // if you uncomment both the computation time should go up in orders of
  // magnitude.
  options += "Sparse  true        forward\n";
  options += "Sparse  true        reverse\n";
  // NOTE: Currently the solver has a maximum time limit of 0.5 seconds.
  // Change this as you see fit.
  options += "Numeric max_cpu_time          0.5\n";

  // place to return solution
  CppAD::ipopt::solve_result<Dvector> solution;
  // solve the problem
  // std::cout<<vars.size()<<", "<<vars_lowerbound.size()<<", "<<vars_upperbound.size()<<", "<<constraints_lowerbound.size()<<", "<<constraints_upperbound.size()<<", "<<std::endl;
  CppAD::ipopt::solve<Dvector, FG_eval>(
      options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound,
      constraints_upperbound, fg_eval, solution);
  
  // Check some of the solution values
  ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;

  // Cost
  auto cost = solution.obj_value;
  // std::cout <<"status "<<ok<< ", Cost " << cost << std::endl;

  // Return the first actuator values, along with predicted x and y values to plot in the simulator.
  std::vector<double> solved;
  if (ok)
  {
    solved.push_back(solution.x[a_left_start]);
    solved.push_back(solution.x[a_right_start]);
    for (int i = 0; i < N; ++i)
    {
      solved.push_back(solution.x[x_start + i]);
      solved.push_back(solution.x[y_start + i]);
    }
    // std::cout<<"return solved"<<std::endl;
  }
  return solved;
}

int main(int argc, char** argv){
    ros::init(argc, argv, "mpc_controller_node");
    ros::NodeHandle nh;
    mpc_controller controller(nh);
    ros::Rate loop_rate(50);
        
    // double px = 5.54;
    // double py = 5.54;
    // double psi = 0;
    // double v_l = 0;
    // double v_r = 0;
    // double a_l = 0;
    // double a_r = 0;



    while(ros::ok()){
        
        //controller.processData();

        ros::spinOnce();
        loop_rate.sleep();
    }
    //controller.~mpc_controller();
    return 0;
}