TieredVector.hpp

#pragma once

#include "../common/Misc.h"

#include <vector>
#include <shared_mutex>

/// Double ended queue with fixed size, does not support resize when full
/// The sequence of elements supports O(1) indexing and O(n) insert at index
/// Support O(1) insert and remove at the start and end of the sequence
template <typename T>
struct FixedDeque {
private:
	std::vector<T> data; ///< The container for the data, contains size + 1 slots to avoid edge cases
	int front = 0; ///< Index of the first element
	int back = 0; ///< Index one after the last element
public:
	/// Create empty deque without initializing with any size
	FixedDeque() = default;

	/// Initialize with the fixed size
	/// @param size - the fixed number of elements queue will have
	void init(int size) {
		mAssert(!isInit());
		data.resize(size + 1);
	}

	void clear() {
		mAssert(isInit());
		data.clear();
	}

	/// Check if the queue has already been inited
	bool isInit() const {
		return data.size() > 0;
	}

	/// Check if the number of inserted elements is 0
	bool isEmpty() const {
		mAssert(isInit());
		return front == back;
	}

	/// Check if all slots in the queue are full
	bool isFull() const {
		return next(back) == front;
	}

	/// Insert an element at a given index shifting all existing elements back
	/// @param index - the position the element will be at
	/// @param value - the element
	void insert(int index, const T &value) {
		mAssert(!isFull());
		const int idx = move(front, index);
		for (int c = size(); c > index; c--) {
			data[move(front, c)] = data[move(front, c - 1)];
		}
		data[idx] = value;
		back = next(back);
	}

	void insertAfter(int index, const T &value) {
		return insert(index + 1, value);
	}

	/// Remove the element at given index, keeping relative order, by moving all elements after it forward
	void remove(int index) {
		mAssert(!isEmpty());
		for (int c = move(front, index + 1); c != back; c = next(c)) {
			data[prev(c)] = data[c];
		}
		back = prev(back);
	}

	/// Insert element at the end of the sequence
	void pushBack(const T &value) {
		data[back] = value;
		back = next(back);
	}

	/// Remove last element of the sequence
	void popBack() {
		back = prev(back);
	}

	/// Insert element at the front of the sequence
	void pushFront(const T &value) {
		front = prev(front);
		data[front] = value;
	}

	/// Remove first element from the sequence
	void popFront() {
		front = next(front);
	}

	/// Return the number of element in the sequence
	int size() const {
		mAssert(isInit());
		if (back >= front) {
			return back - front;
		} else {
			return back + int(data.size()) - front;
		}
	}

	T &first() {
		mAssert(isInit());
		return data[front];
	}

	T &last() {
		mAssert(isInit());
		const int index = prev(back);
		return data[index];
	}

	/// Index the queue with logical index [0, size())
	T &operator[](int index) {
		const int offsetIndex = move(front, index);
		return data[offsetIndex];
	}

	const T &first() const {
		mAssert(isInit());
		return data[front];
	}

	const T &last() const {
		mAssert(isInit());
		const int index = prev(back);
		return data[index];
	}

	/// Index the queue with logical index [0, size())
	const T &operator[](int index) const {
		const int offsetIndex = move(front, index);
		return data[offsetIndex];
	}
private:
	/// Offset an index in data, wrapping at both ends
	int move(int index, int offset) const {
		mAssert(isInit());
		return (index + offset + int(data.size())) % int(data.size());
	}

	/// Get the index succeeding the given
	int next(int index) const {
		return move(index, 1);
	}

	/// Get the index preceding the given
	int prev(int index) const {
		return move(index, -1);
	}
};



/// Container for elements that supports O(1) indexing, O(TierFactor) insert and remove at index
/// Also support O(1) insert and remove at the start and end of the sequence
/// Must be initialized with the desired TierFactor, it's value should be sqrt(N) where N is the expected number of elements
/// Implemented as a array of FixedDeques each with size TierFactor
/// the first and last deque may not be full but all between them will always be full
template <typename T>
struct TieredVector {
private:
	/// Wrapper over the fixed length deque, pretends to be pointer to FixedDeque
	struct SubArray {
		FixedDeque<T> data;
		std::recursive_mutex mutex;

