#pragma once

#ifndef NO_STATS
#include <iostream>
#endif

#include <memory>
#include <mutex>
#include <type_traits>

#include "assert.hpp"
#include "utils.hpp"

using namespace std;

struct spinlock_t {
	std::atomic_bool ll = { false };

	inline void lock() {
		while( __builtin_expect(ll.exchange(true),false) ) {
			while(ll.load(std::memory_order_relaxed))
				asm volatile("pause");
		}
	}

	inline bool try_lock() {
		return false == ll.exchange(true);
	}

	inline void unlock() {
		ll.store(false, std::memory_order_release);
	}

	inline explicit operator bool() {
		return ll.load(std::memory_order_relaxed);
	}
};

static inline bool bts(std::atomic_size_t & target, size_t bit ) {
	//*
	int result = 0;
	asm volatile(
		"LOCK btsq %[bit], %[target]\n\t"
		:"=@ccc" (result)
		: [target] "m" (target), [bit] "r" (bit)
	);
 	return result != 0;
	/*/
	size_t mask = 1ul << bit;
	size_t ret = target.fetch_or(mask, std::memory_order_relaxed);
	return (ret & mask) != 0;
	//*/
}

static inline bool btr(std::atomic_size_t & target, size_t bit ) {
	//*
	int result = 0;
	asm volatile(
		"LOCK btrq %[bit], %[target]\n\t"
		:"=@ccc" (result)
		: [target] "m" (target), [bit] "r" (bit)
	);
 	return result != 0;
	/*/
	size_t mask = 1ul << bit;
	size_t ret = target.fetch_and(~mask, std::memory_order_relaxed);
	return (ret & mask) != 0;
	//*/
}

extern bool enable_stats;

struct pick_stat {
	struct {
		size_t attempt = 0;
		size_t success = 0;
	} push;
	struct {
		size_t attempt = 0;
		size_t success = 0;
		size_t mask_attempt = 0;
	} pop;
};

struct empty_stat {
	struct {
		size_t value = 0;
		size_t count = 0;
	} push;
	struct {
		size_t value = 0;
		size_t count = 0;
	} pop;
};

template<typename node_t>
struct _LinksFields_t {
	node_t * prev = nullptr;
	node_t * next = nullptr;
	unsigned long long ts = 0;
};

template<typename node_t>
class __attribute__((aligned(128))) relaxed_list {
	static_assert(std::is_same<decltype(node_t::_links), _LinksFields_t<node_t>>::value, "Node must have a links field");


public:
	relaxed_list(unsigned numLists)
	  	: lists(new intrusive_queue_t[numLists])
		, numLists(numLists)
	{
		assertf(7 * 8 * 8 >= numLists, "List currently only supports 448 sublists");
		assert(sizeof(*this) == 128);
	}

	~relaxed_list() {
		lists.reset();
		#ifndef NO_STATS
			std::cout << "Difference   : "
				<< ssize_t(double(intrusive_queue_t::stat::dif.value) / intrusive_queue_t::stat::dif.num  ) << " avg\t"
				<< intrusive_queue_t::stat::dif.max << "max" << std::endl;
		#endif
	}

    	__attribute__((noinline, hot)) void push(node_t * node) {
		node->_links.ts = rdtscl();

		while(true) {
			// Pick a random list
			unsigned i = tls.rng.next() % numLists;

			#ifndef NO_STATS
				tls.pick.push.attempt++;
			#endif

			// If we can't lock it retry
			if( !lists[i].lock.try_lock() ) continue;

			__attribute__((unused)) int num = numNonEmpty;

			// Actually push it
			if(lists[i].push(node)) {
				numNonEmpty++;
				size_t qword = i >> 6ull;
				size_t bit   = i & 63ull;
				assertf((list_mask[qword] & (1ul << bit)) == 0, "Before set %zu:%zu (%u), %zx & %zx", qword, bit, i, list_mask[qword].load(), (1ul << bit));
				__attribute__((unused)) bool ret = bts(list_mask[qword], bit);
				assert(!ret);
				assertf((list_mask[qword] & (1ul << bit)) != 0, "After set %zu:%zu (%u), %zx & %zx", qword, bit, i, list_mask[qword].load(), (1ul << bit));
			}
			assert(numNonEmpty <= (int)numLists);

			// Unlock and return
			lists[i].lock.unlock();

			#ifndef NO_STATS
				tls.pick.push.success++;
				tls.empty.push.value += num;
				tls.empty.push.count += 1;
			#endif
			return;
		}
    	}

