#include "navigation/PathFinder.h"
#include "external/nlohmann/json.hpp"
#include "utils/Utils.h"
#include <algorithm>
#include <cmath>
#include <iostream>
#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;
    }
}

void PathFinder::build(const std::vector<ObstacleMesh>& obstacleMeshes,
                       const std::string& configPath,
                       const std::string& zipPath)
{
    ready = false;
    loadedConfigPath = configPath;
    loadedZipPath = zipPath;
    obstacles = obstacleMeshes;

    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) ||
        !findNearestWalkableCell(end, endCell)) {
        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)) {
                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) || !isCellWalkable(vertical)) {
                    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);

    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;
}

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);
}

void PathFinder::loadConfig(const std::string& configPath, const std::string& zipPath)
{
    cellSize = 0.4f;
    agentRadius = 0.45f;
    floorY = 0.0f;
    objectPadding = 0.25f;
    areas.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);

        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);
        }
    } 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);
}

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 ObstacleMesh& obstacle : obstacles) {
        if (!obstacle.mesh) {
            continue;
        }
        for (const Eigen::Vector3f& point : obstacle.mesh->PositionData) {
            const Eigen::Vector3f worldPoint = point + obstacle.offset;
            includePoint(worldPoint.x(), worldPoint.z());
        }
    }

    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();
    markObstacleMeshesBlocked();
}

void PathFinder::markAvailableAreasWalkable()
{
    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);

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

void PathFinder::markObstacleMeshesBlocked()
{
    const float padding = agentRadius + objectPadding;

    for (const ObstacleMesh& obstacle : obstacles) {
        if (!obstacle.mesh || obstacle.mesh->PositionData.empty()) {
            continue;
        }

        Eigen::Vector3f minPoint = obstacle.mesh->PositionData[0] + obstacle.offset;
        Eigen::Vector3f maxPoint = minPoint;
        for (const Eigen::Vector3f& localPoint : obstacle.mesh->PositionData) {
            const Eigen::Vector3f point = localPoint + obstacle.offset;
            minPoint = minPoint.cwiseMin(point);
            maxPoint = maxPoint.cwiseMax(point);
        }

        Cell from;
        Cell to;
        worldToCell({ minPoint.x() - padding, floorY, minPoint.z() - padding }, from);
        worldToCell({ maxPoint.x() + padding, floorY, maxPoint.z() + padding }, to);
        from.x = (std::max)(from.x, 0);
        from.z = (std::max)(from.z, 0);
        to.x = (std::min)(to.x, gridWidth - 1);
        to.z = (std::min)(to.z, gridDepth - 1);

        for (int z = from.z; z <= to.z; ++z) {
            for (int x = from.x; x <= to.x; ++x) {
                const Cell cell{ x, z };
                if (isInsideGrid(cell)) {
                    walkable[static_cast<size_t>(indexOf(cell))] = 0;
                }
            }
        }
    }
}

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 isInsideGrid(cell) && walkable[static_cast<size_t>(indexOf(cell))] != 0;
}

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

bool PathFinder::findNearestWalkableCell(const Eigen::Vector3f& point, Cell& out) 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)) {
        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)) {
                    out = candidate;
                    return true;
                }
            }
        }
    }

    return false;
}

bool PathFinder::hasLineOfSight(const Cell& from, const Cell& to) 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)) {
            return false;
        }
    }

    return true;
}

std::vector<PathFinder::Cell> PathFinder::smoothCells(const std::vector<Cell>& cells) 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])) {
            --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