Bee4

#include <vector>
#include <queue>
#include <atomic>
#include <mutex>
#include <condition_variable>
#include <thread>
#include <random>
#include <chrono>
#include <iostream>
#include <iomanip>
#include <cassert>

#ifndef unlikely
#define likely(x)       __builtin_expect((x),1)
#define unlikely(x)     __builtin_expect((x),0)
#endif

#if 0
// C++14 shared_timed_mutex, or pthread_rwlock_t can be used instead
#include <boost/thread/shared_mutex.hpp>
#include <boost/thread/shared_lock_guard.hpp>
using shared_mutex = boost::shared_mutex;
template<class T> using shared_lock_guard = boost::shared_lock_guard<T>;
#else
#include <pthread.h>
class shared_mutex {
    pthread_rwlock_t it;
public:
    shared_mutex(const shared_mutex&) = delete;
    shared_mutex& operator = (const shared_mutex&) = delete;
    shared_mutex() {
        pthread_rwlock_init(&it, nullptr); }
    ~shared_mutex() {
        pthread_rwlock_destroy(&it); }
    void lock() {
        pthread_rwlock_wrlock(&it); }
    void unlock() {
        pthread_rwlock_unlock(&it); }
    void lock_shared() {
        pthread_rwlock_rdlock(&it); }
    void unlock_shared() {
        pthread_rwlock_unlock(&it); }
};
template<class T>
  class shared_lock_guard {
    T& it;
public:
    shared_lock_guard(const shared_lock_guard&) = delete;
    shared_lock_guard& operator = (const shared_lock_guard&) = delete;
    shared_lock_guard(T& it): it(it) {
        it.lock_shared(); }
    ~shared_lock_guard() {
        it.unlock_shared(); }
};
#endif

using namespace std;
using the_clock = chrono::steady_clock;
using stamp = chrono::time_point<the_clock>;
using ms = chrono::milliseconds;

const int P = 15, C = 10;
auto producer_sleep = uniform_int_distribution<int>(10*P,20*P);
auto consume_sleep = uniform_int_distribution<int>(1,3);
auto regular_sleep = uniform_int_distribution<int>(3,10);
const auto regular_timeout = ms(30*C);
atomic<int> produced;
atomic<int> consumed;
atomic<int> regular;
atomic<int> outoforder;
bool done(bool producer = false) {
    int stop = 10000; int p = produced * C;
    return p >= stop && (producer || consumed >= p); }

template<class E>
  class event_queue {
public:
    event_queue(size_t nbees = P, size_t bits = 10) {
    //  minimal capacity: 256, default: 1024
    //  must be power of 2 (we rather use and-mask instead of modulo)
        if(bits <= 8) bits = 8;
        size_t size = 1 << bits;
        mask = size-1; buf = a.allocate(size);
        bees = b.allocate(this->nbees = nbees);
        for(int i = 0; i < nbees; i++)
            b.construct(bees + i, *this);
    }
    void push(const E& e) {
        shared lock(sync);  // quick shared lock
        size_t i = wi++;    // reserve the slot
        if(unlikely(i > lr+mask)) { // check full
            push_full(e); return; } // not implemented yet
        a.construct(buf+(i&mask), nbees, e); // emplace
    //  could possibly be implemented as single notify_all()
    //  with global bitfield (e.g. unsigned long log = ~0)
    //  but that would slow bees instead (locking on single mutex)
    //  for busy bee this just increments counter
    //  for sleeping bee this means lock+notify
        for(int i = 0; i < nbees; i++)
            bees[i].notify();
    }
    bool pop(int id, E& e, stamp until) {
        return bees[id].pop(e, until);
    }
private:
//  global synchronization (fast-locking unless we need to grow or manage bees)
    shared_mutex sync;
    typedef shared_lock_guard<shared_mutex> shared;
    typedef lock_guard<shared_mutex> unique;
//  consumed counting
    struct item {
        atomic<size_t> c;   // consumed countdown
        E e;            // the event/element
        item(int nbees, const E& e)
        : c(nbees), e(e) {}};
//  event buffer
    atomic<size_t> wi;  // write index for producers (reservation)
    atomic<size_t> lr;  // lowest read index (for each bee: bee.ri >= lr)
    size_t mask;        // element at buf[index&mask]
    item *buf;          // the buffer
    int nbees;          // number of bees
    struct handle {
        handle(event_queue<E>& q): n(0), ri(0), q(q),
          mx(), cv(), wakeup(false), was_empty(false) {}
        atomic<int> n;  // number of elements available for consumation
        int ri;         // read index
        event_queue<E>& q; // master queue
        mutex mx; condition_variable cv;
        bool wakeup, was_empty;
        void notify() {
            if(++n == 0) {
                { lock_guard<mutex> lock(mx);
                    wakeup = true; }
                cv.notify_one(); }}
        bool pop(E& e, stamp until) {
            if(the_clock::now() >= until)
                return false;
            if(was_empty || --n < 0) {
                was_empty = true;
                unique_lock<mutex> lock(mx);
                if(!cv.wait_until(lock, until, [this] {
                    return wakeup; }))
                    return false;
                wakeup = false;
                was_empty = false; }
        //  get the item
            shared lock(q.sync);
            item& i = q.buf[ri++ & q.mask];
            e = i.e;
        //  desctroy and advance lowest read index if whe were last
            if(--i.c == 0) {
                ++q.lr;
                q.a.destroy(&i); }
            return true;
        }
    } *bees;
    allocator<item> a;
    allocator<handle> b;
    void push_full(E e) {
    //  not implemented
        assert(false);
    //  unique lock(sync); ...lot of work
        throw -1;
    }
};

