#include "WorldExplorationGrid.h"
#include <cmath>
#include <limits>
#include <algorithm>
#include <unordered_set>

WorldExplorationGrid::WorldExplorationGrid(float cellSize)
    : m_CellSize(cellSize) {
}

void WorldExplorationGrid::Reset() {
    m_ExploredGrid.clear();
}

void WorldExplorationGrid::SetCellSize(float newCellSize) {
    m_CellSize = newCellSize;
    Reset(); // Cell size change invalidates previous data
}

WorldExplorationGrid::GridCoords WorldExplorationGrid::WorldToGrid(const Elite::Vector2& pos) const {
    int gridWidth = static_cast<int>(std::ceil(m_WorldInfo.Dimensions.x / m_CellSize));
    int gridHeight = static_cast<int>(std::ceil(m_WorldInfo.Dimensions.y / m_CellSize));

    int centerX = gridWidth / 2;
    int centerY = gridHeight / 2;

    return {
        static_cast<int>(std::floor(pos.x / m_CellSize)) + centerX,
        static_cast<int>(std::floor(pos.y / m_CellSize)) + centerY
    };
}

Elite::Vector2 WorldExplorationGrid::GridToWorld(const GridCoords& coords) const {
    int gridWidth = static_cast<int>(std::ceil(m_WorldInfo.Dimensions.x / m_CellSize));
    int gridHeight = static_cast<int>(std::ceil(m_WorldInfo.Dimensions.y / m_CellSize));

    int centerX = gridWidth / 2;
    int centerY = gridHeight / 2;

    return {
        (coords.x - centerX + 0.5f) * m_CellSize,
        (coords.y - centerY + 0.5f) * m_CellSize
    };
}

void WorldExplorationGrid::MarkExplored(const std::vector<GridCoords>& cells) {
    for (const auto& cell : cells)
        m_ExploredGrid[cell] = true;
}

std::vector<WorldExplorationGrid::GridCoords> WorldExplorationGrid::GetCellsInRadius(const Elite::Vector2& center, float radius) const {
    std::vector<GridCoords> cells;
    int r = static_cast<int>(std::ceil(radius / m_CellSize));

    GridCoords centerCoords = WorldToGrid(center);
    for (int dx = -r; dx <= r; ++dx) {
        for (int dy = -r; dy <= r; ++dy) {
            GridCoords coords{ centerCoords.x + dx, centerCoords.y + dy };
            Elite::Vector2 cellCenter = GridToWorld(coords);
            if (Elite::Distance(center, cellCenter) <= radius + m_CellSize * 0.5f)
                cells.push_back(coords);
        }
    }
    return cells;
}

std::vector<WorldExplorationGrid::GridCoords> WorldExplorationGrid::GetCellsInRect(const Elite::Vector2& center, const Elite::Vector2& size) const {
    std::vector<GridCoords> cells;
    Elite::Vector2 halfSize = size * 0.5f;
    Elite::Vector2 min = center - halfSize;
    Elite::Vector2 max = center + halfSize;

    GridCoords minCoords = WorldToGrid(min);
    GridCoords maxCoords = WorldToGrid(max);

    for (int x = minCoords.x; x <= maxCoords.x; ++x) {
        for (int y = minCoords.y; y <= maxCoords.y; ++y) {
            cells.push_back({ x, y });
        }
    }
    return cells;
}

void WorldExplorationGrid::SetWorldInfo(const WorldInfo& info) {
    m_WorldInfo = info;

	std::cout << "World info: "
		<< "Dimensions: " << m_WorldInfo.Dimensions.x << " x " << m_WorldInfo.Dimensions.y
		<< ", Cell Size: " << m_CellSize << std::endl;
}

