#include "factory/core/pathfinder.h"
#include <algorithm>
#include <cmath>
#include <queue>

namespace factory {
namespace {
int distance(Position a, Position b) { return std::abs(a.x - b.x) + std::abs(a.y - b.y); }
std::uint64_t positionKey(Position p) {
  return (static_cast<std::uint64_t>(p.y) << 8U) | static_cast<std::uint64_t>(p.x);
}
std::uint64_t cellKey(Position p, int time) {
  return (static_cast<std::uint64_t>(time) << 16U) | positionKey(p);
}
std::uint64_t edgeKey(Position from, Position to, int time) {
  return (static_cast<std::uint64_t>(time) << 32U) | (positionKey(from) << 16U) | positionKey(to);
}
using Node = std::tuple<int, int, int>;
std::vector<Position> search(const Project& p, Position start, Position goal,
                             const Reservations* reservations, int horizon) {
  if (!p.traversable(start) || !p.traversable(goal))
    return {};
  using Entry = std::tuple<int, int, Node>;
  std::priority_queue<Entry, std::vector<Entry>, std::greater<>> open;
  std::map<Node, int> costs;
  std::map<Node, Node> parents;
  Node first{start.x, start.y, 0}, best = first;
  costs[first] = 0;
  open.emplace(distance(start, goal), 0, first);
  while (!open.empty()) {
    auto [score, cost, node] = open.top();
    (void)score;
    open.pop();
    if (costs.at(node) != cost)
      continue;
    auto [x, y, time] = node;
    const Position position{x, y};
    if (distance(position, goal) < distance({std::get<0>(best), std::get<1>(best)}, goal))
      best = node;
    if (position == goal) {
      best = node;
      break;
    }
    if (reservations && time >= horizon)
      continue;
    auto neighbors = p.neighbors(position);
    if (reservations)
      neighbors.push_back(position);
    for (Position next : neighbors) {
      const int nextTime = reservations ? time + 1 : 0;
      if (reservations && !reservations->available(position, next, nextTime))
        continue;
      const Node candidate{next.x, next.y, nextTime};
      const int nextCost = cost + 1;
      if (!costs.contains(candidate) || nextCost < costs.at(candidate)) {
        costs[candidate] = nextCost;
        parents[candidate] = node;
        open.emplace(nextCost + distance(next, goal), nextCost, candidate);
      }
    }
  }
  if (!reservations && Position{std::get<0>(best), std::get<1>(best)} != goal)
    return {};
  std::vector<Position> path;
  for (Node cursor = best;; cursor = parents.at(cursor)) {
    path.push_back({std::get<0>(cursor), std::get<1>(cursor)});
    if (cursor == first)
      break;
  }
  std::ranges::reverse(path);
  return path;
}
} // namespace
bool Reservations::available(Position from, Position to, int time) const {
  return !cells.contains(cellKey(to, time)) && !edges.contains(edgeKey(to, from, time));
}
void Reservations::reserve(const std::vector<Position>& path, int horizon) {
  if (path.empty())
    return;
  for (int t = 0; t <= horizon; ++t) {
    const auto pos = path[std::min(static_cast<std::size_t>(t), path.size() - 1)];
    cells.insert(cellKey(pos, t));
    if (t > 0) {
      const auto previous = path[std::min(static_cast<std::size_t>(t - 1), path.size() - 1)];
      edges.insert(edgeKey(previous, pos, t));
    }
  }
}
std::vector<Position> findPath(const Project& p, Position start, Position target) {
  return search(p, start, target, nullptr, 0);
}
std::vector<Position> cooperativePath(const Project& p, Position start, Position target,
                                      const Reservations& reservations, int horizon) {
  return search(p, start, target, &reservations, horizon);
}
} // namespace factory
