#include "path/path_planner.hpp"

#include <algorithm>
#include <cmath>
#include <limits>
#include <queue>
#include <unordered_map>

namespace lm {

namespace {

constexpr double kInf = std::numeric_limits<double>::infinity();

double mapResolution(const nlohmann::json& map)
{
  if (map.contains("resolution") && map["resolution"].is_number())
    return std::max(0.01, map["resolution"].get<double>());
  return 0.05;
}

double mapImageHeight(const nlohmann::json& map)
{
  if (map.contains("height") && map["height"].is_number())
    return map["height"].get<double>();
  return 1000.0;
}

}  // namespace

std::pair<double, double> PathPlanner::worldToPixel(const nlohmann::json& map, double wx, double wy, double img_h)
{
  const double res = mapResolution(map);
  const double ox = map.value("origin_x", 0.0);
  const double oy = map.value("origin_y", 0.0);
  return {((wx - ox) / res), (img_h - (wy - oy) / res)};
}

WorldPoint PathPlanner::pixelToWorld(const nlohmann::json& map, double px, double py, double img_h)
{
  const double res = mapResolution(map);
  const double ox = map.value("origin_x", 0.0);
  const double oy = map.value("origin_y", 0.0);
  return {ox + px * res, oy + (img_h - py) * res};
}

bool PathPlanner::pointInPolygon(double px, double py, 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 > py) != (yj > py)) && (px < (xj - xi) * (py - yi) / ((yj - yi) + 1e-12) + xi);
    if (intersect)
      inside = !inside;
    j = i;
  }
  return inside;
}

bool PathPlanner::pointNearPolyline(double px, double py, const nlohmann::json& points, double half_width)
{
  if (!points.is_array() || points.size() < 2)
    return false;
  const double limit = half_width + 4.0;
  for (size_t j = 0; j + 1 < points.size(); ++j)
  {
    const auto& p1 = points[j];
    const auto& p2 = points[j + 1];
    if (!p1.is_object() || !p2.is_object())
      continue;
    const double x1 = p1.value("x", 0.0);
    const double y1 = p1.value("y", 0.0);
    const double x2 = p2.value("x", 0.0);
    const double y2 = p2.value("y", 0.0);
    const double dx = x2 - x1;
    const double dy = y2 - y1;
    const double len_sq = dx * dx + dy * dy;
    double dist = 0;
    if (len_sq == 0)
      dist = std::hypot(px - x1, py - y1);
    else
    {
      double t = ((px - x1) * dx + (py - y1) * dy) / len_sq;
      t = std::clamp(t, 0.0, 1.0);
      dist = std::hypot(px - (x1 + t * dx), py - (y1 + t * dy));
    }
    if (dist <= limit)
      return true;
  }
  return false;
}

double PathPlanner::zoneTraversalCost(const nlohmann::json& zones, double px, double py, const nlohmann::json& map)
{
  if (!zones.is_array())
    return 1.0;
  for (auto it = zones.rbegin(); it != zones.rend(); ++it)
  {
    const auto& z = *it;
    if (!z.is_object())
      continue;
    const std::string type = z.value("type", "");
    if (type != "forbidden" && type != "preferred" && type != "unpreferred")
      continue;
    const auto& points = z.value("points", nlohmann::json::array());
    bool hit = false;
    if (z.value("geometry", "") == "line")
    {
      double half = 8.0;
      if (z.contains("line_width_cm") && z["line_width_cm"].is_number())
      {
        const double res = mapResolution(map);
        half = std::max(4.0, z["line_width_cm"].get<double>() / (res * 100.0) / 2.0);
      }
      hit = pointNearPolyline(px, py, points, half);
    }
    else
    {
      hit = pointInPolygon(px, py, points);
    }
    if (!hit)
      continue;
    if (type == "forbidden")
      return kInf;
    if (type == "unpreferred")
      return 4.0;
    if (type == "preferred")
      return 0.35;
  }
  return 1.0;
}

double PathPlanner::cellCost(const nlohmann::json& map, double wx, double wy, double img_h)
{
  const auto px_py = worldToPixel(map, wx, wy, img_h);
  const auto& zones = map.value("zones", nlohmann::json::array());
  return zoneTraversalCost(zones, px_py.first, px_py.second, map);
}

std::optional<WorldPoint> PathPlanner::positionWorld(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 WorldPoint{z.value("x", 0.0), z.value("y", 0.0)};
  }
  return std::nullopt;
}