		SubArray() = default;
		SubArray(const SubArray &) = delete;
		SubArray &operator=(const SubArray &) = delete;

		const FixedDeque<T> &operator*() const {
			return data;
		}

		const FixedDeque<T> *operator->() const {
			return &data;
		}

		FixedDeque<T> &operator*() {
			return data;
		}

		FixedDeque<T> *operator->() {
			return &data;
		}
	};
	
	std::vector<SubArray> data; ///< Array of arrays, only used ones are initialized
	int front = 0; ///< The index of the first array in the sequence
	int back = 0; ///< The index of the last array in the sequence
	int count = 0; ///< The count of inserted arrays in the sequence
	int tierFactor; ///< The current tier factor
	std::recursive_mutex globalMutex;

	/// Wrapper over actual index of an element, see @splitIndex
	struct SplitIndex {
		int arrIndex = -1; ///< Logical index of the array
		int elementIndex = -1; ///< Logical index inside the array
	};
public:
	/// Initialize the TieredVector with the desired factor
	/// Allocates memory only for FixedDeques but not for elements
	TieredVector(int tierFactor)
		: data(tierFactor + 1)
		, tierFactor(tierFactor)
	{}

	/// Check if the number of elements in the sequence is 0
	bool isEmpty() const {
		mAssert( (size() == 0) == (front == back) );
		return front == back;
	}

	/// Check if the sequence is full (size() == tierFactor * tierFactor)
	bool isFull() const {
		return next(back) == front;
	}

	int size() const {
		int sum = 0;
		for (int c = 0; c < data.size(); c++) {
			if (data[c]->isInit()) {
				sum += data[c]->size();
			}
		}
		mAssert(count == sum);
		return count;
	}

	/// Add new element at the end of the sequence
	void pushBack(const T &value) {
		SubArray &subArray = getSubArray(subArrayCount() - 1);
		if (isEmpty() || subArray->isFull()) {
			mAssert(!isFull());
			SubArray &newSubArray = pushBackNewSubArray();
			newSubArray->pushBack(value);
		} else {
			subArray->pushBack(value);
		}
		++count;
	}

	/// Add new element at the front of the sequence
	void pushFront(const T &value) {
		SubArray &subArray = getSubArray(0);
		if (isEmpty() || subArray->isFull()) {
			mAssert(!isFull());
			SubArray &newSubArray = pushFrontNewSubArray();
			newSubArray->pushFront(value);
		} else {
			subArray->pushFront(value);
		}
		++count;
	}

	/// Remove an element at an index
	void remove(int index) {
		mAssert(!isEmpty());
		mAssert(index >= 0 && index < size());

		const int arrCount = subArrayCount();
		const SplitIndex idx = splitIndex(index);
		SubArray &subArray = getSubArray(idx.arrIndex);
		subArray->remove(idx.elementIndex);
		if (idx.arrIndex == 0 || idx.arrIndex == arrCount - 1) {
			// first and last array can be non full so nothing to do
			// remove if it became empty
			if (subArray->isEmpty()) {
				if (idx.arrIndex == 0) {
					removeFirstSubArray();
				} else {
					removeLastSubArray();
				}
			}
			--count;
			return;
		}

		const int lastSize = getSubArray(arrCount - 1)->size();
		// must keep fill the hole, pull elements from next array to fill it
		for (int c = idx.arrIndex; c != arrCount - 1; c++) {
			SubArray &current = getSubArray(c);
			// there is one spot in this deque
			mAssert(current->size() == tierFactor - 1);
			SubArray &next = getSubArray(c + 1);
			// the next one is full or is the last one
			mAssert((next->isFull() || c == arrCount - 2) && next->isInit());
			current->pushBack(next->first());
			mAssert(current->isFull());
			next->popFront();
		}

		SubArray &last = getSubArray(arrCount - 1);
		mAssert(lastSize == last->size() + 1);
		// remove the last if it became empty
		if (last->isEmpty()) {
			mAssert(lastSize == 1);
			removeLastSubArray();
		}
		--count;
	}

