#include "path/path_service.hpp"

#include "path/path_planner.hpp"
#include "storage/map_store.hpp"
#include "storage/path_store.hpp"

#include <cmath>
#include <unordered_map>

namespace lm {

PathService::PathService(PathStore& paths, MapStore& maps) : paths_(paths), maps_(maps) {}

nlohmann::json PathService::getOrCreatePath(const std::string& map_id,
                                            const std::string& site_id,
                                            const std::string& from_position_id,
                                            const std::string& to_position_id,
                                            std::string& err)
{
  if (auto cached = paths_.findBetween(map_id, from_position_id, to_position_id))
    return *cached;

  const auto map = maps_.find(map_id);
  if (!map)
  {
    err = "map not found";
    return nlohmann::json::object();
  }

  const auto from = PathPlanner::positionWorld(*map, from_position_id);
  const auto to = PathPlanner::positionWorld(*map, to_position_id);
  if (!from || !to)
  {
    err = "position not found on map";
    return nlohmann::json::object();
  }

  const nlohmann::json points = PathPlanner::plan(*map, *from, *to);
  nlohmann::json payload = {{"site_id", site_id.empty() ? map->value("site_id", "") : site_id},
                            {"map_id", map_id},
                            {"from_position_id", from_position_id},
                            {"to_position_id", to_position_id},
                            {"points", points},
                            {"auto_created", true}};
  const auto saved = paths_.upsert(payload, err);
  return saved ? *saved : nlohmann::json::object();
}

nlohmann::json PathService::pathsBetweenPositions(const std::string& map_id,
                                                  const std::string& from_position_id,
                                                  const std::string& to_position_id) const
{
  if (auto hit = paths_.findBetween(map_id, from_position_id, to_position_id))
    return *hit;
  return nlohmann::json::object();
}

void PathService::invalidateMapPositions(const nlohmann::json& old_map, const nlohmann::json& new_map)
{
  if (!old_map.is_object() || !new_map.is_object())
    return;
  const std::string map_id = new_map.value("id", old_map.value("id", ""));
  if (map_id.empty())
    return;

  auto positionSnapshot = [](const nlohmann::json& map) {
    std::unordered_map<std::string, std::pair<double, double>> out;
    const auto& zones = map.value("zones", nlohmann::json::array());
    if (!zones.is_array())
      return out;
    for (const auto& z : zones)
    {
      if (!z.is_object() || z.value("type", "") != "position")
        continue;
      const std::string id = z.value("id", "");
      if (id.empty())
        continue;
      out[id] = {z.value("x", 0.0), z.value("y", 0.0)};
    }
    return out;
  };

  const auto before = positionSnapshot(old_map);
  const auto after = positionSnapshot(new_map);
  for (const auto& [id, coords] : after)
  {
    const auto it = before.find(id);
    if (it == before.end())
    {
      paths_.removeForPosition(map_id, id);
      continue;
    }
    const double eps = 1e-4;
    if (std::abs(it->second.first - coords.first) > eps || std::abs(it->second.second - coords.second) > eps)
      paths_.removeForPosition(map_id, id);
  }
  for (const auto& [id, _] : before)
  {
    if (after.find(id) == after.end())
      paths_.removeForPosition(map_id, id);
  }
}

}  // namespace lm
