#include "allocPool.hpp"

template<class T>
    requires std::default_initializable<T> && resetable<T>
allocPool<T>::allocPool(size_t defaultAllocNumbers)
    : vec(defaultAllocNumbers), pivot{defaultAllocNumbers} {
    memset(&(vec[0]), 0, sizeof(vec[0]) * vec.size());
    initArray(defaultAllocNumbers);
}

template<class T>
    requires std::default_initializable<T> && resetable<T>
T *allocPool<T>::getPtr() {
    if (pivot == 0)
        resizeVec();

    auto *ptrToReturn{vec[0]};
    std::swap(vec[0], vec[pivot - 1]);
    positionMap[vec[0]] = 0;
    positionMap[vec[pivot - 1]] = pivot - 1;
    pivot--;
    return ptrToReturn;
}

template<class T>
    requires std::default_initializable<T> && resetable<T>
void allocPool<T>::returnPtr(T *ptr) {
    size_t pos = positionMap[ptr];
    vec[pos].reset();
    std::swap(vec[pos], vec[pivot]);
    pivot++;
}

template<class T>
    requires std::default_initializable<T> && resetable<T>
void allocPool<T>::initArray(size_t amount) {
    const auto amountOfThreads{std::thread::hardware_concurrency()};
    assert(amountOfThreads);
    const auto amountPerThreads{amount / amountOfThreads};

    std::vector<std::thread> threads;
    threads.reserve(amountOfThreads);

    for (size_t i{}; i < amountOfThreads; i++)
        threads.emplace_back(&allocPool::initObjects, this, i, amountPerThreads);

    for (auto &t: threads)
        t.join();

    // Remainder
    initObjects(vec[vec.size() - (amount % amountOfThreads)], amount % amountOfThreads);
}

template<class T>
    requires std::default_initializable<T> && resetable<T>
void allocPool<T>::initObjects(size_t startIdx, size_t amount) {
    for (size_t i{}; i < amount; i++) {
        // TODO: Be more cache friendly by making a vector per thread, then doing memcpy into the original vector.
        vec[startIdx + i] = new T;
        positionMap[startIdx + i] = i;
    }
}

template<class T>
    requires std::default_initializable<T> && resetable<T>
void allocPool<T>::resizeVec() {
    size_t size{vec.size()};
    vec.resize(2 * size);
    memcpy(&(vec[size]), &(vec[0]), size);
    initArray(size);
}