text: #include "MPC.h"
  #include <cppad/cppad.hpp>
  #include <cppad/ipopt/solve.hpp>
  #include "Eigen-3.3/Eigen/Core"
  
  using CppAD::AD;
  
  // Seting the timestep length and duration
  size_t N = 20;
  double dt = 0.05;
  
  double ref_v = 80.0;
  size_t x_start = 0;
  size_t y_start = x_start + N;
  size_t psi_start = y_start + N;
  size_t v_start = psi_start + N;
  size_t cte_start = v_start + N;
  size_t epsi_start = cte_start + N;
  size_t delta_start = epsi_start + N;
  size_t a_start = delta_start + N - 1;
  
  
  class FG_eval {
   public:
    // Fitted polynomial coefficients
    Eigen::VectorXd coeffs;
    FG_eval(Eigen::VectorXd coeffs) { this->coeffs = coeffs; }
  
    typedef CPPAD_TESTVECTOR(AD<double>) ADvector;
    void operator()(ADvector& fg, const ADvector& vars) {
      // Implementing MPC
      // `fg` a vector of the cost constraints, `vars` is a vector of variable values (state & actuators)
      // NOTE: You'll probably go back and forth between this function and
      // the Solver function below.
  
      // The cost is stored is the first element of `fg`.
      // Any additions to the cost should be added to `fg[0]`.
      fg[0] = 0;
  
      // Cost function
      // Defining the cost related the reference state and
      // any anything you think may be beneficial.
  
      // The part of the cost based on the reference state.
      for (size_t t = 0; t < N; t++) {
        fg[0] += 1000 * CppAD::pow(vars[cte_start + t], 2);
        fg[0] += 1000 * CppAD::pow(vars[epsi_start + t], 2);
        fg[0] += CppAD::pow(vars[v_start + t] - ref_v, 2);
      }
  
      // Minimizing the use of actuators.
      for (size_t t = 0; t < N - 1; t++) {
        fg[0] += 100 * CppAD::pow(vars[delta_start + t], 2);
        fg[0] += 100 * CppAD::pow(vars[a_start + t], 2);
      }
  
      // Minimizing the value gap between sequential actuations.
      for (size_t t = 0; t < N - 2; t++) {
        fg[0] += 300000 * CppAD::pow(vars[delta_start + t + 1] - vars[delta_start + t], 2);
        fg[0] += 5000 * CppAD::pow(vars[a_start + t + 1] - vars[a_start + t], 2);
      }
  
      // Initialization & constraints
      // Initializing the model to the initial state
      // Recall fg[0] is reserved for the cost value, so the other indices are bumped up by 1.
      fg[1 + x_start] = vars[x_start];
      fg[1 + y_start] = vars[y_start];
      fg[1 + psi_start] = vars[psi_start];
      fg[1 + v_start] = vars[v_start];
      fg[1 + cte_start] = vars[cte_start];
      fg[1 + epsi_start] = vars[epsi_start];
  
      for (size_t t = 1; t < N; t++) {
        // The 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> v1 = vars[v_start + t];
        AD<double> cte1 = vars[cte_start + t];
        AD<double> epsi1 = vars[epsi_start + t];
  
        // The 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> v0 = vars[v_start + t - 1];
        AD<double> cte0 = vars[cte_start + t - 1];
        AD<double> epsi0 = vars[epsi_start + t - 1];
  
        // Only consider the actuation at time t.
        AD<double> delta0 = vars[delta_start + t - 1];
        AD<double> a0 = vars[a_start + t - 1];
  
        AD<double> f0 = coeffs[0] + coeffs[1] * x0 + coeffs[2] * CppAD::pow(x0, 2);
        AD<double> psides0 = CppAD::atan(coeffs[1] + 2 * coeffs[2] * x0);
        // In case of using a polynomial of degree 3 uncoment following couple of lines
        //AD<double> f0 = coeffs[0] + coeffs[1] * x0 + coeffs[2] * CppAD::pow(x0, 2) + coeffs[3] * CppAD::pow(x0, 3);
        //AD<double> psides0 = CppAD::atan(coeffs[1] + 2 * coeffs[2] * x0 + 3 * coeffs[3] * CppAD::pow(x0, 2));
  
        // Here's `x` to get you started.
        // The idea here is to constraint this value to be 0.
        //
        // Recall the equations for the model:
        // x_[t] = x[t-1] + v[t-1] * cos(psi[t-1]) * dt
        // y_[t] = y[t-1] + v[t-1] * sin(psi[t-1]) * dt
        // psi_[t] = psi[t-1] + v[t-1] / Lf * delta[t-1] * dt
        // v_[t] = v[t-1] + a[t-1] * dt
        // cte[t] = f(x[t-1]) - y[t-1] + v[t-1] * sin(epsi[t-1]) * dt
        // epsi[t] = psi[t] - psides[t-1] + v[t-1] * delta[t-1] / Lf * dt
        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 + v0 * delta0 / Lf * dt);
        fg[1 + v_start + t] = v1 - (v0 + a0 * dt);
        fg[1 + cte_start + t] = cte1 - ((f0 - y0) + (v0 * CppAD::sin(epsi0) * dt));
        fg[1 + epsi_start + t] = epsi1 - ((psi0 - psides0) + v0 * delta0 / Lf * dt);
      }
    }
  };
  
  //
  // MPC class definition implementation.
  //
  MPC::MPC() {}
  MPC::~MPC() {}
  
  vector<double> MPC::Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs) {
    bool ok = true;
    typedef CPPAD_TESTVECTOR(double) Dvector;
  
    double x = state[0];
    double y = state[1];
    double psi = state[2];
    double v = state[3];
    double cte = state[4];
    double epsi = state[5];
  
    // Setting the number of model variables (includes both states and inputs).
    size_t n_vars = 6 * N + 2 * (N - 1);
    // Setting the number of constraints
    size_t n_constraints = 6 * N;
  
    // Initial value of the independent variables.
    // SHOULD BE 0 besides initial state.
    Dvector vars(n_vars);
    for (size_t i = 0; i < n_vars; i++) {
      vars[i] = 0.0;
    }
    // Setting the initial variable values
    vars[x_start] = x;
    vars[y_start] = y;
    vars[psi_start] = psi;
    vars[v_start] = v;
    vars[cte_start] = cte;
    vars[epsi_start] = epsi;
  
    // Setting lower and upper limits for variables.
    Dvector vars_lowerbound(n_vars);
    Dvector vars_upperbound(n_vars);
    // Setting all non-actuators upper and lowerlimits to the max negative and positive values.
    for (size_t i = 0; i < delta_start; i++) {
      vars_lowerbound[i] = -1.0e19;
      vars_upperbound[i] = 1.0e19;
    }
    // The upper and lower limits of delta are set to -25 and 25 degrees (values in radians).
    for (size_t i = delta_start; i < a_start; i++) {
      vars_lowerbound[i] = -0.436332;
      vars_upperbound[i] = 0.436332;
    }
    // Acceleration/decceleration upper and lower limits.
    for (size_t i = a_start; i < n_vars; i++) {
      vars_lowerbound[i] = -1.0;
      vars_upperbound[i] = 1.0;
    }
  
    // Lower and upper limits for the constraints
    // Should be 0 besides initial state.
    Dvector constraints_lowerbound(n_constraints);
    Dvector constraints_upperbound(n_constraints);
    for (size_t i = 0; i < n_constraints; i++) {
      constraints_lowerbound[i] = 0;
      constraints_upperbound[i] = 0;
    }
    // Setting lower limits for the constraints
    constraints_lowerbound[x_start] = x;
    constraints_lowerbound[y_start] = y;
    constraints_lowerbound[psi_start] = psi;
    constraints_lowerbound[v_start] = v;
    constraints_lowerbound[cte_start] = cte;
    constraints_lowerbound[epsi_start] = epsi;
    // Setting upper limits for the constraints
    constraints_upperbound[x_start] = x;
    constraints_upperbound[y_start] = y;
    constraints_upperbound[psi_start] = psi;
    constraints_upperbound[v_start] = v;
    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
    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 << "*** Cost " << cost << std::endl;
  
    // The variables can be accessed with `solution.x[i]`.
    // Returning actuator values given by the latency index.
    vector<double> result;
    result.push_back(solution.x[delta_start]);
    result.push_back(solution.x[a_start]);
    for (size_t i = 1; i < N; i++) {
      result.push_back(solution.x[x_start + i]);
      result.push_back(solution.x[y_start + i]);
    }
    return result;
  }
  