// the queue (for ints as example)
event_queue<int> work;

// example bee doing random sleeps
class bee {
    const int id;
    random_device rd;
    int next = 0;
    priority_queue<int,vector<int>,greater<int> > ahead;
public:
    bee(int id): id(id) {}
    bee(const bee& src): id(src.id) {};
    void consume(int e) {
        if(e == next) {
            next++;
            while(!ahead.empty() && ahead.top() == next) {
                next++; ahead.pop(); }
        } else {
            ahead.push(e); outoforder++;
            if(ahead.size() > P*3) {
                for(cout << "bee" << id << " ahead: ";
                    !ahead.empty(); ahead.pop())
                    cout << ahead.top() << " ";
                cout << endl;
                assert(("too many skipped elements", false)); }}
        this_thread::sleep_for(ms(consume_sleep(rd)));
        consumed++; }
    void do_regular() {
        this_thread::sleep_for(ms(regular_sleep(rd)));
        regular++; }
    void operator()() {
        stamp next = the_clock::now() + regular_timeout;
        while(!done()) {
            int e;
            if(work.pop(id, e, next))
                consume(e);
            else {
                do_regular();
                next += regular_timeout;
            }
        }
    }
};

// example producer
class producer {
    const int id;
    random_device rd;
public:
    producer(int id): id(id) {}
    producer(const producer& src): id(src.id) {};
    void operator()() {
        while(!done(true)) { work.push(produced++);
            this_thread::sleep_for(ms(producer_sleep(rd)));
}}};

int main() {
    vector<bee> bees;
    while(bees.size() < C) bees.emplace_back(bees.size());
    vector<producer> pros;
    while(pros.size() < P) pros.emplace_back(pros.size());
    vector<thread> all;
    for(auto&& b : bees) all.push_back(thread([&b] {
        try { b(); } catch(...) { assert(("crash", false)); } }));
    for(auto&& p : pros) all.push_back(thread([&p] {
        try { p(); } catch(...) { assert(("crash", false)); } }));
    while(!done()) {
        this_thread::sleep_for(ms(1000));
        cout << setw(5) << (produced.load() * C)
            << "; " << setw(5) << consumed.load()
            << "; " << setw(5) << regular.load() << endl;
    }
    for(auto&& t : all) t.join();
    int pro = produced, con = consumed, reg = regular;
    cout << "produced:  " << setw(5) << (pro*C)
        << " (" << setw(5) << pro << ")" << endl;
    cout << "consumed:  " << setw(5) << con
        << " (" << setw(5) << (con/C) << ")" << endl;
    cout << "regular:   " << setw(5) << reg << endl;
    cout << "out-of-order: " << outoforder << endl;
}