#include "GameObjectPool.hpp"
#include <bit>
#include <cstring>

namespace ougge::game {

GameObjectPool::GameObjectPool(u32 size)
{
    useful_assert(size % 64 == 0, "size of GameObjectPool must be a multiple of 64");
    this->size = size; 
    first_unused_index = 0;
    buffer = new GameObject[size];
    used_indices = new u64[size/64];
    // std::memset(buffer, 0, size*sizeof(GameObject));
    std::memset(used_indices, 0, size/8);
}

GameObjectPool::~GameObjectPool()
{
    delete[] buffer;
    delete[] used_indices;
}

bool GameObjectPool::isIndexUsed(u32 index)
{
    return ( used_indices[index/64] & (u64(1)<<(index%64)) ) != 0;
}

u32 GameObjectPool::getNearestUnusedIndex(u32 startIndex)
{
    if(startIndex >= size)
        return -1;
    if(!isIndexUsed(startIndex))
        return startIndex;
    
    u32 i = startIndex/64;
    // mark previous bits as used
    u64 u = used_indices[i] | ( (u64(1)<<startIndex%64) -1 );
    while(u == u64(-1)){
        i++;
        if(i == size/64)
            return -1;
        u = used_indices[i];
    }
    u32 bitpos = std::countr_one(u);
    if(bitpos == 64)
        return -1;
    u32 index = i*64 + bitpos;
    return index;
}

u32 GameObjectPool::getNearestUsedIndex(u32 startIndex)
{
    if(startIndex >= size)
        return -1;
    if(isIndexUsed(startIndex))
        return startIndex;
    
    u32 i = startIndex/64;
    // mark previous bits as unused
    u64 u = used_indices[i] & !( (u64(1)<<startIndex%64) -1 );
    while(u == 0){
        i++;
        if(i == size/64)
            return -1;
        u = used_indices[i];
    }
    u32 bitpos = std::countr_zero(u);
    if(bitpos == 64)
        return -1;
    u32 index = i*64 + bitpos;
    return index;
}

GameObject& GameObjectPool::get(u32 index)
{
    if(index >= size)
        throw UsefulException(ougge_format("index %i is out of size %i", index, size));
    if(!isIndexUsed(index))
        throw UsefulException(ougge_format("there is no object at index %i", index));
    return buffer[index];
}

std::pair<u32, GameObject&> GameObjectPool::emplace(GameObject&& new_obj)
{
    u32 i = first_unused_index;
    if(i == u32(-1))
        throw UsefulException("can't put new GameObject to GameObjectPool because it's full");
    
    buffer[i] = std::move(new_obj);
    GameObject& r = buffer[i];
    used_indices[i/64] |= u64(1)<<(i%64); // mark index bit as used
    first_unused_index = getNearestUnusedIndex(i+1);
    return std::pair<u32, GameObject&>(i, r);
}

void GameObjectPool::erase(u32 index)
{
    if(index >= size)
        throw UsefulException(ougge_format("index %i is out of size %i", index, size));
    if(!isIndexUsed(index))
        throw UsefulException(ougge_format("there is no object at index %i", index));
    
    buffer[index] = GameObject();
    used_indices[index/64] &= ~(u64(1)<<(index%64)); // mark index bit as unused
    if(index < first_unused_index)
        first_unused_index = index;
}

GameObjectPool::iterator::iterator(GameObjectPool* pool, u32 index)
    : pool(pool), index(index)
{
}

std::pair<u32, GameObject&> GameObjectPool::iterator::operator*()
{
    if(index >= pool->size)
        throw UsefulException("can't get value of end() iterator");
    
    GameObject& r = pool->buffer[index];
    return  std::pair<u32, GameObject&>(index, r);
}

GameObjectPool::iterator& GameObjectPool::iterator::operator++()
{
    index = pool->getNearestUsedIndex(index+1);
    return *this;
}

}