main.cpp

#include "json.hpp"
#include <math.h>
#include <uWS/uWS.h>
#include <chrono>
#include <iostream>
#include <thread>
#include <vector>
#include "Eigen-3.3/Eigen/Core"
#include "Eigen-3.3/Eigen/QR"
#include "MPC.h"
#include <fstream>
// for convenience
using json = nlohmann::json;

// 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(); }
// Creating a file to store steering values
//std::ofstream steering_vals;


// Checks if the SocketIO event has JSON data.
// If there is data the JSON object in string format will be returned,
// else the empty string "" will be returned.
string hasData(string s) {
  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 "";
}

// Evaluate a polynomial.
double 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.
// 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;
  }

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

// Method that translates the values from world state to the vehicle prespective
Eigen::MatrixXd toCarCoordinates(double px, 
                          double py, 
                          double psi, 
                          const vector<double>& ptsx, 
                          const vector<double>& ptsy){
    
    unsigned len = ptsx.size();
    auto waypoints = Eigen::MatrixXd(2, len);//heck the use of auto
    for (auto i = 0; i < len ; ++i){
      double dx = ptsx[i] - px;
      double dy = ptsy[i] - py;
      waypoints(0,i) = cos(psi) * dx + sin(psi) * dy;
      waypoints(1,i) = - sin(psi) * dx + cos(psi) * dy;  
    }
    return waypoints;
}

int main() {
  // File stream to write steering data to
//  steering_vals.open("../data/steering_vals.txt", ios::out);
//  cout << "opening steering_vals.txt file" << endl;
//  // Check for errors opening the files
//  if( !steering_vals.is_open() )
//  {
//    cout << "Error opening steering_vals.txt file" << endl;
//    exit(1);
//  }
  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"];
          double px = j[1]["x"];
          double py = j[1]["y"];
          double psi = j[1]["psi"];
          double v = j[1]["speed"];

          /*  
          * TODO: Calculate steering angle and throttle using MPC.
          *
          * Both are in between [-1, 1].
          *
          */

          // Translate to car coordinate system and orientation
          Eigen::MatrixXd waypoints = toCarCoordinates(px,py,psi,ptsx,ptsy);

          // // For each point 
          // for (unsigned int i = 0; i < ptsx.size(); i++) {
          //   double dx = ptsx[i] - px;
          //   double dy = ptsy[i] - py;
          //   waypoints_x.push_back(dx * cos(-psi) - dy * sin(-psi));
          //   waypoints_y.push_back(dx * sin(-psi) + dy * cos(-psi));
          // }
          Eigen::VectorXd waypoints_x = waypoints.row(0);
          Eigen::VectorXd waypoints_y = waypoints.row(1);
          // cout << "waypoints_x size " << endl;
          // cout << waypoints_x.size() << endl;
          // cout << "Reached" << endl;

          // Fit a 1st order polynomial to the x and y waypoints
          auto coeffs = polyfit(waypoints_x, waypoints_y, 3);
          // calculate Cross track erro
          double cte = polyeval(coeffs, 0);
          // calculate the orientation error
          double epsi = -atan(coeffs[1]);


          // Init the steering and throttle values with the old ones
          // double steer_value = j[1]["steering_angle"];
          // double throttle_value = j[1]["throttle"];

          // Init a state with x, y both zeros
          Eigen::VectorXd state(6);
          state << 0, 0, 0, v, cte, epsi;

          // Apply MPC 
          Result solved_values = mpc.Solve(state, coeffs);
          // Check that we have values
          double steer_value = solved_values.delta.at(mpc.latency_timestep);
          double throttle_value = solved_values.a.at(mpc.latency_timestep);

          // Set the values for the previous as the current ones 
          mpc.prev_a = throttle_value;
          mpc.prev_delta = steer_value;

          // cout << "Steering value: " << steer_value << endl;
          // cout << "throttle value: " << throttle_value << endl; 

          json msgJson;
          // cout << "Tis is the " << deg2rad(25) << endl;
          // NOTE: Remember to divide by deg2rad(25) before you send the steering value back.
          // Otherwise the values will be in between [-deg2rad(25), deg2rad(25] instead of [-1, 1].
          // Negative value for the angle because the simulator has a mirrored orientation
          msgJson["steering_angle"] = - steer_value/ deg2rad(25);
          msgJson["throttle"] = throttle_value;
          // Writing steering values to the file 
//          steering_vals << - steer_value/ deg2rad(25) << endl;
          


          //.. 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
          // Setting the displayable MPC predicted trajectory
          msgJson["mpc_x"] = solved_values.x;
          msgJson["mpc_y"] = solved_values.y;

          //Display the waypoints/reference line
          vector<double> next_x_vals;
          vector<double> next_y_vals;

          //.. 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
          // Setting the displayable trajectory yellow line
          for (double i = 0; i < ptsy.size(); ++i){
                next_x_vals.push_back(waypoints_x(i));
                next_y_vals.push_back(waypoints_y(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(100));
          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();
}