	/// Insert an element at a given index, all elements after it will have their index increment by 1
	/// Performs at most 2 * TierFactor steps, moving elements towards the back of the sequence
	/// This involves at most TierFactor inside one FixedDeque and at most TierFactor moves between different FixedDeques
	/// @param index - the index of the inserted elements
	/// @param value - the value to insert
	void insert(int index, const T &value) {
		mAssert(!isFull());
		mAssert(index >= 0 && index < count);

		const SplitIndex idx = splitIndex(index);
		SubArray &insertArray = getSubArray(idx.arrIndex);
		if (!insertArray->isFull()) {
			// if this array is not full, just insert the element inside
			mAssert(&insertArray == &getSubArray(0) || &insertArray == &getSubArray(subArrayCount() - 1));
			insertArray->insert(idx.elementIndex, value);
			++count;
			return;
		}

		SubArray &last = getSubArray(subArrayCount() - 1);
		if (last->isFull()) {
			// there must be at least one free slot in the last deque to move element to it
			pushBackNewSubArray();
		}

		// free one slot in the deque the element will be inserted
		// starting from the last deque, move the last element of the preceding deque and put in as first in the current
		for (int c = subArrayCount() - 1; c != idx.arrIndex; c--) {
			SubArray &current = getSubArray(c);
			mAssert(current->isInit() && !current->isFull());
			SubArray &previous = getSubArray(c - 1);
			mAssert(previous->isInit());
			// the last element in the previous dequeue is the first in this one because of reverse iteration
			current->pushFront(previous->last());
			previous->popBack();
		}

		// there must be only one free slot in this deque that was made by the for loop above
		mAssert(insertArray->size() == tierFactor - 1);
		insertArray->insert(idx.elementIndex, value);
		++count;
	}

	/// Get the element at logical index [0, size())
	T &operator[](int index) {
		const SplitIndex idx = splitIndex(index);
		SubArray &subArray = getSubArray(idx.arrIndex);
		mAssert(subArray->isInit());
		T &value = (*subArray)[idx.elementIndex];
		return value;
	}

	T &first() {
		return (*this)[0];
	}

	T &last() {
		return (*this)[size() - 1];
	}

	/// Get the element at logical index [0, size())
	const T &operator[](int index) const {;
		const SplitIndex idx = splitIndex(index);
		const SubArray &subArray = getSubArray(idx.arrIndex);
		mAssert(subArray->isInit());
		const T &value = (*subArray)[idx.elementIndex];
		return value;
	}

	const T &first() const {
		return (*this)[0];
	}

	const T &last() const {
		return (*this)[size() - 1];
	}

	/// Insert an element at a given index, all elements after it will have their index increment by 1
	/// Performs at most 2 * TierFactor steps, moving elements towards the back of the sequence
	/// This involves at most TierFactor inside one FixedDeque and at most TierFactor moves between different FixedDeques
	/// @param index - the index of the inserted elements
	/// @param value - the value to insert
	void tsInsert(int index, const T &value) {
		mAssert(!isFull());
		mAssert(index >= 0 && index < size());

		std::unique_lock<std::recursive_mutex> globalLock(globalMutex);
		++count;
		const SplitIndex beforePush = splitIndex(index - 1);
		// make space for the new element
		if (getSubArray(0)->isFull() && getSubArray(beforePush.arrIndex)->isFull()) {
			pushFrontNewSubArray();
		}

		const SplitIndex insertIndex = splitIndex(index);
		const SplitIndex idx = splitIndex(index - 1);
		SubArray &insertArray = getSubArray(idx.arrIndex);
		if (!insertArray->isFull()) {
			std::lock_guard<std::recursive_mutex> lock(insertArray.mutex);
			globalLock.unlock();
			// if this array is not full, just insert the element inside
			mAssert(&insertArray == &getSubArray(0) || &insertArray == &getSubArray(subArrayCount() - 1));
			insertArray->insertAfter(idx.elementIndex, value);
			return;
		}

		for (int c = 0; c < idx.arrIndex; c++) {
			SubArray &current = getSubArray(c);
			mAssert(!current->isFull());
			SubArray &next = getSubArray(c + 1);
			mAssert(next->isFull());
			current->pushBack(next->first());
			next->popFront();
		}

		// there must be only one free slot in this deque that was made by the for loop above
		mAssert(insertArray->size() == tierFactor - 1);
		insertArray->insert(idx.elementIndex, value);
	}


