text: #include <uWS/uWS.h>
  #include <fstream>
  #include <iostream>
  #include <string>
  #include <vector>
  #include "Eigen-3.3/Eigen/Core"
  #include "Eigen-3.3/Eigen/QR"
  #include "json.hpp"
  #include "helpers.h"
  #include "prediction.h"
  #include "behavior.h"
  #include "trajectory.h"
  
  // for convenience
  using nlohmann::json;
  using std::string;
  using std::vector;
  using std::cout;
  using std::endl;
  
  typedef struct CarParameters {
    BehaviorState_t state;
    int lane;
    double ref_vel;
    bool changing_lane;
  } CarParameters_t;
  
  int main() {
    uWS::Hub h;
  
    // Load up map values for waypoint's x,y,s and d normalized normal vectors
    vector<double> map_waypoints_x;
    vector<double> map_waypoints_y;
    vector<double> map_waypoints_s;
    vector<double> map_waypoints_dx;
    vector<double> map_waypoints_dy;
  
    // Waypoint map to read from
    string map_file_ = "../data/highway_map.csv";
    // The max s value before wrapping around the track back to 0
    double max_s = 6945.554;
  
    std::ifstream in_map_(map_file_.c_str(), std::ifstream::in);
  
    string line;
    while (getline(in_map_, line)) {
      std::istringstream iss(line);
      double x;
      double y;
      float s;
      float d_x;
      float d_y;
      iss >> x;
      iss >> y;
      iss >> s;
      iss >> d_x;
      iss >> d_y;
      map_waypoints_x.push_back(x);
      map_waypoints_y.push_back(y);
      map_waypoints_s.push_back(s);
      map_waypoints_dx.push_back(d_x);
      map_waypoints_dy.push_back(d_y);
    }
  
    CarParameters_t config;
    
    config.lane = 1;
    config.ref_vel = 0;
    config.state = kKeepLane;
    config.changing_lane = false;
    
    
    h.onMessage([&config, &map_waypoints_x,&map_waypoints_y,&map_waypoints_s,
                 &map_waypoints_dx,&map_waypoints_dy]
                (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
      if (length && length > 2 && data[0] == '4' && data[1] == '2') {
  
        auto s = hasData(data);
  
        if (s != "") {
          auto j = json::parse(s);
            
          string event = j[0].get<string>();
            
          if (event == "telemetry") {
            // j[1] is the data JSON object
            
            // Main car's localization Data
            double car_x = j[1]["x"];
            double car_y = j[1]["y"];
            double car_s = j[1]["s"];
            double car_d = j[1]["d"];
            double car_yaw = j[1]["yaw"];
            double car_speed = j[1]["speed"];
  
            // Previous path data given to the Planner
            // basically, it is a list of points used by simulation
            auto previous_path_x = j[1]["previous_path_x"];
            auto previous_path_y = j[1]["previous_path_y"];
            // Previous path's end s and d values 
            double end_path_s = j[1]["end_path_s"];
            double end_path_d = j[1]["end_path_d"];
  
            // Sensor Fusion Data, a list of all other cars on the same side 
            //   of the road.
            vector<vector<double>> sensor_fusion = j[1]["sensor_fusion"];   // auto
  
            json msgJson;
          
            // define the actual (x,y) points we will use for the planner
            vector<double> next_x_vals;
            vector<double> next_y_vals;
  
            /**
             * TODO: define a path made up of (x,y) points that the car will visit
             *   sequentially every .02 seconds
             */
            vector<vector<double>> predictions;
            vector<vector<double>> next_trajectory;
            
            int prev_size = previous_path_x.size();
            bool too_close = false;
            
            
            if(prev_size>0)	{
              car_s = end_path_s;
            }
            
            if(!sensor_fusion.empty())  {
              /******* 1. Prediction **********/
              predictions = Predict(sensor_fusion, prev_size);
  
              /******* 1.1. Too close **********/
              too_close = TooClose(predictions, car_s, config.lane);
              
              /******* 2. Behavior planning **********/
              if(too_close && config.state==kKeepLane) {
                config.state = StateTransition(predictions, config.state, car_s, car_speed, config.lane);
              } else if(config.state==kPrepareLaneChangeLeft || config.state==kPrepareLaneChangeRight)  {
                config.state = StateTransition(predictions, config.state, car_s, car_speed, config.lane);             
              }
  
            }
            
            /******* 2.1. Lane Change **********/
            config.lane = LaneChange(config.state, config.lane, config.changing_lane);
  
  #if defined(DEBUGGING)
            cout << "Lane: "<< config.lane << " | car_d: " << car_d << " | changing_lane: " << config.changing_lane << endl;
  #endif
  
            // If changing lane is complete, behavior state should be back to keep lane state
            if(config.state==kChangeLaneLeft || config.state==kChangeLaneRight) {
              if( car_d>(2+4*config.lane-1) && car_d<(2+4*config.lane+1))  {    // checking if car almost reaches to center of lane
                config.state = kKeepLane;
                config.changing_lane = false;
              }
            }
                     
            /******* 2.2. Adjust velocity **********/
            config.ref_vel = AdjustVelocity(too_close, config.ref_vel);
            
  #if defined(DEBUGGING)
            cout << "ref_vel = " <<config.ref_vel<<endl;
  #endif
   
            /******* 3. Trajectory **********/
            vector<double> car = {car_x, car_y, car_s, car_yaw, (double)config.lane, (double)prev_size, config.ref_vel};
            next_trajectory = Trajectory(car, previous_path_x, previous_path_y, map_waypoints_x, map_waypoints_y, map_waypoints_s);
  
      
  
            msgJson["next_x"] = next_trajectory[0]; //next_x_vals;
            msgJson["next_y"] = next_trajectory[1]; //next_y_vals;
  
            auto msg = "42[\"control\","+ msgJson.dump()+"]";
  
            ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT);
          }  // end "telemetry" if
        } else {
          // Manual driving
          std::string msg = "42[\"manual\",{}]";
          ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT);
        }
      }  // end websocket if
    }); // end h.onMessage
  
    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();
  }