void WorldExplorationGrid::RenderDebug() {
    if (!m_pRenderer) return;
    // Draw explored cells as before
    const Elite::Vector3 exploredColor{ 0.f, 1.f, 0.f };
    const float depth = 0.8f;

    for (const auto& pair : m_ExploredGrid) {
        if (!pair.second) continue;

        Elite::Vector2 center = GridToWorld(pair.first);
        float half = m_CellSize / 2.f;

        Elite::Vector2 verts[4] = {
            center + Elite::Vector2{-half, -half},
            center + Elite::Vector2{ half, -half},
            center + Elite::Vector2{ half,  half},
            center + Elite::Vector2{-half,  half}
        };

        m_pRenderer->Draw_SolidPolygon(verts, 4, exploredColor, depth);
    }

    // Draw the world bounds outline in red
    int gridWidth = static_cast<int>(std::ceil(m_WorldInfo.Dimensions.x / m_CellSize));
    int gridHeight = static_cast<int>(std::ceil(m_WorldInfo.Dimensions.y / m_CellSize));

    Elite::Vector2 topLeft = GridToWorld({ 0, 0 }) - Elite::Vector2{ m_CellSize / 2.f, m_CellSize / 2.f };
    Elite::Vector2 topRight = GridToWorld({ gridWidth - 1, 0 }) + Elite::Vector2{ m_CellSize / 2.f, -m_CellSize / 2.f };
    Elite::Vector2 bottomRight = GridToWorld({ gridWidth - 1, gridHeight - 1 }) + Elite::Vector2{ m_CellSize / 2.f, m_CellSize / 2.f };
    Elite::Vector2 bottomLeft = GridToWorld({ 0, gridHeight - 1 }) + Elite::Vector2{ -m_CellSize / 2.f, m_CellSize / 2.f };

    Elite::Vector2 borderVerts[4] = { topLeft, topRight, bottomRight, bottomLeft };
    const Elite::Vector3 borderColor{ 1.f, 0.f, 0.f }; // red

    m_pRenderer->Draw_Polygon(borderVerts, 4, borderColor, 1.f); // Draw outline

}

bool WorldExplorationGrid::IsExplored(const Elite::Vector2& position) const {
    GridCoords coords = WorldToGrid(position);
    auto it = m_ExploredGrid.find(coords);
    return it != m_ExploredGrid.end() && it->second;
}

void WorldExplorationGrid::Update(const AgentInfo& agentInfo,
    const std::vector<HouseInfo>& visibleHouses) {
    // Mark agent's field of view as explored
    auto visionCells = GetCellsInRadius(agentInfo.Position, agentInfo.FOV_Range / 2);
    MarkExplored(visionCells);

    // Optionally mark house interiors if visible
    for (const auto& house : visibleHouses) {
        auto houseCells = GetCellsInRect(house.Center, house.Size);
        MarkExplored(houseCells);
    }
}

float WorldExplorationGrid::GetExploredPercentage() const {
    if (m_WorldInfo.Dimensions.x <= 0.0f || m_WorldInfo.Dimensions.y <= 0.0f)
        return 0.0f;

    int gridWidth = static_cast<int>(std::ceil(m_WorldInfo.Dimensions.x / m_CellSize));
    int gridHeight = static_cast<int>(std::ceil(m_WorldInfo.Dimensions.y / m_CellSize));
    int totalCells = gridWidth * gridHeight;

    return totalCells == 0 ? 0.0f : static_cast<float>(m_ExploredGrid.size()) / totalCells;
}


float WorldExplorationGrid::GetHouseExploredPercentage(const HouseInfo& house) const {
    auto houseCells = GetCellsInRect(house.Center, house.Size);
    int explored = 0;
    for (const auto& cell : houseCells)
        if (m_ExploredGrid.count(cell) > 0 && m_ExploredGrid.at(cell)) ++explored;

    return houseCells.empty() ? 0.0f : static_cast<float>(explored) / houseCells.size();
}
//
//Elite::Vector2 WorldExplorationGrid::FindNearestUnexplored(const Elite::Vector2& fromPosition) {
//    GridCoords start = WorldToGrid(fromPosition);
//
//    const int maxRadius = 10; // Search range
//    for (int r = 1; r < maxRadius; ++r) {
//        for (int dx = -r; dx <= r; ++dx) {
//            for (int dy = -r; dy <= r; ++dy) {
//                if (std::abs(dx) != r && std::abs(dy) != r) continue; // Only border
//                GridCoords coords{ start.x + dx, start.y + dy };
//                if (m_ExploredGrid.count(coords) == 0)
//                    return GridToWorld(coords);
//            }
//        }
//    }
//
//    return fromPosition; // No unexplored found
//}
//
//Elite::Vector2 WorldExplorationGrid::FindNearestUnexplored(const Elite::Vector2& fromPosition) {
//    // Start search from the last unexplored cell to get a local spiral effect
//    GridCoords start = m_LastUnexploredCell;
//
//    int gridWidth = static_cast<int>(std::ceil(m_WorldInfo.Dimensions.x / m_CellSize));
//    int gridHeight = static_cast<int>(std::ceil(m_WorldInfo.Dimensions.y / m_CellSize));
//
//    // Clamp start inside the world bounds
//    start = ClampToWorldBounds(start);
//
//    const int maxRadius = 10; // max search radius in cells
//
//    for (int r = 0; r <= maxRadius; ++r) {
//        for (int dx = -r; dx <= r; ++dx) {
//            for (int dy = -r; dy <= r; ++dy) {
//                if (std::abs(dx) != r && std::abs(dy) != r) continue; // only border cells at radius r
//
//                GridCoords candidate{ start.x + dx, start.y + dy };
//                candidate = ClampToWorldBounds(candidate);
//
//                if (m_ExploredGrid.count(candidate) == 0) {
//                    m_LastUnexploredCell = candidate; // update last unexplored
//                    return GridToWorld(candidate);
//                }
//            }
//        }
//    }
//
//    // fallback - no unexplored cell found, return current position
//    return fromPosition;
//}

