#include "io/io_zone_runtime.hpp"

#include "io/io_module_service.hpp"
#include "storage/database.hpp"
#include "storage/map_store.hpp"
#include "util/id_util.hpp"

namespace lm {

namespace {

void updatePose(Database& db, double x, double y, double yaw)
{
  nlohmann::json rt;
  if (!db.getDocument("robot_runtime", rt) || !rt.is_object())
    rt = nlohmann::json::object();
  rt["pose"] = {{"x", x}, {"y", y}, {"yaw", yaw}};
  rt["updated_at"] = IdUtil::nowIso8601();
  db.setDocument("robot_runtime", rt);
}

}  // namespace

bool IoZoneRuntime::pointInPolygon(double x, double y, const nlohmann::json& points)
{
  if (!points.is_array() || points.size() < 3)
    return false;
  bool inside = false;
  size_t j = points.size() - 1;
  for (size_t i = 0; i < points.size(); ++i)
  {
    const auto& pi = points[i];
    const auto& pj = points[j];
    if (!pi.is_object() || !pj.is_object())
      continue;
    const double xi = pi.value("x", 0.0);
    const double yi = pi.value("y", 0.0);
    const double xj = pj.value("x", 0.0);
    const double yj = pj.value("y", 0.0);
    const bool intersect = ((yi > y) != (yj > y)) && (x < (xj - xi) * (y - yi) / ((yj - yi) + 1e-12) + xi);
    if (intersect)
      inside = !inside;
    j = i;
  }
  return inside;
}

bool IoZoneRuntime::zoneContainsPoint(const nlohmann::json& zone, double x, double y)
{
  const auto& points = zone.value("points", nlohmann::json::array());
  return pointInPolygon(x, y, points);
}

std::optional<std::pair<double, double>> IoZoneRuntime::positionCoords(const nlohmann::json& map,
                                                                       const std::string& position_id)
{
  const auto& zones = map.value("zones", nlohmann::json::array());
  if (!zones.is_array())
    return std::nullopt;
  for (const auto& z : zones)
  {
    if (!z.is_object() || z.value("type", "") != "position")
      continue;
    if (z.value("id", "") != position_id)
      continue;
    return std::pair<double, double>{z.value("x", 0.0), z.value("y", 0.0)};
  }
  return std::nullopt;
}

void IoZoneRuntime::onRobotAtPosition(Database& db,
                                      MapStore& maps,
                                      IoModuleService& io,
                                      const std::string& map_id,
                                      const std::string& position_id,
                                      nlohmann::json& log)
{
  const auto map = maps.find(map_id);
  if (!map)
    return;
  const auto coords = positionCoords(*map, position_id);
  if (!coords)
    return;

  const double x = coords->first;
  const double y = coords->second;
  updatePose(db, x, y, 0.0);

  const auto& zones = map->value("zones", nlohmann::json::array());
  if (!zones.is_array())
    return;

  for (const auto& zone : zones)
  {
    if (!zone.is_object() || zone.value("type", "") != "io")
      continue;
    if (!zoneContainsPoint(zone, x, y))
      continue;

    const std::string zone_id = zone.value("id", "");
    const std::string key = map_id + ":" + zone_id;
    if (key == last_zone_key_)
      continue;
    last_zone_key_ = key;

    std::string err;
    if (!io.applyZoneSettings(zone, err))
    {
      log.push_back({{"ts", IdUtil::nowIso8601()},
                     {"level", "error"},
                     {"message", "I/O zone failed: " + err}});
      continue;
    }
    log.push_back({{"ts", IdUtil::nowIso8601()},
                   {"level", "info"},
                   {"message", "I/O zone activated: " + zone.value("io_module", "")}});
  }
}

}  // namespace lm