	__attribute__((noinline, hot)) node_t * pop() {
		#if !defined(NO_BITMASK)
			// for(int r = 0; r < 10 && numNonEmpty != 0; r++) {
			// 	// Pick two lists at random
			// 	unsigned i = tls.rng.next() % numLists;
			// 	unsigned j = tls.rng.next() % numLists;

			// 	if(auto node = try_pop(i, j)) return node;
			// }
			int nnempty;
			while(0 != (nnempty = numNonEmpty)) {
				unsigned i, j;
				if( numLists < 4 || (numLists / nnempty) < 4 ) {
					// Pick two lists at random
					i = tls.rng.next() % numLists;
					j = tls.rng.next() % numLists;
				} else {
					#ifndef NO_STATS
						// tls.pick.push.mask_attempt++;
					#endif

					// Pick two lists at random
					unsigned num = ((numLists - 1) >> 6) + 1;

					unsigned ri = tls.rng.next();
					unsigned rj = tls.rng.next();

					unsigned wdxi = (ri >> 6u) % num;
					unsigned wdxj = (rj >> 6u) % num;

					size_t maski = list_mask[wdxi].load(std::memory_order_relaxed);
					size_t maskj = list_mask[wdxj].load(std::memory_order_relaxed);

					if(maski == 0 && maskj == 0) continue;

					unsigned biti = maski ? ri % __builtin_popcountl(maski) : 0;
					unsigned bitj = maskj ? rj % __builtin_popcountl(maskj) : 0;

					unsigned bi = 64 - nthSetBit(maski, biti + 1);
					unsigned bj = 64 - nthSetBit(maskj, bitj + 1);

					assertf(bi < 64, "%zu %u %u", maski, biti, bi);
					assertf(bj < 64, "%zu %u %u", maskj, bitj, bj);

					i = bi | (wdxi << 6);
					j = bj | (wdxj << 6);

					assertf(i < numLists, "%u", wdxi << 6);
					assertf(j < numLists, "%u", wdxj << 6);
				}

				if(auto node = try_pop(i, j)) return node;
			}
		#else
			while(numNonEmpty != 0) {
				// Pick two lists at random
				int i = tls.rng.next() % numLists;
				int j = tls.rng.next() % numLists;

				if(auto node = try_pop(i, j)) return node;
			}
		#endif

		return nullptr;
    	}

private:
	node_t * try_pop(unsigned i, unsigned j) {
		#ifndef NO_STATS
			tls.pick.pop.attempt++;
		#endif

		// Pick the bet list
		int w = i;
		if( __builtin_expect(lists[j].ts() != 0, true) ) {
			w = (lists[i].ts() < lists[j].ts()) ? i : j;
		}

		auto & list = lists[w];
		// If list looks empty retry
		if( list.ts() == 0 ) return nullptr;

		// If we can't get the lock retry
		if( !list.lock.try_lock() ) return nullptr;

		__attribute__((unused)) int num = numNonEmpty;

		// If list is empty, unlock and retry
		if( list.ts() == 0 ) {
			list.lock.unlock();
			return nullptr;
		}

		// Actually pop the list
		node_t * node;
		bool emptied;
		std::tie(node, emptied) = list.pop();
		assert(node);

		if(emptied) {
			numNonEmpty--;
			size_t qword = w >> 6ull;
			size_t bit   = w & 63ull;
			assert((list_mask[qword] & (1ul << bit)) != 0);
			__attribute__((unused)) bool ret = btr(list_mask[qword], bit);
			assert(ret);
			assert((list_mask[qword] & (1ul << bit)) == 0);
		}

		// Unlock and return
		list.lock.unlock();
		assert(numNonEmpty >= 0);
		#ifndef NO_STATS
			tls.pick.pop.success++;
			tls.empty.pop.value += num;
			tls.empty.pop.count += 1;
		#endif
		return node;
	}

private:

