cipher982

// j[1] is the data JSON object
vector<double> ptsx = j[1]["ptsx"];
vector<double> ptsy = j[1]["ptsy"];
double px  = j[1]["x"];
double py  = j[1]["y"];
double psi = j[1]["psi"];
double v   = j[1]["speed"];

cipher982

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

cipher982

double deg2rad(double x) { return x * pi() / 180; }
double rad2deg(double x) { return x * 180 / pi(); } 

cipher982

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

cipher982

// actuator shock absorber
if (t < N - 2) {
  //steering
  fg[0] += 30000.0*CppAD::pow(vars[delta_start + t + 1] - vars[delta_start + t], 2);

  //acceleration
  fg[0] += CppAD::pow(vars[a_start + t + 1] - vars[a_start + t], 2);    
}

cipher982

// simulate latency compensation
const double latency = 0.1;

px = px + v * cos(psi) * latency;
py = py + v * sin(psi) * latency;

cipher982

if (d > 0 && d < 4) // d represents location in meters from left-most edge
{
  car_lane = 0; // 0-4 meters = lane 0, (left-lane)
}
else if (d > 4 && d < 8)
{
  car_lane = 1; // middle-lane
}
else if (d > 8 && d < 12)
{

cipher982

// estimate longitudinal position of a car (s)
check_car_s += ((double)prev_size * 0.02 * check_speed);

if (car_lane == lane) // if the other car is in 'lane' (our lane)
{
  // is it within 30m ahead of us?
  car_ahead |= check_car_s > car_s && check_car_s - car_s < 30; 
}
else if (car_lane - lane == -1) // if left of us
{

cipher982

if (car_ahead)
  {
    cout << "Car Ahead!!!\n\n\n\n\n\n"
       << endl;
    if (!car_left && lane > 0) // no left-car, yes left-lane
    {
      lane--; // Change lane left
    }
    else if (!car_right && lane != 2) // no right-car, yes right-lane
    {

cipher982

// create a spline
tk::spline s;
// set (x,y) points to the spline
s.set_points(ptsx, ptsy);