#pragma once

#include <algorithm>
#include <cassert>
#include <cstddef>
#include <cstring>
#include <exception>
#include <thread>
#include <unordered_map>
#include <vector>

template<class T>
concept resetable = requires(T val) {
    val.reset();
};

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

    ~allocPool() {
        for (auto i: vec)
            delete i;
    }

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

    void returnPtr(T *ptr) {
        size_t pos = positionMap[ptr];
        (vec[pos])->reset();
        std::swap(vec[pos], vec[pivot]);
        positionMap[vec[pos]] = pos;
        positionMap[vec[pivot]] = pivot;
        pivot++;
    }

private:
    std::vector<T *> vec;
    std::mutex positionMapMutex;
    std::unordered_map<T *, size_t> positionMap;
    size_t pivot;

    void 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, amountPerThreads);

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

        // Remainder
        initObjects(amount - (amount % amountOfThreads), amount % amountOfThreads);
    }

    void 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;
        }
        std::lock_guard<std::mutex> guard(positionMapMutex);
        for (size_t i{}; i < amount; i++) {
            positionMap[vec[startIdx + i]] = i;
        }
    }

    void resizeVec() {
        size_t size{vec.size()};
        vec.resize(2 * size);
        pivot = size;
        memcpy(&(vec[size]), &(vec[0]), sizeof(vec[0]) * size);

        for (size_t i{}; i < size; i++)
            positionMap[vec[size + i]] = size + i;

        initArray(size);
    }
};