auto found_null = s.find("null");

auto b1 = s.find_first_of("[");

auto b2 = s.rfind("}]");

if (found_null != string::npos) {

return "";

} else if (b1 != string::npos && b2 != string::npos) {

return s.substr(b1, b2 - b1 + 2);

}

return "";

double result = 0.0;

for (int i = 0; i < coeffs.size(); i++) {

result += coeffs[i] * pow(x, i);

}

return result;

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;

uWS::Hub h;

// MPC is initialized here!

MPC mpc;

h.onMessage([&mpc](uWS::WebSocket<uWS::SERVER> ws, char *data, size_t length, uWS::OpCode opCode) {

// "42" at the start of the message means there's a websocket message event.

// The 4 signifies a websocket message

// The 2 signifies a websocket event

string sdata = string(data).substr(0, length);

//cout << sdata << endl;

if (sdata.size() > 2 && sdata[0] == '4' && sdata[1] == '2') {

string s = hasData(sdata);

if (s != "") {

auto j = json::parse(s);

string event = j[0].get<string>();

if (event == "telemetry") {

// j[1] is the data JSON object

vector<double> ptsx = j[1]["ptsx"];

vector<double> ptsy = j[1]["ptsy"];

const double px = j[1]["x"];

const double py = j[1]["y"];

const double psi = j[1]["psi"];

const double v_raw = j[1]["speed"];

const double v = v_raw * 0.447;// mph to m/s

const double steering_angle = j[1]["steering_angle"];

const double throttle = j[1]["throttle"];

const int N = ptsx.size(); // Number of waypoints

const double cospsi = cos(-psi);

const double sinpsi = sin(-psi);

/*

const double dt = DT;

const double Lf = LF;

// Convert to the vehicle coordinate system

VectorXd x_veh(N);

VectorXd y_veh(N);

for(int i = 0; i < N; i++) {

const double dx = ptsx[i] - px;

const double dy = ptsy[i] - py;

x_veh[i] = dx * cospsi - dy * sinpsi;

y_veh[i] = dy * cospsi + dx * sinpsi;

}

auto coeffs = polyfit(x_veh, y_veh, 3); // Fit waypoints

const double cte = coeffs[0];

const double epsi = -atan(coeffs[1]); //-f'(0)

// Kinematic model is used to predict vehicle state at the actual

// moment of control (current time + delay dt)

const double px_act = v * dt;

const double py_act = 0;

const double psi_act = - v * steering_angle * dt / Lf;

const double v_act = v + throttle * dt;

const double cte_act = cte + v * sin(epsi) * dt;

const double epsi_act = epsi + psi_act;

VectorXd state(6);

state << px_act, py_act, psi_act, v_act, cte_act, epsi_act;

vector<double> mpc_results = mpc.Solve(state, coeffs);

double steer_value = mpc_results[0]/ deg2rad(25); // convert to [-1..1] range

double throttle_value = mpc_results[1];

json msgJson;

msgJson["steering_angle"] = -steer_value;

msgJson["throttle"] = throttle_value;

//Display the MPC predicted trajectory

//.. add (x,y) points to list here, points are in reference to the vehicle's coordinate system

// the points in the simulator are connected by a Green line

vector<double> mpc_x_vals = mpc.mpc_x;

vector<double> mpc_y_vals = mpc.mpc_y;

msgJson["mpc_x"] = mpc_x_vals;

msgJson["mpc_y"] = mpc_y_vals;

//Display the waypoints/reference line

//.. add (x,y) points to list here, points are in reference to the vehicle's coordinate system

// the points in the simulator are connected by a Yellow line

vector<double> next_x_vals;

vector<double> next_y_vals;

for(int i = 0; i<ptsx.size();i++){

next_x_vals.push_back(x_veh[i]);

next_y_vals.push_back(y_veh[i]);

}

msgJson["next_x"] = next_x_vals;

msgJson["next_y"] = next_y_vals;

auto msg = "42[\"steer\"," + msgJson.dump() + "]";

//std::cout << msg << std::endl;

// Latency

// The purpose is to mimic real driving conditions where

// the car does actuate the commands instantly.

//

// Feel free to play around with this value but should be to drive

// around the track with 100ms latency.

//

// NOTE: REMEMBER TO SET THIS TO 100 MILLISECONDS BEFORE

// SUBMITTING.

this_thread::sleep_for(chrono::milliseconds(int(dt*1000))); //s to ms

ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT);

}

} else {

// Manual driving

std::string msg = "42[\"manual\",{}]";

ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT);

}

}

});

// We don't need this since we're not using HTTP but if it's removed the

// program doesn't compile :-(

h.onHttpRequest([](uWS::HttpResponse *res, uWS::HttpRequest req, char *data,

size_t, size_t) {

const std::string s = "<h1>Hello world!</h1>";

if (req.getUrl().valueLength == 1) {

res->end(s.data(), s.length());

} else {

// i guess this should be done more gracefully?

res->end(nullptr, 0);

}

});

h.onConnection([&h](uWS::WebSocket<uWS::SERVER> ws, uWS::HttpRequest req) {

std::cout << "Connected!!!" << std::endl;

});

h.onDisconnection([&h](uWS::WebSocket<uWS::SERVER> ws, int code,

char *message, size_t length) {

ws.close();

std::cout << "Disconnected" << std::endl;

});

int port = 4567;

if (h.listen(port)) {

std::cout << "Listening to port " << port << std::endl;

} else {

std::cerr << "Failed to listen to port" << std::endl;

return -1;

}

h.run();