double DP_PerpendicularDistance(Location p, Location a, Location b) {
  double dx = b.longitude_meter - a.longitude_meter;
  double dy = b.latitude_meter  - a.latitude_meter;

  if (dx == 0 && dy == 0) {
    dx = p.longitude_meter - a.longitude_meter;
    dy = p.latitude_meter  - a.latitude_meter;
    return sqrt(dx*dx + dy*dy);
  }

  double numerator = fabs(dy*p.longitude_meter - dx*p.latitude_meter + b.longitude_meter*a.latitude_meter - b.latitude_meter*a.longitude_meter);
  double denominator = sqrt(dx*dx + dy*dy);
  return numerator / denominator;
}

void DP_Douglas_Peucker(Location *locations, size_t start, size_t end, double epsilon, bool *keep) {

  if (end <= start + 1) return;

  double max_dist = 0.0;
  size_t index    = -1;

  for (size_t i = start + 1; i < end; ++i) {
    double distance = DP_PerpendicularDistance(locations[i], locations[start], locations[end]);
    if (distance > max_dist) {
      max_dist = distance;
      index    = i;
    }
  }

  double eps = epsilon;
  if (eps == -1) {
    double speed_avg = (locations[start].speed + locations[end].speed) / 2;
    double base      = config_global.douglas_base;
    double k         = config_global.douglas_k;
    eps = base + speed_avg*k;
  }
  if (max_dist > eps) {
    keep[index] = true;
    DP_Douglas_Peucker(locations, start, index, epsilon, keep);
    DP_Douglas_Peucker(locations, index, end  , epsilon, keep);
  }

}

size_t DP_Simplify(Location *locations, double epsilon, Location **out) {
  size_t n = arrlen(locations);
  bool *keep = (bool *) calloc(n, sizeof(size_t));
  keep[0]     = true;
  keep[n - 1] = true;

  DP_Douglas_Peucker(locations, 0, arrlen(locations), epsilon, keep);

  size_t out_count = 0;
  for (size_t i = 0; i < n; ++i) {
    if (keep[i] || locations[i].stoped || locations[i].always_keep) {
      arrput(*out, locations[i]);
      out_count++;
    }
  }

  return out_count;
}
