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

namespace factory {
Simulation::Simulation(Project project) : project_(std::move(project)) {
  const auto errors = project_.validate();
  if (!errors.empty())
    throw std::invalid_argument(errors.front());
  components_.assign(project_.tiles.size(), -1);
  int group = 0;
  for (int y = 0; y < project_.height; ++y)
    for (int x = 0; x < project_.width; ++x) {
      const Position start{x, y};
      if (!project_.traversable(start) || component(start) >= 0)
        continue;
      std::queue<Position> pending;
      pending.push(start);
      components_.at(static_cast<std::size_t>(y) * static_cast<std::size_t>(project_.width) +
                     static_cast<std::size_t>(x)) = group;
      while (!pending.empty()) {
        const auto position = pending.front();
        pending.pop();
        for (const auto next : project_.neighbors(position))
          if (component(next) < 0) {
            components_.at(static_cast<std::size_t>(next.y) *
                               static_cast<std::size_t>(project_.width) +
                           static_cast<std::size_t>(next.x)) = group;
            pending.push(next);
          }
      }
      ++group;
    }
  for (const auto& spec : project_.vehicles)
    state_.vehicles.push_back({spec, spec.home, Phase::Idle, std::nullopt, 0});
  std::ranges::sort(state_.vehicles, {}, [](const Vehicle& v) { return v.spec.id; });
  assigned_.resize(project_.orders.size());
  delivered_.resize(project_.orders.size());
}
int Simulation::component(Position position) const {
  if (!project_.contains(position))
    return -1;
  return components_.at(static_cast<std::size_t>(position.y) *
                            static_cast<std::size_t>(project_.width) +
                        static_cast<std::size_t>(position.x));
}
Position Simulation::target(const Vehicle& v) const {
  if (v.phase == Phase::Pickup || v.phase == Phase::Delivery) {
    if (!v.order.has_value())
      throw std::logic_error("Transport phase requires an order");
    const auto& order = project_.orders.at(v.order.value());
    return v.phase == Phase::Pickup ? order.pickup : order.delivery;
  }
  return v.spec.home;
}
void Simulation::arrive(Vehicle& v) {
  if (v.position != target(v))
    return;
  if (v.phase == Phase::Pickup)
    v.phase = Phase::Delivery;
  else if (v.phase == Phase::Delivery) {
    if (!v.order.has_value())
      throw std::logic_error("Delivery requires an order");
    delivered_.at(v.order.value()) = true;
    ++state_.delivered;
    v.order.reset();
    v.phase = Phase::Returning;
  } else if (v.phase == Phase::Returning)
    v.phase = Phase::Idle;
}
void Simulation::step() {
  if (state_.outcome != Outcome::Running)
    return;
  ++state_.tick;
  state_.blocked = 0;
  state_.diagnostics.clear();
  for (auto& v : state_.vehicles) {
    arrive(v);
    if (v.phase != Phase::Idle)
      continue;
    std::optional<std::size_t> pending;
    std::size_t bestDistance = project_.tiles.size() + 1;
    std::vector<std::pair<int, std::size_t>> candidates;
    candidates.reserve(assigned_.size());
    for (std::size_t i = 0; i < assigned_.size(); ++i) {
      if (assigned_[i])
        continue;
      if (component(v.position) != component(project_.orders[i].pickup) ||
          component(project_.orders[i].pickup) != component(project_.orders[i].delivery))
        continue;
      const auto pickup = project_.orders[i].pickup;
      candidates.emplace_back(
          std::abs(v.position.x - pickup.x) + std::abs(v.position.y - pickup.y) + 1, i);
    }
    std::ranges::sort(candidates);
    for (const auto& [lowerBound, i] : candidates) {
      if (static_cast<std::size_t>(lowerBound) > bestDistance)
        break;
      const auto approach = findPath(project_, v.position, project_.orders[i].pickup);
      if (!approach.empty() &&
          (approach.size() < bestDistance ||
           (pending && approach.size() == bestDistance && i < pending.value()))) {
        pending = i;
        bestDistance = approach.size();
      }
    }
    if (pending) {
      const auto index = *pending;
      assigned_[index] = true;
      v.order = index;
      v.phase = Phase::Pickup;
      arrive(v);
    }
  }
  for (std::size_t i = 0; i < assigned_.size(); ++i)
    if (!assigned_[i] &&
        component(project_.orders[i].pickup) != component(project_.orders[i].delivery))
      state_.diagnostics.push_back("Order " + std::to_string(project_.orders[i].id) +
                                   ": no connected route");
  constexpr int horizon = 12;
  Reservations planned;
  std::vector<std::size_t> priority(state_.vehicles.size());
  std::iota(priority.begin(), priority.end(), std::size_t{0});
  std::ranges::stable_sort(priority, [&](std::size_t a, std::size_t b) {
    return state_.vehicles[a].waitingTicks > state_.vehicles[b].waitingTicks;
  });
  std::vector<Position> next;
  next.reserve(state_.vehicles.size());
  for (const auto& v : state_.vehicles)
    next.push_back(v.position);
  std::vector<bool> processed(state_.vehicles.size());
  for (auto index : priority) {
    auto& v = state_.vehicles[index];
    auto reservations = planned;
    // Unplanned vehicles hold their current cells. This conservative policy never
    // assumes that another vehicle will move away before its plan has been accepted.
    for (std::size_t other = 0; other < state_.vehicles.size(); ++other)
      if (other != index && !processed[other])
        reservations.reserve({state_.vehicles[other].position}, horizon);
    auto path = v.phase == Phase::Idle
                    ? std::vector<Position>{v.position}
                    : cooperativePath(project_, v.position, target(v), reservations, horizon);
    if (path.empty())
      path.push_back(v.position);
    next[index] = path.size() > 1 ? path[1] : v.position;
    if (next[index] == v.position && v.phase != Phase::Idle) {
      ++v.waitingTicks;
      ++state_.waitingTicks;
      ++state_.blocked;
      state_.diagnostics.push_back("Vehicle " + std::to_string(v.spec.id) +
                                   (findPath(project_, v.position, target(v)).empty()
                                        ? ": no connected route"
                                        : ": waiting for a reserved route"));
    } else
      v.waitingTicks = 0;
    planned.reserve(path, horizon);
    processed[index] = true;
  }
  for (std::size_t i = 0; i < next.size(); ++i) {
    state_.vehicles[i].position = next[i];
    arrive(state_.vehicles[i]);
  }
  if (state_.delivered == static_cast<int>(project_.orders.size()))
    state_.outcome = Outcome::Won;
  else if (state_.tick >= project_.tickLimit)
    state_.outcome = Outcome::Lost;
}
} // namespace factory