	void tsPushFront(const T &value) {
		globalMutex.lock();
		SubArray &subArray = getSubArray(0);
		++count;
		if (isEmpty() || subArray->isFull()) {
			mAssert(!isFull());
			SubArray &newSubArray = pushFrontNewSubArray();
			newSubArray->pushFront(value);
			globalMutex.unlock();
		} else {
			std::lock_guard<std::mutex> lock(subArray.mutex);
			globalMutex.unlock();
			subArray->pushFront(value);
		}
		
	}
	
	void tsPushBack(const T &value) {
		globalMutex.lock();
		++count;
		SubArray &subArray = getSubArray(subArrayCount() - 1);
		if (isEmpty() || subArray->isFull()) {
			mAssert(!isFull());
			SubArray &newSubArray = pushBackNewSubArray();
			newSubArray->pushBack(value);
			globalMutex.unlock();
		} else {
			std::lock_guard<std::mutex> lock(subArray.mutex);
			globalMutex.unlock();
			subArray->pushBack(value);
		}
		
	}

	void dump(const std::vector<T> &vector) const {
		puts("----------------");
		const int sz = std::max<int>(size(), vector.size());
		for (int c = 0; c < sz; c++) {
			const SplitIndex idx = splitIndex(c);
			if (c < size() && c < vector.size()) {
				printf("[%d] v[%d] tv[%d] (%d %d)\n", c, vector[c], operator[](c), idx.arrIndex, idx.elementIndex);
			} else if (c < size()) {
				printf("[%d] v[NULL] tv[%d] (%d %d)\n", c, operator[](c), idx.arrIndex, idx.elementIndex);
			} else {
				printf("[%d] v[%d] tv[NULL] (%d %d)\n", c, vector[c], idx.arrIndex, idx.elementIndex);
			}
		}
		puts("----------------");
	}

	bool operator==(const std::vector<T> &vector) const {
		if (vector.size() != size()) {
			dump(vector);
			return false;
		}

		for (int c = 0; c < size(); c++) {
			if (vector[c] != operator[](c)) {
				dump(vector);
				return false;
			}
		}

		return true;
	}
private:
	/// Get the number of arrays that are initialized (contain at least one element)
	int subArrayCount() const {
		if (back >= front) {
			return back - front;
		} else {
			return back + int(data.size()) - front;
		}
	}

	/// Split logical index into its two components
	SplitIndex splitIndex(int index) const {
		const int firstSize = getSubArray(0)->size();
		if (index < firstSize) {
			return {0, index};
		}
		const int offsetIndex = index + (tierFactor - firstSize);
		const int arrayIndex = offsetIndex / tierFactor;
		const int elementIndex = offsetIndex % tierFactor;
		return {arrayIndex, elementIndex};
	}

	/// Get the SubArray wrapper for a logical index [0, subArrayCount())
	const SubArray &getSubArray(int index) const {
		const int offsetIndex = move(front, index);
		return data[offsetIndex];
	}

	/// Get the SubArray wrapper for a logical index [0, subArrayCount())
	SubArray &getSubArray(int index) {
		const int offsetIndex = move(front, index);
		return data[offsetIndex];
	}

	/// Add new array at the back of the sequence and initialize it
	SubArray &pushBackNewSubArray() {
		mAssert(!isFull());
		SubArray &created = data[back];
		mAssert(!created->isInit());
		created->init(tierFactor);
		back = next(back);
		return created;
	}

	/// Add new array at the front of the sequence and initialize it
	SubArray &pushFrontNewSubArray() {
		mAssert(!isFull());
		const int index = prev(front);
		SubArray &created = data[index];
		mAssert(!created->isInit());
		created->init(tierFactor);
		front = index;
		return created;
	}

	void removeLastSubArray() {
		back = prev(back);
		data[back]->clear();
	}

	void removeFirstSubArray() {
		data[front]->clear();
		front = next(front);
	}

	/// Offset an index wrapping in both ends if needed
	int move(int index, int offset) const {
		return (index + offset + int(data.size())) % int(data.size());
	}

	/// Get the index succeeding the given
	int next(int index) const {
		return move(index, 1);
	}

	/// Get the index preceding the given
	int prev(int index) const {
		return move(index, -1);
	}
};