#include "turnstile.h"
#include <vector>
#include <cassert>
#include <iostream>
#include <queue>

namespace {
    Turnstile* const dummy = new Turnstile();

class TurnstilePool {
private:
    std::mutex m;
    std::vector<Turnstile*> pool;
    int size;

    void extendPool() {
        size *= 2;
        for (int i = 1; i <= size; ++i)
            pool.push_back(new Turnstile());
    }

public:
    TurnstilePool() :
            size(2) {
        for (int i = 1; i <= size; ++i)
            pool.push_back(new Turnstile());
    }

    ~TurnstilePool() {
        while (!pool.empty()) {
            Turnstile *remove = pool.back();
            pool.pop_back();
            delete remove;
        }
        delete dummy;
    }

    Turnstile *getTurnstile() {
        std::lock_guard<std::mutex> lk(m);

        if (pool.empty())
            extendPool();

        Turnstile *ret = pool.back();
        pool.pop_back();

        return ret;
    }

    void returnToPool(Turnstile *turnstile) {
        std::lock_guard<std::mutex> lk(m);

        pool.push_back(turnstile);
        if (size > 2 && (int)pool.size() * 2 >= size * 3) {
            for (int i = 1; i <= size / 2; ++i) {
                Turnstile *remove = pool.back();
                pool.pop_back();
                delete remove;
            }

            size /= 2;
        }
    }
};

TurnstilePool& pool() {
    static TurnstilePool pool;
    return pool;
}

inline std::mutex& getInternalMutex(Mutex *ptr) {
    static const int MUTEX_NO = 307;
    static std::mutex internalMutexes[MUTEX_NO];

    return internalMutexes[((long long)((long long)ptr % MUTEX_NO) * 97LL) % MUTEX_NO];
}

}

Turnstile::Turnstile() :
            waitingNo(0), secure(false) {
        m = new std::mutex;
}

Turnstile::~Turnstile() {
    delete m;
}

Mutex::Mutex() {
    ptr = nullptr;
}

void Mutex::lock() {
    getInternalMutex(this).lock();

    if (ptr == nullptr)
        ptr = dummy;
    else {
        if (ptr == dummy)
            ptr = pool().getTurnstile();

        ptr->waitingNo++;
        getInternalMutex(this).unlock();

        std::unique_lock<std::mutex> lk(*(ptr->m));
        ptr->cv.wait(lk, [this] { return ptr->secure; });

        getInternalMutex(this).lock();
        ptr->secure = false;
        ptr->waitingNo--;

        if (ptr->waitingNo == 0) {
            pool().returnToPool(ptr);
            ptr = dummy;
        }
    }

    getInternalMutex(this).unlock();
}

void Mutex::unlock() {
    getInternalMutex(this).lock();

    if (ptr == dummy)
        ptr = nullptr;
    else {
        assert(ptr->waitingNo);
        std::lock_guard<std::mutex> lk(*(ptr->m));
        ptr->secure = true;
        ptr->cv.notify_one();
    }

    getInternalMutex(this).unlock();
}