#include <math.h>
#include <uWS/uWS.h>
#include <iostream>
#include "json.hpp"
#include "particle_filter.h"

using namespace std;

// for convenience
using json = nlohmann::json;

// 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.
std::string hasData(std::string s) {
  auto found_null = s.find("null");
  auto b1 = s.find_first_of("[");
  auto b2 = s.find_first_of("]");
  if (found_null != std::string::npos) {
    return "";
  } else if (b1 != std::string::npos && b2 != std::string::npos) {
    return s.substr(b1, b2 - b1 + 1);
  }
  return "";
}

int main() {
  uWS::Hub h;

  // Set up parameters here
  double delta_t = 0.1;      // Time elapsed between measurements [sec]
  double sensor_range = 50;  // Sensor range [m]

  double sigma_pos[3] = {
      0.3, 0.3,
      0.01};  // GPS measurement uncertainty [x [m], y [m], theta [rad]]
  double sigma_landmark[2] = {
      0.3, 0.3};  // Landmark measurement uncertainty [x [m], y [m]]

  // Read map data
  Map map;
  if (!read_map_data("../data/map_data.txt", map)) {
    cout << "Error: Could not open map file" << endl;
    return -1;
  }

  // Create particle filter
  ParticleFilter pf;

  h.onMessage([&pf, &map, &delta_t, &sensor_range, &sigma_pos, &sigma_landmark](
                  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(std::string(data));
      static double accumulated_time = 0;

      if (s != "") {
        auto j = json::parse(s);
        std::string event = j[0].get<std::string>();

        if (event == "telemetry") {
          // j[1] is the data JSON object

          auto start_time = std::chrono::system_clock::now();

          if (!pf.initialized()) {
            // Sense noisy position data from the simulator
            double sense_x = std::stod(j[1]["sense_x"].get<std::string>());
            double sense_y = std::stod(j[1]["sense_y"].get<std::string>());
            double sense_theta =
                std::stod(j[1]["sense_theta"].get<std::string>());

            pf.init(sense_x, sense_y, sense_theta, sigma_pos);
          } else {
            // Predict the vehicle's next state from previous (noiseless
            // control) data.
            double previous_velocity =
                std::stod(j[1]["previous_velocity"].get<std::string>());
            double previous_yawrate =
                std::stod(j[1]["previous_yawrate"].get<std::string>());

            pf.prediction(delta_t, sigma_pos, previous_velocity,
                          previous_yawrate);
          }

          // receive noisy observation data from the simulator
          // sense_observations in JSON format
          // [{obs_x,obs_y},{obs_x,obs_y},...{obs_x,obs_y}]
          vector<LandmarkObs> noisy_observations;
          string sense_observations_x = j[1]["sense_observations_x"];
          string sense_observations_y = j[1]["sense_observations_y"];

          std::vector<float> x_sense;
          std::istringstream iss_x(sense_observations_x);

          std::copy(std::istream_iterator<float>(iss_x),
                    std::istream_iterator<float>(),
                    std::back_inserter(x_sense));

          std::vector<float> y_sense;
          std::istringstream iss_y(sense_observations_y);

          std::copy(std::istream_iterator<float>(iss_y),
                    std::istream_iterator<float>(),
                    std::back_inserter(y_sense));

          for (uint i = 0; i < x_sense.size(); i++) {
            LandmarkObs obs;
            obs.x = x_sense[i];
            obs.y = y_sense[i];
            noisy_observations.push_back(obs);
          }

          // Update the weights and resample
          pf.updateWeights(sensor_range, sigma_landmark, noisy_observations,
                           map);
          pf.resample();

          // Calculate and output the average weighted error of the particle
          // filter over all time steps so far.
          vector<Particle> particles = pf.particles;
          int num_particles = particles.size();
          double highest_weight = -1.0;
          Particle best_particle;
          double weight_sum = 0.0;
          for (int i = 0; i < num_particles; ++i) {
            if (particles[i].weight > highest_weight) {
              highest_weight = particles[i].weight;
              best_particle = particles[i];
            }
            weight_sum += particles[i].weight;
          }

          auto end_time = std::chrono::system_clock::now();
          std::chrono::duration<double> dur = end_time - start_time;
          double calc_time = dur.count();

          accumulated_time += calc_time;

          cout << "t_accu " << accumulated_time << " ";
          cout << "highest w " << highest_weight << " ";
          cout << "average w " << weight_sum / num_particles << " ";
          cout << "t_singe " << calc_time << endl;

          json msgJson;
          msgJson["best_particle_x"] = best_particle.x;
          msgJson["best_particle_y"] = best_particle.y;
          msgJson["best_particle_theta"] = best_particle.theta;

          // Optional message data used for debugging particle's sensing and
          // associations
          msgJson["best_particle_associations"] =
              pf.getAssociations(best_particle);
          msgJson["best_particle_sense_x"] = pf.getSenseX(best_particle);
          msgJson["best_particle_sense_y"] = pf.getSenseY(best_particle);

          auto msg = "42[\"best_particle\"," + msgJson.dump() + "]";
          // std::cout << msg << std::endl;
          ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT);
        }
      } else {
        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();
}