nlohmann::json PathPlanner::plan(const nlohmann::json& map, const WorldPoint& from, const WorldPoint& to)
{
  nlohmann::json fallback = nlohmann::json::array();
  fallback.push_back({{"x", from.x}, {"y", from.y}});
  fallback.push_back({{"x", to.x}, {"y", to.y}});

  const double img_h = mapImageHeight(map);
  const double res = mapResolution(map);
  const double cell_m = std::max(res * 4.0, 0.2);

  const double min_x = std::min(from.x, to.x) - 2.0;
  const double max_x = std::max(from.x, to.x) + 2.0;
  const double min_y = std::min(from.y, to.y) - 2.0;
  const double max_y = std::max(from.y, to.y) + 2.0;

  const int cols = std::clamp(static_cast<int>(std::ceil((max_x - min_x) / cell_m)) + 1, 8, 120);
  const int rows = std::clamp(static_cast<int>(std::ceil((max_y - min_y) / cell_m)) + 1, 8, 120);

  auto toCell = [&](double wx, double wy) -> GridCell {
    const int cx = std::clamp(static_cast<int>(std::round((wx - min_x) / cell_m)), 0, cols - 1);
    const int cy = std::clamp(static_cast<int>(std::round((wy - min_y) / cell_m)), 0, rows - 1);
    return {cx, cy};
  };

  auto cellWorld = [&](int cx, int cy) -> WorldPoint {
    return {min_x + cx * cell_m, min_y + cy * cell_m};
  };

  const GridCell start = toCell(from.x, from.y);
  const GridCell goal = toCell(to.x, to.y);

  const auto key = [](const GridCell& c) { return std::to_string(c.x) + "," + std::to_string(c.y); };

  std::vector<double> costs(static_cast<size_t>(cols * rows), kInf);
  std::vector<GridCell> parent(static_cast<size_t>(cols * rows), {-1, -1});
  auto idx = [&](int x, int y) { return y * cols + x; };

  struct Node
  {
    double f;
    int x;
    int y;
  };
  auto cmp = [](const Node& a, const Node& b) { return a.f > b.f; };
  std::priority_queue<Node, std::vector<Node>, decltype(cmp)> open(cmp);

  const auto start_cost = cellCost(map, from.x, from.y, img_h);
  if (!std::isfinite(start_cost))
    return fallback;
  costs[static_cast<size_t>(idx(start.x, start.y))] = 0;
  open.push({0, start.x, start.y});

  const int dx[8] = {1, -1, 0, 0, 1, 1, -1, -1};
  const int dy[8] = {0, 0, 1, -1, 1, -1, 1, -1};

  bool found = false;
  while (!open.empty())
  {
    const Node cur = open.top();
    open.pop();
    if (cur.x == goal.x && cur.y == goal.y)
    {
      found = true;
      break;
    }
    const size_t cur_i = static_cast<size_t>(idx(cur.x, cur.y));
    if (cur.f > costs[cur_i] + 1e-6)
      continue;

    for (int k = 0; k < 8; ++k)
    {
      const int nx = cur.x + dx[k];
      const int ny = cur.y + dy[k];
      if (nx < 0 || ny < 0 || nx >= cols || ny >= rows)
        continue;
      const WorldPoint wp = cellWorld(nx, ny);
      const double step = cellCost(map, wp.x, wp.y, img_h);
      if (!std::isfinite(step))
        continue;
      const double move = (k < 4) ? 1.0 : 1.414;
      const size_t ni = static_cast<size_t>(idx(nx, ny));
      const double next = costs[cur_i] + step * move;
      if (next + 1e-9 < costs[ni])
      {
        costs[ni] = next;
        parent[ni] = {cur.x, cur.y};
        const double h = std::hypot(nx - goal.x, ny - goal.y);
        open.push({next + h, nx, ny});
      }
    }
  }

  if (!found)
    return fallback;

  std::vector<GridCell> cells;
  GridCell c = goal;
  while (c.x >= 0)
  {
    cells.push_back(c);
    if (c.x == start.x && c.y == start.y)
      break;
    const size_t pi = static_cast<size_t>(idx(c.x, c.y));
    c = parent[pi];
    if (c.x < 0)
      break;
  }
  std::reverse(cells.begin(), cells.end());

  nlohmann::json out = nlohmann::json::array();
  out.push_back({{"x", from.x}, {"y", from.y}});
  for (size_t i = 1; i + 1 < cells.size(); ++i)
  {
    const WorldPoint wp = cellWorld(cells[i].x, cells[i].y);
    out.push_back({{"x", wp.x}, {"y", wp.y}});
  }
  out.push_back({{"x", to.x}, {"y", to.y}});
  return out;
}

}  // namespace lm
