#include "navigation/terrain_grid.hpp"

namespace navigation {

TerrainGrid::TerrainGrid(float cellSize)
    : m_cellSize((cellSize > 0.5f) ? cellSize : kDefaultCellSize)
    , m_cells(kGridDim * kGridDim, CELL_WALKABLE) {
}

void TerrainGrid::Initialize(float centerX, float centerY) {
    m_originX = centerX - (static_cast<float>(kGridDim) / 2.0f) * m_cellSize;
    m_originY = centerY - (static_cast<float>(kGridDim) / 2.0f) * m_cellSize;
    std::fill(m_cells.begin(), m_cells.end(), CELL_WALKABLE);
    m_blockedCount = 0;
    m_initialized = true;
}

void TerrainGrid::Reset() {
    std::fill(m_cells.begin(), m_cells.end(), CELL_WALKABLE);
    m_blockedCount = 0;
    m_initialized = false;
    m_hasNativeTerrain = false;
    m_nativeTerrain = {};
}

bool TerrainGrid::LoadFromNativeTerrain(const memory::NativeTerrainData& nativeTerrain, float playerWorldX, float playerWorldY) {
    if (!nativeTerrain.IsValid()) return false;

    m_nativeTerrain = nativeTerrain;
    m_hasNativeTerrain = true;

    // Khởi tạo cửa sổ lưới cục bộ bao quanh player
    Initialize(playerWorldX, playerWorldY);

    // Điền dữ liệu tĩnh từ nativeTerrain vào lưới cục bộ
    for (int32_t gy = 0; gy < kGridDim; ++gy) {
        for (int32_t gx = 0; gx < kGridDim; ++gx) {
            float wx, wy;
            GridToWorld(gx, gy, wx, wy);
            if (!m_nativeTerrain.IsWalkableWorld(wx, wy)) {
                m_cells[Index(gx, gy)] = CELL_BLOCKED;
                ++m_blockedCount;
            }
        }
    }

    return true;
}

void TerrainGrid::RecenterIfNeeded(float playerX, float playerY) {
    if (!m_initialized) {
        Initialize(playerX, playerY);
        return;
    }

    const float centerX = m_originX + (static_cast<float>(kGridDim) / 2.0f) * m_cellSize;
    const float centerY = m_originY + (static_cast<float>(kGridDim) / 2.0f) * m_cellSize;
    const float distSq = (playerX - centerX) * (playerX - centerX) + (playerY - centerY) * (playerY - centerY);
    const float threshold = (static_cast<float>(kGridDim) / 3.0f) * m_cellSize;

    if (distSq > threshold * threshold) {
        // Tạo grid mới ở tâm mới
        const float newOriginX = playerX - (static_cast<float>(kGridDim) / 2.0f) * m_cellSize;
        const float newOriginY = playerY - (static_cast<float>(kGridDim) / 2.0f) * m_cellSize;
        std::vector<uint8_t> newCells(kGridDim * kGridDim, CELL_WALKABLE);
        uint32_t newBlocked = 0;

        // INV-NAV-TERRAIN-COMMERCIAL: Nếu có NativeTerrainData, nạp lại mẫu cho toàn bộ lưới mới từ RAM game
        if (m_hasNativeTerrain && m_nativeTerrain.IsValid()) {
            for (int32_t ngy = 0; ngy < kGridDim; ++ngy) {
                for (int32_t ngx = 0; ngx < kGridDim; ++ngx) {
                    float nwx = newOriginX + (static_cast<float>(ngx) + 0.5f) * m_cellSize;
                    float nwy = newOriginY + (static_cast<float>(ngy) + 0.5f) * m_cellSize;
                    if (!m_nativeTerrain.IsWalkableWorld(nwx, nwy)) {
                        newCells[static_cast<size_t>(ngy) * kGridDim + static_cast<size_t>(ngx)] = CELL_BLOCKED;
                        ++newBlocked;
                    }
                }
            }
        }

        // Kế thừa các ô vật cản động / nguy hiểm từ lưới cũ trong vùng giao nhau
        for (int32_t gy = 0; gy < kGridDim; ++gy) {
            for (int32_t gx = 0; gx < kGridDim; ++gx) {
                uint8_t val = m_cells[Index(gx, gy)];
                if (val != CELL_WALKABLE) {
                    float wx, wy;
                    GridToWorld(gx, gy, wx, wy);
                    int32_t ngx = static_cast<int32_t>(std::floor((wx - newOriginX) / m_cellSize));
                    int32_t ngy = static_cast<int32_t>(std::floor((wy - newOriginY) / m_cellSize));
                    if (ngx >= 0 && ngx < kGridDim && ngy >= 0 && ngy < kGridDim) {
                        size_t nIdx = static_cast<size_t>(ngy) * kGridDim + static_cast<size_t>(ngx);
                        if (newCells[nIdx] == CELL_WALKABLE && val == CELL_BLOCKED) {
                            ++newBlocked;
                        }
                        newCells[nIdx] = val;
                    }
                }
            }
        }

        m_originX = newOriginX;
        m_originY = newOriginY;
        m_cells = std::move(newCells);
        m_blockedCount = newBlocked;
    }
}

bool TerrainGrid::WorldToGrid(float worldX, float worldY, int32_t& outGx, int32_t& outGy) const {
    if (!m_initialized) return false;
    int32_t gx = static_cast<int32_t>(std::floor((worldX - m_originX) / m_cellSize));
    int32_t gy = static_cast<int32_t>(std::floor((worldY - m_originY) / m_cellSize));
    if (gx < 0 || gx >= kGridDim || gy < 0 || gy >= kGridDim) {
        return false;
    }
    outGx = gx;
    outGy = gy;
    return true;
}

void TerrainGrid::GridToWorld(int32_t gx, int32_t gy, float& outWx, float& outWy) const {
    outWx = m_originX + (static_cast<float>(gx) + 0.5f) * m_cellSize;
    outWy = m_originY + (static_cast<float>(gy) + 0.5f) * m_cellSize;
}

bool TerrainGrid::IsWalkable(float worldX, float worldY) const {
    if (m_hasNativeTerrain) {
        if (!m_nativeTerrain.IsWalkableWorld(worldX, worldY)) {
            return false;
        }
    }

    int32_t gx, gy;
    if (!WorldToGrid(worldX, worldY, gx, gy)) {
        if (m_hasNativeTerrain) {
            return m_nativeTerrain.IsWalkableWorld(worldX, worldY);
        }
        return false; // Ngoài ranh giới lưới
    }
    return m_cells[Index(gx, gy)] == CELL_WALKABLE;
}

bool TerrainGrid::IsWalkableGrid(int32_t gx, int32_t gy) const {
    if (gx < 0 || gx >= kGridDim || gy < 0 || gy >= kGridDim) return false;
    return m_cells[Index(gx, gy)] == CELL_WALKABLE;
}

void TerrainGrid::SetBlocked(float worldX, float worldY, float radius) {
    if (!m_initialized) {
        Initialize(worldX, worldY);
    }
    int32_t centerGx, centerGy;
    if (!WorldToGrid(worldX, worldY, centerGx, centerGy)) return;

    int32_t radCells = static_cast<int32_t>(std::ceil(radius / m_cellSize));
    float rSq = radius * radius;

    for (int32_t dy = -radCells; dy <= radCells; ++dy) {
        int32_t gy = centerGy + dy;
        if (gy < 0 || gy >= kGridDim) continue;
        for (int32_t dx = -radCells; dx <= radCells; ++dx) {
            int32_t gx = centerGx + dx;
            if (gx < 0 || gx >= kGridDim) continue;

            float wx, wy;
            GridToWorld(gx, gy, wx, wy);
            float distSq = (wx - worldX) * (wx - worldX) + (wy - worldY) * (wy - worldY);
            if (distSq <= rSq) {
                size_t idx = Index(gx, gy);
                if (m_cells[idx] != CELL_BLOCKED) {
                    m_cells[idx] = CELL_BLOCKED;
                    ++m_blockedCount;
                }
            }
        }
    }
}

void TerrainGrid::SetHazard(float worldX, float worldY, float radius) {
    if (!m_initialized) Initialize(worldX, worldY);
    int32_t centerGx, centerGy;
    if (!WorldToGrid(worldX, worldY, centerGx, centerGy)) return;

    int32_t radCells = static_cast<int32_t>(std::ceil(radius / m_cellSize));
    float rSq = radius * radius;

    for (int32_t dy = -radCells; dy <= radCells; ++dy) {
        int32_t gy = centerGy + dy;
        if (gy < 0 || gy >= kGridDim) continue;
        for (int32_t dx = -radCells; dx <= radCells; ++dx) {
            int32_t gx = centerGx + dx;
            if (gx < 0 || gx >= kGridDim) continue;

            float wx, wy;
            GridToWorld(gx, gy, wx, wy);
            float distSq = (wx - worldX) * (wx - worldX) + (wy - worldY) * (wy - worldY);
            if (distSq <= rSq) {
                size_t idx = Index(gx, gy);
                if (m_cells[idx] == CELL_WALKABLE) {
                    m_cells[idx] = CELL_HAZARD;
                }
            }
        }
    }
}

void TerrainGrid::SetWalkable(float worldX, float worldY, float radius) {
    if (!m_initialized) return;
    int32_t centerGx, centerGy;
    if (!WorldToGrid(worldX, worldY, centerGx, centerGy)) return;

    int32_t radCells = static_cast<int32_t>(std::ceil(radius / m_cellSize));
    float rSq = radius * radius;

    for (int32_t dy = -radCells; dy <= radCells; ++dy) {
        int32_t gy = centerGy + dy;
        if (gy < 0 || gy >= kGridDim) continue;
        for (int32_t dx = -radCells; dx <= radCells; ++dx) {
            int32_t gx = centerGx + dx;
            if (gx < 0 || gx >= kGridDim) continue;

            float wx, wy;
            GridToWorld(gx, gy, wx, wy);
            float distSq = (wx - worldX) * (wx - worldX) + (wy - worldY) * (wy - worldY);
            if (distSq <= rSq) {
                size_t idx = Index(gx, gy);
                if (m_cells[idx] == CELL_BLOCKED) {
                    if (m_blockedCount > 0) --m_blockedCount;
                }
                m_cells[idx] = CELL_WALKABLE;
            }
        }
    }
}

bool TerrainGrid::HasLineOfSight(float x1, float y1, float x2, float y2) const {
    if (!m_initialized) return true; // Chưa có bản đồ -> mặc định coi như thoáng

    int32_t gx0, gy0, gx1, gy1;
    if (!WorldToGrid(x1, y1, gx0, gy0) || !WorldToGrid(x2, y2, gx1, gy1)) {
        return false;
    }

    // Thuật toán đường kẻ Bresenham trên lưới ma trận
    int32_t dx = std::abs(gx1 - gx0);
    int32_t dy = -std::abs(gy1 - gy0);
    int32_t sx = (gx0 < gx1) ? 1 : -1;
    int32_t sy = (gy0 < gy1) ? 1 : -1;
    int32_t err = dx + dy;
    int32_t curX = gx0;
    int32_t curY = gy0;

    while (true) {
        if (curX >= 0 && curX < kGridDim && curY >= 0 && curY < kGridDim) {
            // Không tính chính ô xuất phát (để tránh trường hợp nhân vật đứng sát vách)
            if (!(curX == gx0 && curY == gy0) && !(curX == gx1 && curY == gy1)) {
                if (m_cells[Index(curX, curY)] == CELL_BLOCKED) {
                    return false; // Bị tường/vật cản chắn ngang tầm nhìn!
                }
            }
        }
        if (curX == gx1 && curY == gy1) break;
        int32_t e2 = 2 * err;
        // Kiểm tra chống cắt góc tường khi bước chéo (Corner-Cutting Prevention)
        if (e2 >= dy && e2 <= dx) {
            int32_t adjX = curX + sx;
            int32_t adjY = curY + sy;
            if (adjX >= 0 && adjX < kGridDim && adjY >= 0 && adjY < kGridDim) {
                if (m_cells[Index(adjX, curY)] == CELL_BLOCKED && m_cells[Index(curX, adjY)] == CELL_BLOCKED) {
                    return false; // Hai ô vật cản tiếp xúc chéo chặn hoàn toàn đường nhìn
                }
            }
        }
        if (e2 >= dy) { err += dy; curX += sx; }
        if (e2 <= dx) { err += dx; curY += sy; }
    }

    return true;
}

bool TerrainGrid::FindNearestWalkable(float worldX, float worldY, float& outX, float& outY, float maxSearchRadius) const {
    if (!m_initialized) {
        outX = worldX;
        outY = worldY;
        return true;
    }

    int32_t startGx, startGy;
    if (!WorldToGrid(worldX, worldY, startGx, startGy)) {
        outX = worldX;
        outY = worldY;
        return false;
    }

    if (m_cells[Index(startGx, startGy)] == CELL_WALKABLE) {
        outX = worldX;
        outY = worldY;
        return true;
    }

    int32_t maxRadiusCells = static_cast<int32_t>(std::ceil(maxSearchRadius / m_cellSize));
    for (int32_t r = 1; r <= maxRadiusCells; ++r) {
        for (int32_t dy = -r; dy <= r; ++dy) {
            for (int32_t dx = -r; dx <= r; ++dx) {
                if (std::abs(dx) != r && std::abs(dy) != r) continue; // Chỉ quét viền ngoài hình vuông
                int32_t gx = startGx + dx;
                int32_t gy = startGy + dy;
                if (gx >= 0 && gx < kGridDim && gy >= 0 && gy < kGridDim) {
                    if (m_cells[Index(gx, gy)] == CELL_WALKABLE) {
                        GridToWorld(gx, gy, outX, outY);
                        return true;
                    }
                }
            }
        }
    }

    outX = worldX;
    outY = worldY;
    return false;
}

} // namespace navigation