#include "navigation/PathFinder.h"
#include "external/nlohmann/json.hpp"
#include "utils/Utils.h"
#include <algorithm>
#include <cmath>
#include <fstream>
#include <iostream>
#include <sstream>
#include <limits>
#include <queue>

namespace ZL {

using json = nlohmann::json;

namespace {
    const float INF_COST = (std::numeric_limits<float>::max)();

    float distanceCells(const PathFinder::Cell& a, const PathFinder::Cell& b)
    {
        const float dx = static_cast<float>(a.x - b.x);
        const float dz = static_cast<float>(a.z - b.z);
        return std::sqrt(dx * dx + dz * dz);
    }

    std::vector<Eigen::Vector2f> readPolygon(const json& item)
    {
        std::vector<Eigen::Vector2f> polygon;
        if (!item.contains("polygon") || !item["polygon"].is_array()) {
            return polygon;
        }

        for (const auto& point : item["polygon"]) {
            if (point.is_array() && point.size() >= 2) {
                polygon.emplace_back(point[0].get<float>(), point[1].get<float>());
            } else if (point.is_object()) {
                polygon.emplace_back(point.value("x", 0.0f), point.value("z", 0.0f));
            }
        }

        return polygon;
    }

    float distancePointToSegment2D(const Eigen::Vector2f& p,
                                   const Eigen::Vector2f& a,
                                   const Eigen::Vector2f& b)
    {
        const Eigen::Vector2f ab = b - a;
        const float abLenSq = ab.squaredNorm();
        if (abLenSq <= 1e-8f) {
            return (p - a).norm();
        }
        float t = (p - a).dot(ab) / abLenSq;
        t = (std::max)(0.0f, (std::min)(1.0f, t));
        const Eigen::Vector2f proj = a + t * ab;
        return (p - proj).norm();
    }

    Eigen::Vector2f closestPointOnSegment2D(const Eigen::Vector2f& p,
                                            const Eigen::Vector2f& a,
                                            const Eigen::Vector2f& b)
    {
        const Eigen::Vector2f ab = b - a;
        const float abLenSq = ab.squaredNorm();
        if (abLenSq <= 1e-8f) {
            return a;
        }

        float t = (p - a).dot(ab) / abLenSq;
        t = (std::max)(0.0f, (std::min)(1.0f, t));
        return a + t * ab;
    }