Elite::Vector2 WorldExplorationGrid::FindNearestUnexplored(const Elite::Vector2& fromPosition) {
    GridCoords start = WorldToGrid(fromPosition);

    // We'll use a priority queue that prioritizes cells that are both close to the start
    // and have more explored neighbors
    struct Candidate {
        GridCoords coords;
        float distance;
        int exploredNeighbors;

        bool operator<(const Candidate& other) const {
            // First prioritize cells with more explored neighbors
            if (exploredNeighbors != other.exploredNeighbors)
                return exploredNeighbors < other.exploredNeighbors;
            // Then by distance
            return distance > other.distance;
        }
    };

    std::priority_queue<Candidate> candidates;
    std::unordered_set<GridCoords, GridCoordsHash> visited;

    // Check 4-connected neighbors first for efficiency
    const int dx4[] = { 0, 1, 0, -1 };
    const int dy4[] = { 1, 0, -1, 0 };

    // Start with the immediate neighbors
    for (int i = 0; i < 4; ++i) {
        GridCoords neighbor{ start.x + dx4[i], start.y + dy4[i] };
        if (visited.insert(neighbor).second) {
            float dist = Elite::Distance(fromPosition, GridToWorld(neighbor));
            int exploredNeighbors = CountExploredNeighbors(neighbor);
            candidates.push({ neighbor, dist, exploredNeighbors });
        }
    }

    const int maxSearchRadius = 20; // Limit search area
    int currentRadius = 1;

    while (!candidates.empty()) {
        auto current = candidates.top();
        candidates.pop();

        // If this cell is unexplored, return it
        if (m_ExploredGrid.count(current.coords) == 0) {
            return GridToWorld(current.coords);
        }

        // Expand search to this cell's neighbors
        if (current.distance < maxSearchRadius) {
            for (int i = 0; i < 4; ++i) {
                GridCoords neighbor{ current.coords.x + dx4[i], current.coords.y + dy4[i] };
                if (visited.insert(neighbor).second) {
                    float dist = Elite::Distance(fromPosition, GridToWorld(neighbor));
                    int exploredNeighbors = CountExploredNeighbors(neighbor);
                    candidates.push({ neighbor, dist, exploredNeighbors });
                }
            }
        }
    }

    // Fallback: spiral search if priority queue didn't find anything
    for (int r = 1; r < maxSearchRadius; ++r) {
        for (int dx = -r; dx <= r; ++dx) {
            for (int dy = -r; dy <= r; ++dy) {
                if (std::abs(dx) != r && std::abs(dy) != r) continue; // Only border
                GridCoords coords{ start.x + dx, start.y + dy };
                if (m_ExploredGrid.count(coords) == 0)
                    return GridToWorld(coords);
            }
        }
    }

    return fromPosition; // No unexplored found
}

int WorldExplorationGrid::CountExploredNeighbors(const GridCoords& coords) const {
    const int dx[] = { 0, 1, 0, -1, 1, 1, -1, -1 }; // 8-connected neighbors
    const int dy[] = { 1, 0, -1, 0, 1, -1, 1, -1 };

    int count = 0;
    for (int i = 0; i < 8; ++i) {
        GridCoords neighbor{ coords.x + dx[i], coords.y + dy[i] };
        if (m_ExploredGrid.count(neighbor) > 0 && m_ExploredGrid.at(neighbor)) {
            count++;
        }
    }
    return count;
}

Elite::Vector2 WorldExplorationGrid::FindUnexploredInHouse(const Elite::Vector2& fromPosition, const HouseInfo& house) const {
    auto houseCells = GetCellsInRect(house.Center, house.Size);
    Elite::Vector2 closestPos = fromPosition;
    //hardcode max
    float minDistSq = 10000000.f;

    for (const auto& cell : houseCells) {
        if (m_ExploredGrid.count(cell) == 0) {
            Elite::Vector2 worldPos = GridToWorld(cell);
            float distSq = Elite::DistanceSquared(fromPosition, worldPos);
            if (distSq < minDistSq) {
                closestPos = worldPos;
                minDistSq = distSq;
            }
        }
    }

    return closestPos;
}