	class __attribute__((aligned(128))) intrusive_queue_t {
	public:
		typedef spinlock_t lock_t;

		friend class relaxed_list<node_t>;

		struct stat {
			ssize_t diff = 0;

			static struct Dif {
				ssize_t value = 0;
				size_t  num   = 0;
				ssize_t max   = 0;
			} dif;
		};

	private:
		struct sentinel_t {
			_LinksFields_t<node_t> _links;
		};

		lock_t lock;
		sentinel_t before;
		sentinel_t after;
		#ifndef NO_STATS
			stat s;
		#endif

		static constexpr auto fields_offset = offsetof( node_t, _links );
	public:
		intrusive_queue_t()
			: before{{ nullptr, tail() }}
			, after {{ head(), nullptr }}
		{
			/* paranoid */ assert((reinterpret_cast<uintptr_t>( head() ) + fields_offset) == reinterpret_cast<uintptr_t>(&before));
			/* paranoid */ assert((reinterpret_cast<uintptr_t>( tail() ) + fields_offset) == reinterpret_cast<uintptr_t>(&after ));
			/* paranoid */ assert(head()->_links.prev == nullptr);
			/* paranoid */ assert(head()->_links.next == tail() );
			/* paranoid */ assert(tail()->_links.next == nullptr);
			/* paranoid */ assert(tail()->_links.prev == head() );
			/* paranoid */ assert(sizeof(*this) == 128);
			/* paranoid */ assert((intptr_t(this) % 128) == 0);
		}

		~intrusive_queue_t() {
			#ifndef NO_STATS
				stat::dif.value+= s.diff;
				stat::dif.num  ++;
				stat::dif.max  = std::abs(stat::dif.max) > std::abs(s.diff) ? stat::dif.max : s.diff;
			#endif
		}

		inline node_t * head() const {
			node_t * rhead = reinterpret_cast<node_t *>(
				reinterpret_cast<uintptr_t>( &before ) - fields_offset
			);
			assert(rhead);
			return rhead;
		}

		inline node_t * tail() const {
			node_t * rtail = reinterpret_cast<node_t *>(
				reinterpret_cast<uintptr_t>( &after ) - fields_offset
			);
			assert(rtail);
			return rtail;
		}

		inline bool push(node_t * node) {
			assert(lock);
			assert(node->_links.ts != 0);
			node_t * tail = this->tail();

			node_t * prev = tail->_links.prev;
			// assertf(node->_links.ts >= prev->_links.ts,
			// 	"New node has smaller timestamp: %llu < %llu", node->_links.ts, prev->_links.ts);
			node->_links.next = tail;
			node->_links.prev = prev;
			prev->_links.next = node;
			tail->_links.prev = node;
			#ifndef NO_STATS
				if(enable_stats) s.diff++;
			#endif
			if(before._links.ts == 0l) {
				before._links.ts = node->_links.ts;
				assert(node->_links.prev == this->head());
				return true;
			}
			return false;
		}

		inline std::pair<node_t *, bool> pop() {
			assert(lock);
			node_t * head = this->head();
			node_t * tail = this->tail();

			node_t * node = head->_links.next;
			node_t * next = node->_links.next;
			if(node == tail) return {nullptr, false};

			head->_links.next = next;
			next->_links.prev = head;

			#ifndef NO_STATS
				if(enable_stats) s.diff--;
			#endif
			if(next == tail) {
				before._links.ts = 0l;
				return {node, true};
			}
			else {
				assert(next->_links.ts != 0);
				before._links.ts = next->_links.ts;
				assert(before._links.ts != 0);
				return {node, false};
			}
		}

		long long ts() const {
			return before._links.ts;
		}
	};


public:

	static __attribute__((aligned(128))) thread_local struct TLS {
		Random     rng = { int(rdtscl()) };
		pick_stat  pick;
		empty_stat empty;
	} tls;

public:
	std::atomic_int numNonEmpty  = 0;  // number of non-empty lists
	std::atomic_size_t list_mask[7] = { 0 }; // which queues are empty
private:
    	__attribute__((aligned(64))) std::unique_ptr<intrusive_queue_t []> lists;
	const unsigned numLists;

public:
	static const constexpr size_t sizeof_queue = sizeof(intrusive_queue_t);
};