    float distancePointToPolygonEdges(const Eigen::Vector2f& p,
                                      const std::vector<Eigen::Vector2f>& polygon)
    {
        if (polygon.size() < 2) {
            return (std::numeric_limits<float>::max)();
        }

        float best = (std::numeric_limits<float>::max)();
        for (size_t i = 0; i < polygon.size(); ++i) {
            const Eigen::Vector2f& a = polygon[i];
            const Eigen::Vector2f& b = polygon[(i + 1) % polygon.size()];
            best = (std::min)(best, distancePointToSegment2D(p, a, b));
        }
        return best;
    }
}

void PathFinder::build(const std::string& configPath,
                       const std::string& zipPath)
{
    ready = false;
    loadedConfigPath = configPath;
    loadedZipPath = zipPath;

    const bool isTxt = configPath.size() >= 4 &&
                       configPath.compare(configPath.size() - 4, 4, ".txt") == 0;
    if (isTxt) {
        if (loadGrid(configPath, zipPath)) {
            ready = true;
            std::cout << "[nav] Loaded precomputed grid: " << gridWidth << "x" << gridDepth
                      << ", cell=" << cellSize << "\n";
        } else {
            std::cerr << "[nav] Failed to load precomputed grid: " << configPath << "\n";
        }
        return;
    }

    loadConfig(configPath, zipPath);
    resetGridBounds();

    if (gridWidth <= 0 || gridDepth <= 0) {
        std::cerr << "[nav] Could not build grid: no navigation polygons\n";
        walkable.clear();
        return;
    }

    rebuildWalkableGrid();
    ready = true;

    std::cout << "[nav] Polygon grid built: " << gridWidth << "x" << gridDepth
              << ", cell=" << cellSize << ", areas=" << areas.size() << "\n";
}

std::vector<Eigen::Vector3f> PathFinder::findPath(const Eigen::Vector3f& start,
                                                  const Eigen::Vector3f& end) const
{
    if (!ready || walkable.empty()) {
        return {};
    }

    Cell startCell;
    Cell endCell;
    if (!findNearestWalkableCell(start, startCell, walkable) ||
        !findNearestWalkableCell(end, endCell, walkable)) {
        return {};
    }

    if (startCell.x == endCell.x && startCell.z == endCell.z) {
        return { cellCenter(endCell) };
    }

    struct QueueNode {
        int index = 0;
        float priority = 0.0f;

        bool operator<(const QueueNode& other) const
        {
            return priority > other.priority;
        }
    };

    const int cellCount = gridWidth * gridDepth;
    std::vector<float> cost(static_cast<size_t>(cellCount), INF_COST);
    std::vector<int> cameFrom(static_cast<size_t>(cellCount), -1);
    std::priority_queue<QueueNode> open;

    const int startIndex = indexOf(startCell);
    const int endIndex = indexOf(endCell);

    cost[static_cast<size_t>(startIndex)] = 0.0f;
    open.push({ startIndex, 0.0f });

    static const int offsets[8][2] = {
        { 1, 0 }, { -1, 0 }, { 0, 1 }, { 0, -1 },
        { 1, 1 }, { 1, -1 }, { -1, 1 }, { -1, -1 }
    };

    while (!open.empty()) {
        const QueueNode current = open.top();
        open.pop();

        if (current.index == endIndex) {
            break;
        }

        const Cell currentCell{ current.index % gridWidth, current.index / gridWidth };

        for (const auto& offset : offsets) {
            Cell next{ currentCell.x + offset[0], currentCell.z + offset[1] };
            if (!isCellWalkable(next, walkable)) {
                continue;
            }

            const bool diagonal = offset[0] != 0 && offset[1] != 0;
            if (diagonal) {
                Cell horizontal{ currentCell.x + offset[0], currentCell.z };
                Cell vertical{ currentCell.x, currentCell.z + offset[1] };
                if (!isCellWalkable(horizontal, walkable) || !isCellWalkable(vertical, walkable)) {
                    continue;
                }
            }

            const int nextIndex = indexOf(next);
            const float stepCost = diagonal ? 1.41421356f : 1.0f;
            const float newCost = cost[static_cast<size_t>(current.index)] + stepCost;
            if (newCost >= cost[static_cast<size_t>(nextIndex)]) {
                continue;
            }

            cost[static_cast<size_t>(nextIndex)] = newCost;
            cameFrom[static_cast<size_t>(nextIndex)] = current.index;
            const float priority = newCost + distanceCells(next, endCell);
            open.push({ nextIndex, priority });
        }
    }

    if (cameFrom[static_cast<size_t>(endIndex)] == -1) {
        return {};
    }

    std::vector<Cell> cells;
    for (int current = endIndex; current != -1; current = cameFrom[static_cast<size_t>(current)]) {
        cells.push_back({ current % gridWidth, current / gridWidth });
        if (current == startIndex) {
            break;
        }
    }
    std::reverse(cells.begin(), cells.end());
    cells = smoothCells(cells, walkable);

    std::vector<Eigen::Vector3f> path;
    path.reserve(cells.size());
    for (const Cell& cell : cells) {
        path.push_back(cellCenter(cell));
    }

    if (!path.empty() && (path.front() - Eigen::Vector3f(start.x(), floorY, start.z())).norm() < cellSize * 0.75f) {
        path.erase(path.begin());
    }

    if (!path.empty()) {
        Cell requestedEndCell;
        if (worldToCell(end, requestedEndCell) &&
            requestedEndCell.x == endCell.x &&
            requestedEndCell.z == endCell.z) {
            path.back() = Eigen::Vector3f(end.x(), floorY, end.z());
        }
    }

    return path;
}

std::vector<Eigen::Vector3f> PathFinder::findPath(const Eigen::Vector3f& start,
                                                  const Eigen::Vector3f& end,
                                                  const std::vector<DynamicObstacle>& dynamicObstacles) const
{
    if (!ready || walkable.empty()) {
        return {};
    }

    std::vector<unsigned char> walkableGrid = walkable;
    if (!dynamicObstacles.empty()) {
        for (const DynamicObstacle& obstacle : dynamicObstacles) {
            const float radius = obstacle.radius + agentRadius;
            if (radius <= 0.0f) continue;

            const float minWorldX = obstacle.position.x() - radius;
            const float maxWorldX = obstacle.position.x() + radius;
            const float minWorldZ = obstacle.position.z() - radius;
            const float maxWorldZ = obstacle.position.z() + radius;

            const int minCellX = (std::max)(0, static_cast<int>(std::floor((minWorldX - minX) / cellSize)));
            const int maxCellX = (std::min)(gridWidth - 1, static_cast<int>(std::floor((maxWorldX - minX) / cellSize)));
            const int minCellZ = (std::max)(0, static_cast<int>(std::floor((minWorldZ - minZ) / cellSize)));
            const int maxCellZ = (std::min)(gridDepth - 1, static_cast<int>(std::floor((maxWorldZ - minZ) / cellSize)));

            const float radiusSq = radius * radius;
            for (int z = minCellZ; z <= maxCellZ; ++z) {
                for (int x = minCellX; x <= maxCellX; ++x) {
                    const Cell cell{ x, z };
                    const Eigen::Vector3f center = cellCenter(cell);
                    const float dx = center.x() - obstacle.position.x();
                    const float dz = center.z() - obstacle.position.z();
                    if (dx * dx + dz * dz <= radiusSq) {
                        const int idx = indexOf(cell);
                        if (idx >= 0 && idx < static_cast<int>(walkableGrid.size())) {
                            walkableGrid[static_cast<size_t>(idx)] = 0;
                        }
                    }
                }
            }
        }
    }

    Cell startCell;
    Cell endCell;
    if (!findNearestWalkableCell(start, startCell, walkableGrid) ||
        !findNearestWalkableCell(end, endCell, walkableGrid)) {
        return {};
    }

    if (startCell.x == endCell.x && startCell.z == endCell.z) {
        return { cellCenter(endCell) };
    }

    struct QueueNode {
        int index = 0;
        float priority = 0.0f;

        bool operator<(const QueueNode& other) const
        {
            return priority > other.priority;
        }
    };

    const int cellCount = gridWidth * gridDepth;
    std::vector<float> cost(static_cast<size_t>(cellCount), INF_COST);
    std::vector<int> cameFrom(static_cast<size_t>(cellCount), -1);
    std::priority_queue<QueueNode> open;

    const int startIndex = indexOf(startCell);
    const int endIndex = indexOf(endCell);

    cost[static_cast<size_t>(startIndex)] = 0.0f;
    open.push({ startIndex, 0.0f });

    static const int offsets[8][2] = {
        { 1, 0 }, { -1, 0 }, { 0, 1 }, { 0, -1 },
        { 1, 1 }, { 1, -1 }, { -1, 1 }, { -1, -1 }
    };

    while (!open.empty()) {
        const QueueNode current = open.top();
        open.pop();

        if (current.index == endIndex) {
            break;
        }

        const Cell currentCell{ current.index % gridWidth, current.index / gridWidth };

        for (const auto& offset : offsets) {
            Cell next{ currentCell.x + offset[0], currentCell.z + offset[1] };
            if (!isCellWalkable(next, walkableGrid)) {
                continue;
            }

            const bool diagonal = offset[0] != 0 && offset[1] != 0;
            if (diagonal) {
                Cell horizontal{ currentCell.x + offset[0], currentCell.z };
                Cell vertical{ currentCell.x, currentCell.z + offset[1] };
                if (!isCellWalkable(horizontal, walkableGrid) || !isCellWalkable(vertical, walkableGrid)) {
                    continue;
                }
            }

            const int nextIndex = indexOf(next);
            const float stepCost = diagonal ? 1.41421356f : 1.0f;
            const float newCost = cost[static_cast<size_t>(current.index)] + stepCost;
            if (newCost >= cost[static_cast<size_t>(nextIndex)]) {
                continue;
            }

            cost[static_cast<size_t>(nextIndex)] = newCost;
            cameFrom[static_cast<size_t>(nextIndex)] = current.index;
            const float priority = newCost + distanceCells(next, endCell);
            open.push({ nextIndex, priority });
        }
    }

    if (cameFrom[static_cast<size_t>(endIndex)] == -1) {
        return {};
    }

    std::vector<Cell> cells;
    for (int current = endIndex; current != -1; current = cameFrom[static_cast<size_t>(current)]) {
        cells.push_back({ current % gridWidth, current / gridWidth });
        if (current == startIndex) {
            break;
        }
    }
    std::reverse(cells.begin(), cells.end());
    cells = smoothCells(cells, walkableGrid);

    std::vector<Eigen::Vector3f> path;
    path.reserve(cells.size());
    for (const Cell& cell : cells) {
        path.push_back(cellCenter(cell));
    }

    if (!path.empty() && (path.front() - Eigen::Vector3f(start.x(), floorY, start.z())).norm() < cellSize * 0.75f) {
        path.erase(path.begin());
    }

    if (!path.empty()) {
        Cell requestedEndCell;
        if (worldToCell(end, requestedEndCell) &&
            requestedEndCell.x == endCell.x &&
            requestedEndCell.z == endCell.z) {
            path.back() = Eigen::Vector3f(end.x(), floorY, end.z());
        }
    }

    return path;
}

std::vector<Eigen::Vector3f> PathFinder::findNearestReachableImpl(
    const Eigen::Vector3f& start,
    const Eigen::Vector3f& end,
    const std::vector<unsigned char>& walkableGrid) const
{
    if (!ready || walkableGrid.empty()) return {};

    Cell startCell;
    Cell endCell;
    if (!findNearestWalkableCell(start, startCell, walkableGrid)) return {};
    if (!findNearestWalkableCell(end,   endCell,   walkableGrid)) return {};

    struct QueueNode {
        int index = 0;
        float priority = 0.0f;
        bool operator<(const QueueNode& other) const { return priority > other.priority; }
    };

    const int cellCount = gridWidth * gridDepth;
    std::vector<float> cost(static_cast<size_t>(cellCount), INF_COST);
    std::vector<int> cameFrom(static_cast<size_t>(cellCount), -1);
    std::priority_queue<QueueNode> open;

    const int startIndex = indexOf(startCell);
    const int endIndex   = indexOf(endCell);

    cost[static_cast<size_t>(startIndex)] = 0.f;
    open.push({ startIndex, 0.f });

    int   bestIndex = startIndex;
    float bestDist  = distanceCells(startCell, endCell);

    static const int offsets[8][2] = {
        { 1, 0 }, { -1, 0 }, { 0, 1 }, { 0, -1 },
        { 1, 1 }, { 1, -1 }, { -1, 1 }, { -1, -1 }
    };

    while (!open.empty()) {
        const QueueNode current = open.top();
        open.pop();

        if (current.index == endIndex) {
            bestIndex = endIndex;
            break;
        }

        if (cost[static_cast<size_t>(current.index)] < current.priority - distanceCells({ current.index % gridWidth, current.index / gridWidth }, endCell)) {
            continue; // stale entry
        }

        const Cell currentCell{ current.index % gridWidth, current.index / gridWidth };
        const float d = distanceCells(currentCell, endCell);
        if (d < bestDist) {
            bestDist  = d;
            bestIndex = current.index;
        }

        for (const auto& offset : offsets) {
            Cell next{ currentCell.x + offset[0], currentCell.z + offset[1] };
            if (!isCellWalkable(next, walkableGrid)) continue;

            const bool diagonal = offset[0] != 0 && offset[1] != 0;
            if (diagonal) {
                Cell h{ currentCell.x + offset[0], currentCell.z };
                Cell v{ currentCell.x, currentCell.z + offset[1] };
                if (!isCellWalkable(h, walkableGrid) || !isCellWalkable(v, walkableGrid)) continue;
            }

            const int nextIndex   = indexOf(next);
            const float stepCost  = diagonal ? 1.41421356f : 1.0f;
            const float newCost   = cost[static_cast<size_t>(current.index)] + stepCost;
            if (newCost >= cost[static_cast<size_t>(nextIndex)]) continue;

            cost[static_cast<size_t>(nextIndex)]    = newCost;
            cameFrom[static_cast<size_t>(nextIndex)] = current.index;
            open.push({ nextIndex, newCost + distanceCells(next, endCell) });
        }
    }

    if (bestIndex == startIndex) return {};

    std::vector<Cell> cells;
    for (int cur = bestIndex; cur != -1; cur = cameFrom[static_cast<size_t>(cur)]) {
        cells.push_back({ cur % gridWidth, cur / gridWidth });
        if (cur == startIndex) break;
    }
    std::reverse(cells.begin(), cells.end());
    cells = smoothCells(cells, walkableGrid);

    std::vector<Eigen::Vector3f> path;
    path.reserve(cells.size());
    for (const Cell& cell : cells) path.push_back(cellCenter(cell));

    if (!path.empty() && (path.front() - Eigen::Vector3f(start.x(), floorY, start.z())).norm() < cellSize * 0.75f)
        path.erase(path.begin());

    return path;
}

std::vector<Eigen::Vector3f> PathFinder::findPathToNearest(
    const Eigen::Vector3f& start, const Eigen::Vector3f& end) const
{
    auto path = findPath(start, end);
    if (!path.empty()) return path;
    return findNearestReachableImpl(start, end, walkable);
}

std::vector<Eigen::Vector3f> PathFinder::findPathToNearest(
    const Eigen::Vector3f& start, const Eigen::Vector3f& end,
    const std::vector<DynamicObstacle>& dynamicObstacles) const
{
    auto path = findPath(start, end, dynamicObstacles);
    if (!path.empty()) return path;
    path = findPath(start, end);
    if (!path.empty()) return path;
    return findNearestReachableImpl(start, end, walkable);
}

bool PathFinder::setAreaAvailable(const std::string& areaName, bool available)
{
    bool found = false;
    bool changed = false;
    for (NavigationArea& area : areas) {
        if (area.name == areaName) {
            found = true;
            changed = changed || area.available != available;
            area.available = available;
        }
    }

    if (changed) {
        rebuildWalkableGrid();
    }

    return found;
}

bool PathFinder::isWalkable(const Eigen::Vector3f& point) const
{
    Cell cell;
    return worldToCell(point, cell) && isCellWalkable(cell, walkable);
}

void PathFinder::addObstaclePolygon(const ObstaclePolygon& polygon)
{
    obstaclePolygons.push_back(polygon);
    if (ready) {
        rebuildWalkableGrid();
    }
}

bool PathFinder::saveConfig(const std::string& path) const
{
    json root;
    root["cellSize"]        = cellSize;
    root["agentRadius"]     = agentRadius;
    root["floorY"]          = floorY;
    root["objectPadding"]   = objectPadding;
    root["boundaryPadding"] = boundaryPadding;

    json areasArr = json::array();
    for (const auto& area : areas) {
        json areaObj;
        areaObj["name"]      = area.name;
        areaObj["available"] = area.available;
        json poly = json::array();
        for (const auto& pt : area.polygon) {
            poly.push_back(json::array({ pt.x(), pt.y() }));
        }
        areaObj["polygon"] = poly;
        areasArr.push_back(areaObj);
    }
    root["areas"] = areasArr;

    json obstArr = json::array();
    for (const auto& obs : obstaclePolygons) {
        json obsObj;
        obsObj["name"] = obs.name;
        json poly = json::array();
        for (const auto& pt : obs.polygon) {
            poly.push_back(json::array({ pt.x(), pt.y() }));
        }
        obsObj["polygon"] = poly;
        obstArr.push_back(obsObj);
    }
    root["obstacles"] = obstArr;

    std::ofstream f(path);
    if (!f.is_open()) {
        return false;
    }
    f << root.dump(2);
    return f.good();
}

bool PathFinder::saveGrid(const std::string& path) const
{
    if (!ready) return false;
    std::ofstream f(path);
    if (!f.is_open()) return false;

    f << "cellSize "        << cellSize        << "\n"
      << "agentRadius "     << agentRadius     << "\n"
      << "floorY "          << floorY          << "\n"
      << "objectPadding "   << objectPadding   << "\n"
      << "boundaryPadding " << boundaryPadding << "\n"
      << "minX "            << minX            << "\n"
      << "minZ "            << minZ            << "\n"
      << "gridWidth "       << gridWidth       << "\n"
      << "gridDepth "       << gridDepth       << "\n";

    for (int z = 0; z < gridDepth; ++z) {
        for (int x = 0; x < gridWidth; ++x) {
            f << (walkable[static_cast<size_t>(z * gridWidth + x)] ? '1' : '0');
        }
        f << '\n';
    }
    return f.good();
}

bool PathFinder::loadGrid(const std::string& path, const std::string& zipPath)
{
    std::string content;
    try {
        if (!zipPath.empty()) {
            const auto bytes = readFileFromZIP(path, zipPath);
            content.assign(bytes.begin(), bytes.end());
        } else {
            content = readTextFile(path);
        }
    } catch (const std::exception& e) {
        std::cerr << "[nav] loadGrid error: " << e.what() << "\n";
        return false;
    }
    if (content.empty()) return false;

    std::istringstream ss(content);
    std::string line;

    auto tryHeader = [&](const std::string& ln) -> bool {
        std::istringstream ls(ln);
        std::string key;
        if (!(ls >> key)) return false;
        if      (key == "cellSize")        { ls >> cellSize;        return true; }
        else if (key == "agentRadius")     { ls >> agentRadius;     return true; }
        else if (key == "floorY")          { ls >> floorY;          return true; }
        else if (key == "objectPadding")   { ls >> objectPadding;   return true; }
        else if (key == "boundaryPadding") { ls >> boundaryPadding; return true; }
        else if (key == "minX")            { ls >> minX;            return true; }
        else if (key == "minZ")            { ls >> minZ;            return true; }
        else if (key == "gridWidth")       { ls >> gridWidth;       return true; }
        else if (key == "gridDepth")       { ls >> gridDepth;       return true; }
        return false;
    };

    std::vector<std::string> rows;
    bool headerDone = false;
    while (std::getline(ss, line)) {
        if (!headerDone && tryHeader(line)) continue;
        headerDone = true;
        if (!line.empty()) rows.push_back(line);
    }

    if (gridWidth <= 0 || gridDepth <= 0) return false;

    walkable.assign(static_cast<size_t>(gridWidth * gridDepth), 0);
    for (int z = 0; z < static_cast<int>(rows.size()) && z < gridDepth; ++z) {
        const std::string& row = rows[z];
        for (int x = 0; x < static_cast<int>(row.size()) && x < gridWidth; ++x) {
            walkable[static_cast<size_t>(z * gridWidth + x)] = (row[x] == '1') ? 1 : 0;
        }
    }
    return true;
}

void PathFinder::loadConfig(const std::string& configPath, const std::string& zipPath)
{
    cellSize = 0.4f;
    agentRadius = 0.45f;
    floorY = 0.0f;
    objectPadding = 0.25f;
    boundaryPadding = 0.0f;
    areas.clear();
    obstaclePolygons.clear();

    try {
        std::string content;
        if (!zipPath.empty()) {
            const std::vector<char> bytes = readFileFromZIP(configPath, zipPath);
            content.assign(bytes.begin(), bytes.end());
        } else {
            content = readTextFile(configPath);
        }

        if (content.empty()) {
            std::cerr << "[nav] Navigation config is empty or missing: " << configPath << "\n";
            return;
        }

        const json root = json::parse(content);
        cellSize = root.value("cellSize", cellSize);
        agentRadius = root.value("agentRadius", agentRadius);
        floorY = root.value("floorY", floorY);
        objectPadding = root.value("objectPadding", objectPadding);
        boundaryPadding = root.value("boundaryPadding", boundaryPadding);

        if (!root.contains("areas") || !root["areas"].is_array()) {
            std::cerr << "[nav] Navigation config has no 'areas' array: " << configPath << "\n";
            return;
        }

        for (const auto& item : root["areas"]) {
            NavigationArea area;
            area.name = item.value("name", "");
            area.available = item.value("available", true);
            area.polygon = readPolygon(item);

            if (area.name.empty()) {
                std::cerr << "[nav] Skipping navigation area without name\n";
                continue;
            }
            if (area.polygon.size() < 3) {
                std::cerr << "[nav] Skipping navigation area '" << area.name << "': polygon has fewer than 3 points\n";
                continue;
            }

            areas.push_back(area);
        }

        if (root.contains("obstacles") && root["obstacles"].is_array()) {
            for (const auto& item : root["obstacles"]) {
                ObstaclePolygon obstacle;
                obstacle.name = item.value("name", "");
                obstacle.polygon = readPolygon(item);
                if (obstacle.polygon.size() < 3) {
                    continue;
                }
                obstaclePolygons.push_back(std::move(obstacle));
            }
        }
    } catch (const std::exception& e) {
        std::cerr << "[nav] Failed to load config '" << configPath << "': " << e.what() << "\n";
    }

    cellSize = (std::max)(cellSize, 0.1f);
    agentRadius = (std::max)(agentRadius, 0.0f);
    objectPadding = (std::max)(objectPadding, 0.0f);
    boundaryPadding = (std::max)(boundaryPadding, 0.0f);
}

void PathFinder::resetGridBounds()
{
    if (areas.empty()) {
        gridWidth = 0;
        gridDepth = 0;
        return;
    }

    minX = areas.front().polygon.front().x();
    float maxX = minX;
    minZ = areas.front().polygon.front().y();
    float maxZ = minZ;

    auto includePoint = [&](float x, float z) {
        minX = (std::min)(minX, x);
        maxX = (std::max)(maxX, x);
        minZ = (std::min)(minZ, z);
        maxZ = (std::max)(maxZ, z);
    };

    for (const NavigationArea& area : areas) {
        for (const Eigen::Vector2f& point : area.polygon) {
            includePoint(point.x(), point.y());
        }
    }

    for (const ObstaclePolygon& obstacle : obstaclePolygons) {
        for (const Eigen::Vector2f& point : obstacle.polygon) {
            includePoint(point.x(), point.y());
        }
    }

    const float padding = cellSize * 2.0f + agentRadius + objectPadding;
    minX -= padding;
    minZ -= padding;
    maxX += padding;
    maxZ += padding;

    gridWidth = static_cast<int>(std::ceil((maxX - minX) / cellSize));
    gridDepth = static_cast<int>(std::ceil((maxZ - minZ) / cellSize));
}

void PathFinder::rebuildWalkableGrid()
{
    walkable.assign(static_cast<size_t>(gridWidth * gridDepth), 0);
    markAvailableAreasWalkable();
    markObstaclePolygonsBlocked();
}

void PathFinder::markAvailableAreasWalkable()
{
    const auto insideAvailableArea = [this](const Eigen::Vector2f& point) {
        for (const NavigationArea& area : areas) {
            if (!area.available) {
                continue;
            }
            if (pointInPolygon(point.x(), point.y(), area.polygon)) {
                return true;
            }
        }
        return false;
    };

    const auto externalBoundaryDistance = [this, &insideAvailableArea](const Eigen::Vector2f& point) {
        if (boundaryPadding <= 0.0f) {
            return (std::numeric_limits<float>::max)();
        }

        const float sampleOffset = (std::max)(cellSize * 0.25f, 0.05f);
        float best = (std::numeric_limits<float>::max)();

        for (const NavigationArea& area : areas) {
            if (!area.available || area.polygon.size() < 2) {
                continue;
            }

            for (size_t i = 0; i < area.polygon.size(); ++i) {
                const Eigen::Vector2f& a = area.polygon[i];
                const Eigen::Vector2f& b = area.polygon[(i + 1) % area.polygon.size()];
                const Eigen::Vector2f edge = b - a;
                if (edge.squaredNorm() <= 1e-8f) {
                    continue;
                }

                const Eigen::Vector2f closest = closestPointOnSegment2D(point, a, b);
                Eigen::Vector2f normal(-edge.y(), edge.x());
                normal.normalize();

                const bool sideAInside = insideAvailableArea(closest + normal * sampleOffset);
                const bool sideBInside = insideAvailableArea(closest - normal * sampleOffset);
                if (sideAInside && sideBInside) {
                    continue;
                }

                best = (std::min)(best, distancePointToSegment2D(point, a, b));
            }
        }

        return best;
    };

    for (int z = 0; z < gridDepth; ++z) {
        for (int x = 0; x < gridWidth; ++x) {
            const Cell cell{ x, z };
            const Eigen::Vector3f center = cellCenter(cell);
            const Eigen::Vector2f point(center.x(), center.z());

            for (const NavigationArea& area : areas) {
                if (!area.available) {
                    continue;
                }
                if (pointInPolygon(center.x(), center.z(), area.polygon)) {
                    if (boundaryPadding > 0.0f && externalBoundaryDistance(point) < boundaryPadding) {
                        continue;
                    }
                    walkable[static_cast<size_t>(indexOf(cell))] = 1;
                    break;
                }
            }
        }
    }
}

void PathFinder::markObstaclePolygonsBlocked()
{
    if (obstaclePolygons.empty()) {
        return;
    }

    const float padding = agentRadius + objectPadding;

    for (int z = 0; z < gridDepth; ++z) {
        for (int x = 0; x < gridWidth; ++x) {
            const Cell cell{ x, z };
            const int idx = indexOf(cell);
            if (idx < 0 || idx >= static_cast<int>(walkable.size())) {
                continue;
            }
            if (!walkable[static_cast<size_t>(idx)]) {
                continue;
            }

            const Eigen::Vector3f center3 = cellCenter(cell);
            const Eigen::Vector2f center(center3.x(), center3.z());

            for (const ObstaclePolygon& obstacle : obstaclePolygons) {
                if (pointInPolygon(center.x(), center.y(), obstacle.polygon)) {
                    walkable[static_cast<size_t>(idx)] = 0;
                    break;
                }
                if (padding > 0.0f &&
                    distancePointToPolygonEdges(center, obstacle.polygon) <= padding) {
                    walkable[static_cast<size_t>(idx)] = 0;
                    break;
                }
            }
        }
    }
}

bool PathFinder::worldToCell(const Eigen::Vector3f& point, Cell& out) const
{
    if (gridWidth <= 0 || gridDepth <= 0) {
        out = {};
        return false;
    }

    out.x = static_cast<int>(std::floor((point.x() - minX) / cellSize));
    out.z = static_cast<int>(std::floor((point.z() - minZ) / cellSize));
    return isInsideGrid(out);
}

Eigen::Vector3f PathFinder::cellCenter(const Cell& cell) const
{
    return {
        minX + (static_cast<float>(cell.x) + 0.5f) * cellSize,
        floorY,
        minZ + (static_cast<float>(cell.z) + 0.5f) * cellSize
    };
}

bool PathFinder::isInsideGrid(const Cell& cell) const
{
    return cell.x >= 0 && cell.z >= 0 && cell.x < gridWidth && cell.z < gridDepth;
}

bool PathFinder::isCellWalkable(const Cell& cell) const
{
    return isCellWalkable(cell, walkable);
}

bool PathFinder::isCellWalkable(const Cell& cell, const std::vector<unsigned char>& walkableGrid) const
{
    const int idx = indexOf(cell);
    return isInsideGrid(cell) &&
           idx >= 0 &&
           idx < static_cast<int>(walkableGrid.size()) &&
           walkableGrid[static_cast<size_t>(idx)] != 0;
}

int PathFinder::indexOf(const Cell& cell) const
{
    return cell.z * gridWidth + cell.x;
}

bool PathFinder::findNearestWalkableCell(const Eigen::Vector3f& point, Cell& out) const
{
    return findNearestWalkableCell(point, out, walkable);
}

bool PathFinder::findNearestWalkableCell(const Eigen::Vector3f& point, Cell& out, const std::vector<unsigned char>& walkableGrid) const
{
    Cell origin;
    if (!worldToCell(point, origin)) {
        const int x = static_cast<int>(std::floor((point.x() - minX) / cellSize));
        const int z = static_cast<int>(std::floor((point.z() - minZ) / cellSize));
        origin.x = (std::min)((std::max)(x, 0), gridWidth - 1);
        origin.z = (std::min)((std::max)(z, 0), gridDepth - 1);
    }

    if (isCellWalkable(origin, walkableGrid)) {
        out = origin;
        return true;
    }

    const int maxRadius = (std::max)(4, static_cast<int>(std::ceil(8.0f / cellSize)));
    for (int radius = 1; radius <= maxRadius; ++radius) {
        for (int dz = -radius; dz <= radius; ++dz) {
            for (int dx = -radius; dx <= radius; ++dx) {
                if (std::abs(dx) != radius && std::abs(dz) != radius) {
                    continue;
                }

                Cell candidate{ origin.x + dx, origin.z + dz };
                if (isCellWalkable(candidate, walkableGrid)) {
                    out = candidate;
                    return true;
                }
            }
        }
    }

    return false;
}

bool PathFinder::hasLineOfSight(const Cell& from, const Cell& to) const
{
    return hasLineOfSight(from, to, walkable);
}

bool PathFinder::hasLineOfSight(const Cell& from, const Cell& to, const std::vector<unsigned char>& walkableGrid) const
{
    const Eigen::Vector3f a = cellCenter(from);
    const Eigen::Vector3f b = cellCenter(to);
    const float distance = (b - a).norm();
    const int steps = (std::max)(1, static_cast<int>(std::ceil(distance / (cellSize * 0.5f))));

    for (int i = 0; i <= steps; ++i) {
        const float t = static_cast<float>(i) / static_cast<float>(steps);
        const Eigen::Vector3f p = a * (1.0f - t) + b * t;
        Cell cell;
        if (!worldToCell(p, cell) || !isCellWalkable(cell, walkableGrid)) {
            return false;
        }
    }

    return true;
}

std::vector<PathFinder::Cell> PathFinder::smoothCells(const std::vector<Cell>& cells) const
{
    return smoothCells(cells, walkable);
}

std::vector<PathFinder::Cell> PathFinder::smoothCells(const std::vector<Cell>& cells, const std::vector<unsigned char>& walkableGrid) const
{
    if (cells.size() <= 2) {
        return cells;
    }

    std::vector<Cell> result;
    size_t anchor = 0;
    result.push_back(cells[anchor]);

    while (anchor < cells.size() - 1) {
        size_t next = cells.size() - 1;
        while (next > anchor + 1 && !hasLineOfSight(cells[anchor], cells[next], walkableGrid)) {
            --next;
        }

        result.push_back(cells[next]);
        anchor = next;
    }

    return result;
}

bool PathFinder::pointInPolygon(float x, float z, const std::vector<Eigen::Vector2f>& polygon)
{
    bool inside = false;
    size_t previous = polygon.size() - 1;

    for (size_t current = 0; current < polygon.size(); ++current) {
        const Eigen::Vector2f& a = polygon[current];
        const Eigen::Vector2f& b = polygon[previous];

        const bool crossesZ = (a.y() > z) != (b.y() > z);
        if (crossesZ) {
            const float t = (z - a.y()) / (b.y() - a.y());
            const float intersectionX = a.x() + t * (b.x() - a.x());
            if (x < intersectionX) {
                inside = !inside;
            }
        }

        previous = current;
    }

    return inside;
}

} // namespace ZL
