#pragma once

typedef struct {
  double x;
  double y;
} Vector2;

typedef struct {
  int     *indices;
  int     *indices_ordered;
  Vector2 centroid; // In Meters.
  double  centroid_latitude; // In Degrees.
  double  centroid_longitude; // In Degrees.
  double  covariance[2][2];
  double  lambda1;
  double  lambda2;
  double  spread;
  double  elongation;
} Cluster;
void Cluster_OrderIndices(Location *locations, Cluster *cluster);
void Cluster_ComputeCentriod(Location *locations, Cluster *cluster);
void Cluster_ComputeCovariance(Location *locations, Cluster *cluster);
void Cluster_ComputeEigenvalues2D(Cluster *cluster);
void Cluster_Analyze(Location *locations, Cluster *cluster);

long Cluster_Duration(Location *locations, Cluster *cluster);

#ifdef CLUSTER_IMPLEMENTATION

void Cluster_ComputeCentriod(Location *locations, Cluster *cluster) {
  double sx = 0.0, sy = 0.0;
  double slatitude = 0.0, slongitude = 0.0;

  for (size_t i = 0; i < arrlen(cluster->indices); ++i) {
    sx += locations[cluster->indices[i]].latitude_meter;
    sy += locations[cluster->indices[i]].longitude_meter;

    slatitude  += locations[cluster->indices[i]].latitude;
    slongitude += locations[cluster->indices[i]].longitude;
  }

  cluster->centroid.x = sx / arrlen(cluster->indices);
  cluster->centroid.y = sy / arrlen(cluster->indices);

  cluster->centroid_latitude  = slatitude  / arrlen(cluster->indices);
  cluster->centroid_longitude = slongitude / arrlen(cluster->indices);
}

void Cluster_ComputeCovariance(Location *locations, Cluster *cluster) {
  double sxx = 0.0, syy = 0.0, sxy = 0.0;

  for (int i = 0; i < arrlen(cluster->indices); ++i) {
    double dx = locations[cluster->indices[i]].latitude_meter  - cluster->centroid.x;
    double dy = locations[cluster->indices[i]].longitude_meter - cluster->centroid.y;

    sxx += dx * dx;
    syy += dy * dy;
    sxy += dx * dy;
  }

  cluster->covariance[0][0] = sxx / arrlen(locations);
  cluster->covariance[1][1] = syy / arrlen(locations);
  cluster->covariance[0][1] = cluster->covariance[1][0] = sxy / arrlen(locations);
}

void Cluster_ComputeEigenvalues2D(Cluster *cluster) {
  double a = cluster->covariance[0][0];
  double b = cluster->covariance[0][1];
  double c = cluster->covariance[1][1];

  double trace = a + c;
  double det_term = sqrt((a - c)*(a - c) + 4.0*b*b);

  cluster->lambda1 = 0.5*(trace+det_term);
  cluster->lambda2 = 0.5*(trace-det_term);
}

void Cluster_Analyze(Location *locations, Cluster *cluster) {
  //fprintf(stderr, "Cluster_OrderIndices...\n");
  Cluster_OrderIndices        (locations, cluster);
  //fprintf(stderr, "Cluster_Duration...\n");
  Cluster_Duration            (locations, cluster);
  //fprintf(stderr, "Cluster_ComputeCentriod...\n");
  Cluster_ComputeCentriod     (locations, cluster);
  //fprintf(stderr, "Cluster_ComputeCovariance...\n");
  Cluster_ComputeCovariance   (locations, cluster);
  //fprintf(stderr, "Cluster_ComputeEigenvalues2D...\n");
  Cluster_ComputeEigenvalues2D(cluster);

  cluster->spread = sqrt(cluster->lambda1 + cluster->lambda2);
  cluster->elongation = (cluster->lambda2 > 1e-9) 
    ? (cluster->lambda1 / cluster->lambda2)
    : INFINITY;
}

static int order_indices(const void *a, const void *b) {
  const int ia = *(const int*)a;
  const int ib = *(const int*)b;
  if (ia < ib) return -1;
  if (ia > ib) return 1;
  return 0;
}

void Cluster_OrderIndices(Location *locations, Cluster *cluster) {
  size_t len = arrlen(cluster->indices);
  arrsetlen(cluster->indices_ordered, len);
  memcpy(cluster->indices_ordered, cluster->indices, len * sizeof(int));

  qsort(cluster->indices_ordered, arrlen(cluster->indices_ordered), sizeof(int), order_indices);

  //fprintf(stderr, "[");
  //for (int i = 0; i < arrlen(cluster->indices_ordered); ++i) {
  //  fprintf(stderr, "%d,", cluster->indices_ordered[i]);
  //}
  ////for (int i = 0; i < arrlen(indices); ++i) {
  ////  fprintf(stderr, "%d,", indices[i]);
  ////}
  //fprintf(stderr, "]\n");
}

long Cluster_Duration(Location *locations, Cluster *cluster) {
  if (arrlen(cluster->indices) == 0) return 0;
  return locations[cluster->indices_ordered[arrlen(cluster->indices_ordered) - 1]].time_scaled - locations[cluster->indices_ordered[0]].time_scaled;
}

void Cluster_LocationSampler(Location *locations, Cluster *clusters, Location **out, double *speed_max) {
  int cluster_index = 0;
  int n = 0;
  int speed_max_location_index = 0;
  for (int i = 0; i < arrlen(locations); ++i) {
    Location location = locations[i];
    if (location.stoped) {
      //fprintf(stderr, "%d | %d | %d \n", i, cluster_index, arrlen(clusters[cluster_index].indices));
      Cluster cluster = clusters[cluster_index];
      if (arrlen(cluster.indices) == 0) {
        cluster_index++;
        continue;
      }
      Location last_location_in_cluster = locations[cluster.indices_ordered[arrlen(cluster.indices_ordered)-1]];
      Location _location    = {};
      _location.longitude   = cluster.centroid_longitude;
      _location.latitude    = cluster.centroid_latitude;
      _location.altitude    = location.altitude;
      _location.speed       = location.speed;
      _location.bearing     = location.bearing;
      _location.stoped      = true;
      _location.stoped_time = Cluster_Duration(locations, &cluster);
      _location.accuracy    = location.accuracy;
      _location.time        = last_location_in_cluster.time;
      _location.time_scaled = last_location_in_cluster.time_scaled;
      
      //Location_Print(_location);

      //fprintf(stderr, "%d Insert STOP location to array...| %d\n", i, cluster.indices_ordered[arrlen(cluster.indices_ordered) - 1]+1);
      //fprintf(stderr, "arrlen(cluster): %d\n", arrlen(clusters));
      //fprintf(stderr, "arrlen(cluster.indices_ordered): %d\n", arrlen(cluster.indices_ordered));
      arrput(*out, _location);
      n++;
      i = cluster.indices_ordered[arrlen(cluster.indices_ordered) - 1]+1;
      cluster_index++;
    } else {
      //fprintf(stderr, "%d Insert location to array...\n", i);
      //fprintf(stderr, "Speed: %lf | %lf\n", location.speed, *speed_max);
      if (*speed_max < location.speed) {
        *speed_max = location.speed;
        speed_max_location_index = n;
      }
      arrput(*out, location);
      n++;
    }
  }
  (*out)[speed_max_location_index].always_keep = true;
  Location_Print((*out)[speed_max_location_index]);
}

#endif
