#include "spatialmap.hpp"
#include <fmt/core.h>

using namespace fmt;

using DinkyECS::Entity;

void SpatialMap::insert(Point pos, Entity ent, bool has_collision) {
  if(has_collision) {
    dbc::check(!occupied(pos), "attempt to insert an entity with collision in space with collision");
  }

  $collision.emplace(pos, CollisionData{ent, has_collision});
}

CollisionData SpatialMap::remove(Point pos, Entity ent) {
  auto [begin, end] = $collision.equal_range(pos);
  for(auto it = begin; it != end; ++it) {
    if(it->second.entity == ent) {
      // does the it->second go invalid after erase?
      auto copy = it->second;
      $collision.erase(it);
      return copy;
    }
  }

  dbc::sentinel("failed to find entity to remove");
}

void SpatialMap::move(Point from, Point to, Entity ent) {
  auto data = remove(from, ent);
  insert(to, ent, data.collision);
}

bool SpatialMap::occupied(Point at) const {
  auto [begin, end] = $collision.equal_range(at);
  for(auto it = begin; it != end; ++it) {
    if(it->second.collision) {
      return true;
    }
  }

  return false;
}

bool SpatialMap::something_there(Point at) const {
  return $collision.count(at) > 0;
}

Entity SpatialMap::get(Point at) const {
  dbc::check($collision.contains(at), "attempt to get entity when none there");
  auto [begin, end] = $collision.equal_range(at);
  return begin->second.entity;
}

/*
 * Avoid doing work by using the dy,dx and confirming that
 * at.x or at.y is > 0.  If either is 0 then there can't be
 * a neighbor since that's out of bounds.
 */
inline void find_neighbor(const PointEntityMap &table, EntityList &result, Point at, int dy, int dx) {
  // don't bother checking for cells out of bounds
  if((dx < 0 && at.x <= 0) || (dy < 0 && at.y <= 0)) {
    return;
  }

  Point cell = {at.x + dx, at.y + dy};

  auto it = table.find(cell);
  if (it != table.end()) {
    result.insert(result.end(), it->second.entity);
  }
}

FoundEntities SpatialMap::neighbors(Point cell, bool diag) const {
  EntityList result;

  // just unroll the loop since we only check four directions
  // this also solves the problem that it was detecting that the cell was automatically included as a "neighbor" but it's not
  find_neighbor($collision, result, cell, 0, 1); // north
  find_neighbor($collision, result, cell, 0, -1); // south
  find_neighbor($collision, result, cell, 1, 0); // east
  find_neighbor($collision, result, cell, -1, 0); // west

  if(diag) {
    find_neighbor($collision, result, cell, 1, -1); // south east
    find_neighbor($collision, result, cell, -1, -1); // south west
    find_neighbor($collision, result, cell, 1, 1); // north east
    find_neighbor($collision, result, cell, -1, 1); // north west
  }

  return {!result.empty(), result};
}

inline void update_sorted(SortedEntities& sprite_distance, PointEntityMap& table, Point from, int max_dist) {
  for(const auto &rec : table) {
    Point sprite = rec.first;
    int inside = (from.x - sprite.x) * (from.x - sprite.x) +
        (from.y - sprite.y) * (from.y - sprite.y);

    if(inside < max_dist) {
      sprite_distance.push_back({inside, rec.second.entity});
    }
  }
}

SortedEntities SpatialMap::distance_sorted(Point from, int max_dist) {
  SortedEntities sprite_distance;

  update_sorted(sprite_distance, $collision, from, max_dist);

  std::sort(sprite_distance.begin(), sprite_distance.end(), std::greater<>());

  return sprite_distance;
}