#pragma once

typedef struct {
  size_t index;
  double distance;
} Match;

static int compare_cluster_size(const void *a, const void *b) {
  const Cluster *ca = (const Cluster*)a;
  const Cluster *cb = (const Cluster*)b;
  return (int)arrlen(cb->indices) - (int)arrlen(ca->indices);
}

//#define ADAPTIVE_EPS


void T_DBSCAN_RadiusSearchFiltered_ForStop(struct kdtree *tree, Location *locations, int center_index, double eps_time, Match **out_matches) {
  double pos[2] = { locations[center_index].latitude_meter, locations[center_index].longitude_meter };

  double k = 3.0;
  double eps_stop = location_generalData.accuracy_median*k;
  struct kdres *res = kd_nearest_range(tree, pos, eps_stop);

  while (!kd_res_end(res)) {
    int index = (int)(intptr_t) kd_res_item_data(res);
    double dx = locations[index].latitude_meter  - locations[center_index].latitude_meter;
    double dy = locations[index].longitude_meter - locations[center_index].longitude_meter;
    double distance = sqrt(dx*dx + dy*dy);

    if (fabs(locations[index].time_scaled - locations[center_index].time_scaled) <= eps_time && locations[center_index].speed < 0.7) {
      Match m = {index, distance};
      arrput(*out_matches, m);
    }

    kd_res_next(res);
  }

  kd_res_free(res);
}

void T_DBSCAN_RadiusSearchFiltered(struct kdtree *tree, Location *locations, int center_index, double eps, double eps_time, Match **out_matches) {

  double pos[2] = { locations[center_index].latitude_meter, locations[center_index].longitude_meter };

#ifdef ADAPTIVE_EPS
  double k = 2.5;
  double adaptive_eps = locations[center_index].speed*locations[center_index].time_scaled + locations[center_index].accuracy*k;
  struct kdres *res = kd_nearest_range(tree, pos, adaptive_eps);
#else
  double k = 3.0;
  double eps_stop = locations[center_index].accuracy*config_global.tdbscan_k;
  struct kdres *res = kd_nearest_range(tree, pos, eps_stop);
#endif

  while (!kd_res_end(res)) {
    int index = (int)(intptr_t) kd_res_item_data(res);
    double dx = locations[index].latitude_meter  - locations[center_index].latitude_meter;
    double dy = locations[index].longitude_meter - locations[center_index].longitude_meter;
    double distance = sqrt(dx*dx + dy*dy);

    if (fabs(locations[index].time_scaled - locations[center_index].time_scaled) <= eps_time) {
      Match m = {index, distance};
      arrput(*out_matches, m);
    }

    kd_res_next(res);
  }

  kd_res_free(res);
}

int T_DBSCAN(struct kdtree *tree, Location *locations, double eps, double eps_time, double min_pts, Cluster **clusters) {
  bool *visited = (bool *)calloc(arrlen(locations), sizeof(bool));

  Match *matches = NULL;
  Match *sub_matches = NULL;

  for (size_t i = 0; i < arrlen(locations); ++i) {
    if (visited[i]) continue;

    arrsetlen(matches, 0);
    T_DBSCAN_RadiusSearchFiltered_ForStop(tree, locations, i, eps_time, &matches);
    if (arrlen(matches) < min_pts) continue;
    visited[i] = true;

    Cluster cluster = {};
    //cluster.indices = NULL;
    arrput(cluster.indices, i);
    locations[i].stoped = true;

    while (arrlen(matches) > 0) {
      Match m = arrpop(matches);
      int nb_index = m.index;

      if (visited[nb_index]) continue;
      visited[nb_index] = true;
      arrsetlen(sub_matches, 0);
      T_DBSCAN_RadiusSearchFiltered_ForStop(tree, locations, nb_index, eps_time, &sub_matches);

      if (arrlen(sub_matches) >= min_pts) {
        for (int k = 0; k < arrlen(sub_matches); ++k) {
          arrput(matches, sub_matches[k]);
        }
      }
      arrput(cluster.indices, nb_index);
      locations[nb_index].stoped = true;
    }
    arrput(*clusters, cluster);
    //fprintf(stderr, "%d\n", arrlen(cluster.indices));
  }

  if (clusters) {
    qsort(*clusters, arrlen(*clusters), sizeof(*clusters), compare_cluster_size);
  }
  return arrlen(*clusters);
}
