#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) {
    yes_collision.insert_or_assign(pos, ent);
  } else {
    no_collision.insert_or_assign(pos, ent);
  }
}

bool SpatialMap::remove(Point pos) {
  if(yes_collision.contains(pos)) {
    yes_collision.erase(pos);
    return true;
  } else {
    no_collision.erase(pos);
    return false;
  }
}

void SpatialMap::move(Point from, Point to, Entity ent) {
  bool has_collision = remove(from);
  insert(to, ent, has_collision);
}

bool SpatialMap::occupied(Point at) const {
  return yes_collision.contains(at);
}

bool SpatialMap::something_there(Point at) const {
  return yes_collision.contains(at) || no_collision.contains(at);
}

Entity SpatialMap::get(Point at) const {
  if(yes_collision.contains(at)) {
    return yes_collision.at(at);
  } else {
    return no_collision.at(at);
  }
}

/*
 * 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);
  }
}

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(yes_collision, result, cell, 0, 1); // north
  find_neighbor(yes_collision, result, cell, 0, -1); // south
  find_neighbor(yes_collision, result, cell, 1, 0); // east
  find_neighbor(yes_collision, result, cell, -1, 0); // west

  if(diag) {
    find_neighbor(yes_collision, result, cell, 1, -1); // south east
    find_neighbor(yes_collision, result, cell, -1, -1); // south west
    find_neighbor(yes_collision, result, cell, 1, 1); // north east
    find_neighbor(yes_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});
    }
  }
}

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

  update_sorted(sprite_distance, yes_collision, from, max_dist);
  update_sorted(sprite_distance, no_collision, from, max_dist);

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

  return sprite_distance;
}