#include <cstdio>
#include <cstdlib>
#include <cstdbool>
#include <cstdint>
#include <cmath>

#include <iostream>
#include <osrm/osrm.hpp>
#include <osrm/engine_config.hpp>
#include <osrm/storage_config.hpp>
#include <osrm/engine/api/match_parameters.hpp>
#include <osrm/json_container.hpp>
#include <util/json_renderer.hpp>
#include <fstream>
#include <engine/polyline_compressor.hpp>
#include <ctime>

#include "../nlohmann/json.hpp"

#define STB_DS_IMPLEMENTATION
#include "../stb/stb_ds.h"

#include "../kdtree/kdtree.h"
#include "config.h"

#include "time_utils.h"
#include "location.h"
#define CLUSTER_IMPLEMENTATION
#include "cluster.h"
#include "t_dbscan.h"

using namespace osrm;
#include "osrm_helper.h"

#include "douglas_peucker.h"
#include "timer.h"

#define DEBUG

struct kdtree *tree;
Location *locations;
Cluster  *clusters;

int KdTree2D_Init(struct kdtree **tree, Location *locations) {
  // Create 2-Dimentional KD-Tree.
  *tree = kd_create(2);

  // Populating KD-Tree.
  for (size_t i = 0; i < arrlen(locations); ++i) {
    Location location = locations[i];
    double location_as_array[] = {location.latitude_meter, location.longitude_meter};
    assert(kd_insert(*tree, location_as_array, (void*)i) == 0);
  }

  return 0;
}

std::vector<std::string> geometries_from_routes;
std::vector<std::string> geometries_from_matches;
std::vector<Location_Stop> stops;

EngineConfig    config;
MatchParameters match_parameters;
RouteParameters route_parameters;

double distance_traveled = 0.0;
double speed_max         = 0.0;
int main() {
  Config_Init(&config_global);
  simple_timer timer;
  timer_start(&timer);
  EngineConfig_Init(&config);
  OSRM osrm{config};

  MatchParameters_Init(&match_parameters);
  RouteParameters_Init(&route_parameters);

  size_t location_len = Location_ReadStandardInput(&locations);
  if (location_len == 0) {
    fprintf(stderr, "EXAMPLE USAGE\n");
    // TOOD: EXAMPLE USAGE.
    return 0;
  }

  KdTree2D_Init(&tree, locations);
  int n_clusters = T_DBSCAN(tree, locations, 35, config_global.tdbscan_eps_time, config_global.tdbscan_min_pts, &clusters);
  for (size_t i = 0; i < n_clusters; ++i) {
    Cluster_Analyze(locations, &clusters[i]);
  }

  Location *sampled_locations = NULL;
  Cluster_LocationSampler(locations, clusters, &sampled_locations, &speed_max);
  Location *downsampled_locations = NULL;
  size_t downsampled_locations_len = DP_Simplify(sampled_locations, config_global.douglas_eps, &downsampled_locations);
  
  Location_RemoveOutOfOrder(downsampled_locations);

#ifdef DEBUG
  fprintf(stderr, "Locations %d | Sampled Locations %d | Down Sampled: %d\n", arrlen(locations), arrlen(sampled_locations), arrlen(downsampled_locations));
#endif


  long long datetime_start = downsampled_locations[0].time;
  long long datetime_end   = downsampled_locations[arrlen(downsampled_locations) - 1].time;

  for (size_t i = 0; i < arrlen(downsampled_locations); ++i) {
    Location location = downsampled_locations[i];
    MatchParameters_Add(&match_parameters, location);
    if (location.stoped) {
      Location_Stop ls = Location_Stop_Init(location);
      stops.push_back(ls);
    }
  }

  json::Object result;
  auto status = osrm.Match(match_parameters, result);
  if (status != Status::Ok) {
    std::string output;
    json::render(output, result);
    fprintf(stderr, "Match as Failed!\noutput: %s\n", output.c_str());
    for (size_t i = 0; i < arrlen(downsampled_locations); ++i) {
      Location_Print(downsampled_locations[i]);
    }
  }

  Geometry_Add(geometries_from_matches, &distance_traveled, result, "matchings");
  nlohmann::json geometries_from_matches_json = geometries_from_matches;

  size_t len = geometries_from_matches.size();
  size_t number_of_gaps = len - 1;

  auto start_of_trajectory_coord = osrm::engine::decodePolyline(geometries_from_matches.front()).front();
  auto end_of_trajectory_coord   = osrm::engine::decodePolyline(geometries_from_matches.back()).back();


  for (size_t i = 0; i <= number_of_gaps; ++i) {
    if (i == 0) continue;
    const auto start = osrm::engine::decodePolyline(geometries_from_matches[i-1]).back();
    const auto end   = osrm::engine::decodePolyline(geometries_from_matches[i]  ).front();

    RouteParameters_RouteGaps(&osrm, &route_parameters, geometries_from_routes, &distance_traveled, start, end);
  }

  Location_Trajectory trajectory = Location_Trajectory_Init(
    static_cast<double>(util::toFloating(start_of_trajectory_coord.lat)),
    static_cast<double>(util::toFloating(start_of_trajectory_coord.lon)),
    static_cast<double>(util::toFloating(end_of_trajectory_coord.lat)),
    static_cast<double>(util::toFloating(end_of_trajectory_coord.lon)),
    datetime_start,
    datetime_end  ,
    distance_traveled,
    speed_max
  );

  nlohmann::json trajectory_json = trajectory;

  nlohmann::json geometries_from_routes_json = geometries_from_routes;
  nlohmann::json stops_json                  = stops;

  nlohmann::json final_json = {
    { "trajectory", trajectory_json                          },
    { "matches"   , geometries_from_matches                  },
    { "routes"    , geometries_from_routes                   },
    { "stops"     , stops                                    },
    { "records"   , locations_to_json(downsampled_locations) }
  };

  fprintf(stdout, "%s\n", final_json.dump().c_str());
#ifdef DEBUG
  fprintf(stderr, "Entire Process took: %f sec\n", timer_elapsed(&timer));
#endif

  //Config_Print(config_global);
  return 0;
}
