/*
 * p_tree.h
 *
 *  Created on: Dec 21, 2013
 *      Author: dspies
 */

#ifndef P_TREE_H_
#define P_TREE_H_

#include <cassert>
#include <exception>
#include <stddef.h>
#include <utility>

#include "run_assert.h"
#include "util.h"

namespace p_tree_n {

inline const size_t max_depth(size_t size) {
	return 2 + bit_size(size);
}

inline const bool valid_for(size_t depth, size_t size) {
	return depth <= max_depth(size);
}

template<typename K, typename V, bool is_const>
class iter_sup;

template<typename K, typename V>
class p_tree {
	friend class iter_sup<K, V, false> ;
	friend class iter_sup<K, V, true> ;
public:
	typedef iter_sup<K, V, false> iterator;
	typedef iter_sup<K, V, true> const_iterator;

private:
	class node;
	class ptr_t;

	node* header;
	size_t lsize;
	size_t max_size;

	const bool erase(const ptr_t& p);

	template<bool B>
	const bool erase_iter(const iter_sup<K, V, B>& iter) {
		return erase(iter.my_ptr);
	}
	template<typename S>
	iterator get_iterator_uref(S&& iter,
			typename std::enable_if<is_same_base<S, const_iterator>::value>::type* =
					0) {
		return iterator(forward_ref_type<S>(iter.my_ptr));
	}
	template<typename S, typename T>
	std::pair<iterator, bool> insert_uref(S&& key, T&& val,
			typename std::enable_if<
					is_same_base<S, K>::value && is_same_base<T, V>::value>::type* =
					0) {
		std::pair<iterator, bool> res = header->insert_uref(
				std::forward<S>(key), std::forward<T>(val), 0,
				max_depth(lsize + 1));
		if (res.second) {
			++lsize;
			max_size = std::max(max_size, lsize);
		}
		return res;
	}
public:

	p_tree() :
			header(new node()), lsize(0), max_size(0) {
	}
	p_tree(const p_tree& rhs);
	p_tree(p_tree&& rhs) :
			header(new node()), lsize(0), max_size(0) {
		swap(rhs);
	}
	template<bool B = false>
	p_tree(std::pair<K, V> arr[], size_t size, bool_t<B> can_move = bool_t<B>()) :
			header(new node()), lsize(size), max_size(size) {
		header->left = new node(header, arr, 0, size, can_move);
	}

	~p_tree() {
		clear();
		header->remove_from_list();
	}
	p_tree& operator=(const p_tree& rhs) {
		return *this = p_tree(rhs);
	}
	p_tree& operator=(p_tree&& rhs) {
		swap(rhs);
		return *this;
	}
	void swap(p_tree& rhs) {
		std::swap(header, rhs.header);
		std::swap(lsize, rhs.lsize);
	}
	const size_t size() const {
		return lsize;
	}
	const bool empty() const {
		return lsize == 0;
	}
	const bool erase(const iterator& iter) {
		return erase_iter(iter);
	}
	const bool erase(const const_iterator& iter) {
		return erase_iter(iter);
	}
	void clear() {
		header->erase_children();
		lsize = 0;
	}
	iterator begin() {
		return iterator(ptr_t(header->get_left_most()));
	}
	const_iterator begin() const {
		return const_iterator(ptr_t(header->get_left_most()));
	}
	iterator end() {
		return iterator(ptr_t(header));
	}
	const_iterator end() const {
		return const_iterator(ptr_t(header));
	}
	iterator get_iterator(const const_iterator& iter) {
		return get_iterator_uref(iter);
	}
	iterator get_iterator(const_iterator&& iter) {
		return get_iterator_uref(std::move(iter));
	}
	std::pair<const K, V>& operator[](const const_iterator& iter) {
		return iter.my_ptr->t;
	}
	void to_array(std::pair<K, V> arr[]) {
		if (header->left)
			header->left->to_array(arr, 0);
	}
	std::pair<iterator, bool> insert(const K& k, const V& v) {
		return insert_uref(k, v);
	}
	std::pair<iterator, bool> insert(const K& k, V&& v) {
		return insert_uref(k, v);
	}
	std::pair<iterator, bool> insert(K&& k, const V& v) {
		return insert_uref(k, v);
	}
	std::pair<iterator, bool> insert(K&& k, V&& v) {
		return insert_uref(k, v);
	}
	std::pair<const K, V>& top() {
		return header->left->t;
	}
	const std::pair<const K, V>& top() const {
		return header->left->t;
	}
	const size_t count(const K& k) const {
		return header->count(k);
	}
};

template<typename K, typename V>
class p_tree<K, V>::node {
	friend class p_tree;
private:
	size_t ptr_count;
	node() :
			ptr_count(2), parent(), left(), right() {
	}
	template<typename S, typename T>
	explicit node(S&& k, T&& v, node* parent,
			typename std::enable_if<
					is_same_base<S, K>::value && is_same_base<T, V>::value>::type* =
					0) :
			ptr_count(3), t(std::forward<S>(k), std::forward<T>(v)), parent(
					parent), left(), right() {
	}
	node(const node& rhs);

	template<bool B>
	node(node* parent, std::pair<K, V> arr[], size_t offset, size_t end,
			const bool_t<B>& can_move);

	node& operator=(const node& rhs);

	template<int dir>
	node* const get_dir();

	const size_t count_size() const;

	void rebalance_scapegoat(size_t depth);

	void rebalance(size_t size);

	//This shouldn't really be a member function, but I couldn't figure out
	//how to put it outside of node
	node* const build_from(node* parent, node* arr[], size_t offset,
			size_t end);

	size_t send_to(node* arr[], size_t offset);
public:
	union {
		std::pair<const K, V> t;
	};
	ptr_t parent;
	node* left;
	node* right;
	void increment() {
		ptr_count += 4;
	}
	void decrement() {
		assert(ptr_count >= 4);
		if ((ptr_count -= 4) <= 1) {
			//One of those rare cases where object suicide is the proper course of action
			delete this;
		}
	}
	const bool remove_from_list() {
		assert(ptr_count & 2);
		left = NULL;
		right = NULL;
		if ((ptr_count -= 2) <= 1) {
			delete this;
			return true;
		} else
			return false;
	}
	const bool in_list() const {
		return ptr_count & 2;
	}
	const bool has_t() const {
		return ptr_count & 1;
	}

	~node() {
		assert(ptr_count <= 1);
		if (ptr_count) {
			t.~pair();
		}
	}

	const bool replace_child(node* child, node* new_child) {
		if (left == child) {
			left = new_child;
			return true;
		} else if (right == child) {
			right = new_child;
			return true;
		} else
			return false;
	}

	//TODO should this be const?
	template<int dir>
	node* const get_most();

	node* const get_left_most() {
		return get_most<-1>();
	}

	node* const get_next() {
		return get_dir<1>();
	}
	node* const get_prev() {
		return get_dir<-1>();
	}

	const bool operator<(const node& rhs) const {
		return has_t() && (!rhs.has_t() || t.first < rhs.t.first);
	}

	const size_t to_array(std::pair<K, V> arr[], size_t offset);

	void erase_children();

	node* const my_ptr(const int_t<-1>&) {
		return left;
	}
	node* const my_ptr(const int_t<1>&) {
		return right;
	}
	template<typename S, typename T>
	std::pair<iterator, bool> insert_uref(S&& key, T&& val, size_t cur_depth,
			size_t max_depth,
			typename std::enable_if<
					is_same_base<S, K>::value && is_same_base<T, V>::value>::type* =
					0);

	const size_t count(const K& k) const;
};

template<typename K, typename V>
class p_tree<K, V>::ptr_t {
	friend class node;
	friend class p_tree;
	friend class iter_sup<K, V, false> ;
	friend class iter_sup<K, V, true> ;
private:

	node* my_node;

	void increment() {
		if (my_node)
			my_node->increment();
	}
	void decrement() {
		if (my_node)
			my_node->decrement();
	}
	explicit ptr_t(node* n) :
			my_node(n) {
		increment();
	}
public:
	ptr_t() :
			my_node(NULL) {
	}
	ptr_t(const ptr_t& rhs) :
			my_node(rhs.my_node) {
		increment();
	}
	ptr_t(ptr_t&& rhs) :
			my_node(NULL) {
		swap(rhs);
	}
	~ptr_t() {
		decrement();
	}
	ptr_t& operator=(const ptr_t& rhs) {
		return *this = ptr_t(rhs);
	}
	ptr_t& operator=(ptr_t&& rhs) {
		swap(rhs);
		return *this;
	}
	void swap(ptr_t& rhs) {
		std::swap(my_node, rhs.my_node);
	}
	node* const get() const {
		return my_node;
	}
	node* const operator->() const {
		return my_node;
	}
	node& operator*() const {
		return *my_node;
	}
	void reset() {
		decrement();
		my_node = NULL;
	}
	void reset(node* n) {
		*this = ptr_t(n);
	}
	const bool operator==(const ptr_t& rhs) const {
		return my_node == rhs.my_node;
	}
	const bool operator!=(const ptr_t& rhs) const {
		return my_node != rhs.my_node;
	}

	const bool trace_to_list();
};

template<typename K, typename V, bool is_const>
class iter_sup {
	friend class p_tree<K, V> ;
	friend class iter_sup<K, V, false> ;
	friend class iter_sup<K, V, true> ;
private:
	typedef typename p_tree<K, V>::ptr_t ptr_t;

	template<typename S>
	explicit iter_sup(S&& my_ptr,
			typename std::enable_if<is_same_base<S, ptr_t>::value>::type* = 0) :
			my_ptr(std::forward<S>(my_ptr)) {
	}

	explicit iter_sup(typename p_tree<K, V>::node* n) :
			my_ptr(n) {
	}

	template<bool B>
	const bool eq_iter(const iter_sup<K, V, B>& rhs) const {
		return my_ptr == rhs.my_ptr;
	}
	template<bool B>
	const bool neq_iter(const iter_sup<K, V, B>& rhs) const {
		return my_ptr != rhs.my_ptr;
	}

	typedef typename std::conditional<is_const, const std::pair<const K, V>,
			std::pair<const K, V>>::type ret_type_t;
	template<bool B>
	const bool lt_iter(const iter_sup<K, V, B>& rhs) {
		return *my_ptr < *rhs.my_ptr;
	}
	ptr_t my_ptr;
public:
	iter_sup() :
			my_ptr() {
	}

	template<typename S>
	explicit iter_sup(S&& rhs,
			typename std::enable_if<
					is_const && is_same_base<S, iter_sup<K, V, false>>::value>::type* =
					0) :
			my_ptr(forward_ref_type<S>(rhs.my_ptr)) {
	}

	iter_sup& operator=(const iter_sup& rhs) {
		return *this = iter_sup(rhs);
	}

	iter_sup& operator=(iter_sup&& rhs) {
		swap(rhs);
		return *this;
	}
	void swap(iter_sup& rhs) {
		my_ptr.swap(rhs.my_ptr);
	}
	ret_type_t* const operator->() const {
		return get();
	}
	ret_type_t* const get() const {
		return &**this;
	}
	ret_type_t& operator*() const {
		return my_ptr->t;
	}
	void reset() {
		my_ptr.reset();
	}
	const bool operator==(const iter_sup<K, V, false>& rhs) const {
		return eq_iter(rhs);
	}
	const bool operator==(const iter_sup<K, V, true>& rhs) const {
		return eq_iter(rhs);
	}
	const bool operator!=(const iter_sup<K, V, false>& rhs) const {
		return neq_iter(rhs);
	}
	const bool operator!=(const iter_sup<K, V, true>& rhs) const {
		return neq_iter(rhs);
	}
	iter_sup& operator++() {
		if (!my_ptr.trace_to_list())
			my_ptr.reset(my_ptr->get_next());
		return *this;
	}
	iter_sup operator++(int dummy) {
		iter_sup res = *this;
		++*this;
		return res;
	}
	iter_sup& operator--() {
		my_ptr.trace_to_list();
		//TODO Handle other insertions
		my_ptr.reset(my_ptr->get_prev());
		return *this;
	}
	iter_sup operator--(int dummy) {
		iter_sup res = *this;
		--*this;
		return res;
	}

	const bool in_list() const {
		return my_ptr->in_list();
	}

	const bool is_non_null() const {
		return my_ptr.get();
	}

	const size_t hash() const {
		return size_t(my_ptr.get());
	}

	const bool operator<(const iter_sup<K, V, false>& rhs) {
		return lt_iter(rhs);
	}
	const bool operator<(const iter_sup<K, V, true>& rhs) {
		return lt_iter(rhs);
	}
};

template<typename K, typename V>
const bool p_tree<K, V>::erase(const ptr_t& p) {
	assert(!(header->left) || header->left->left != header->left);
	if (!p->in_list())
		return false;
	assert(p->parent->left == p.get() || p->parent->right == p.get());
	node* nplace = p.get();
	if (nplace->left) {
		nplace = nplace->left;
		if (nplace->right) {
			do {
				nplace = nplace->right;
			} while (nplace->right);
			nplace->parent->right = nplace->left;
			if (nplace->left)
				nplace->left->parent.swap(nplace->parent);
			nplace->parent.swap(p->parent);

			nplace->left = p->left;
			if (nplace->left)
				nplace->left->parent.reset(nplace);
		} else {
			nplace->parent.swap(p->parent);
		}
		nplace->right = p->right;
		if (nplace->right)
			nplace->right->parent.reset(nplace);
		run_assert(nplace->parent->replace_child(p.get(), nplace));

		p->parent.reset(nplace->get_next());
	} else if (nplace->right) {
		nplace = nplace->right;
		nplace->parent.swap(p->parent);
		run_assert(nplace->parent->replace_child(p.get(), nplace));

		p->parent.reset(nplace->get_left_most());
	} else {
		nplace = p->get_next();
		run_assert(p->parent->replace_child(p.get(), NULL));

		p->parent.reset(nplace);
	}
	p->remove_from_list();
	--lsize;
	if (lsize <= max_size / 2) {
		if (lsize > 0) {
			header->left->rebalance(lsize);
		}
		max_size = lsize;
	}
	return true;
}

template<typename K, typename V>
p_tree<K, V>::p_tree(const p_tree& rhs) :
		header(new node()), lsize(0), max_size(0) {
	header->parent.reset();
	std::pair<K, V> array[rhs.size()];
	rhs.to_array(array);
	*this = p_tree(array, rhs.size(), bool_t<true>());
}

template<typename K, typename V>
template<int dir>
auto p_tree<K, V>::node::get_most() -> node* const {
	node* res = this;
	int_t<dir> d;
	while (res->my_ptr(d))
		res = res->my_ptr(d);
	return res;
}

template<typename K, typename V>
template<int dir>
auto p_tree<K, V>::node::get_dir() -> node* const {
	int_t<dir> d;
	node* my_dir = my_ptr(d);
	if (my_dir) {
		return my_dir->get_most<-dir>();
	} else {
		const node* cur = this;
		while (cur->parent->my_ptr(d) == cur) {
			cur = cur->parent.get();
		}
		return cur->parent.get();
	}
}

template<typename K, typename V>
const size_t p_tree<K, V>::node::to_array(std::pair<K, V> arr[],
		size_t offset) {
	if (left)
		offset = left->to_array(arr, offset);
	arr[offset++] = t;
	if (right)
		offset = right->to_array(arr, offset);
	return offset;
}

template<typename K, typename V>
template<bool B>
p_tree<K, V>::node::node(node* parent, std::pair<K, V> arr[], size_t offset,
		size_t end, const bool_t<B>& can_move) :
		ptr_count(3), parent(parent) {
	size_t middle = (offset + end) / 2;
	if (middle > offset) {
		left = new node(this, arr, offset, middle, can_move);
	} else
		left = NULL;
	t = movable_if<B>(arr[middle++]);
	if (middle < end) {
		right = new node(this, arr, middle, end, can_move);
	} else
		right = NULL;
}

template<typename K, typename V>
void p_tree<K, V>::node::erase_children() {
	if (left) {
		left->erase_children();
		left->remove_from_list();
		left = NULL;
	}
	if (right) {
		right->erase_children();
		right->remove_from_list();
		right = NULL;
	}
}

template<typename K, typename V>
const bool p_tree<K, V>::ptr_t::trace_to_list() {
	if (!my_node->in_list()) {
		node* next = my_node->parent.get();
		if (!next->in_list()) {
			do {
				next = next->parent.get();
			} while (!next->in_list());
			for (;;) {
				ptr_t& parent = my_node->parent;
				if (parent->in_list()) {
					assert(parent.get() == next);
					break;
				}
				swap(parent);
				parent.reset(next);
			}
		}
		reset(next);
		return true;
	} else
		return false;
}

template<typename K, typename V>
template<typename S, typename T>
auto p_tree<K, V>::node::insert_uref(S&& key, T&& val, size_t cur_depth,
		size_t max_depth,
		typename std::enable_if<
				is_same_base<S, K>::value && is_same_base<T, V>::value>::type*)->std::pair<iterator, bool> {
	std::pair<iterator, bool> res;
	if (!has_t() || key < t.first) {
		if (left)
			return left->insert_uref(std::forward<S>(key), std::forward<T>(val),
					cur_depth + 1, max_depth);
		else {
			left = new node(std::forward<S>(key), std::forward<T>(val), this);
			res = std::pair<iterator, bool>(iterator(left), true);
		}
	} else if (t.first < key) {
		if (right)
			return right->insert_uref(std::forward<S>(key),
					std::forward<T>(val), cur_depth + 1, max_depth);
		else {
			right = new node(std::forward<S>(key), std::forward<T>(val), this);
			res = std::pair<iterator, bool>(iterator(right), true);
		}
	} else
		return std::pair<iterator, bool>(iterator(this), false);
	if (cur_depth > max_depth) {
		rebalance_scapegoat(1);
	}
	return res;
}

template<typename K, typename V>
const size_t p_tree<K, V>::node::count(const K& k) const {
	if (!has_t() || k < t.first) {
		if (left)
			return left->count(k);
		else
			return 0;
	} else if (t.first < k) {
		if (right)
			return right->count(k);
		else
			return 0;
	} else
		return 1;
}

template<typename K, typename V>
const size_t p_tree<K, V>::node::count_size() const {
	size_t res = 1;
	if (left)
		res += left->count_size();
	if (right)
		res += right->count_size();
	return res;
}

template<typename K, typename V>
void p_tree<K, V>::node::rebalance_scapegoat(size_t depth) {
	size_t size = count_size();
	if (!valid_for(depth, size)) {
		rebalance(size);
	} else {
		parent->rebalance_scapegoat(depth + 1);
	}
}

template<typename K, typename V>
auto p_tree<K, V>::node::build_from(node* parent, node* arr[], size_t offset,
		size_t end) -> node* const {
	size_t middle = (offset + end) / 2;
	if (middle > offset) {
		arr[middle]->left = build_from(arr[middle], arr, offset, middle);
	} else
		arr[middle]->left = NULL;
	arr[middle]->parent.reset(parent);
	if (middle + 1 < end) {
		arr[middle]->right = build_from(arr[middle], arr, middle + 1, end);
	} else
		arr[middle]->right = NULL;
	return arr[middle];
}

template<typename K, typename V>
void p_tree<K, V>::node::rebalance(size_t size) {
	node* arr[size];
	send_to(arr, 0);
	node* my_parent = parent.get();
	run_assert(
			my_parent->replace_child(this,
					build_from(my_parent, arr, 0, size)));
}

template<typename K, typename V>
size_t p_tree<K, V>::node::send_to(node* arr[], size_t offset) {
	if (left)
		offset = left->send_to(arr, offset);
	arr[offset++] = this;
	if (right)
		offset = right->send_to(arr, offset);
	return offset;
}

}

namespace std {
template<typename K, typename V, bool B>
struct hash<p_tree_n::iter_sup<K, V, B>> {
	const size_t operator()(const p_tree_n::iter_sup<K, V, B>& p) const
			noexcept {
		return p.hash();
	}
};
}

#endif /* P_TREE_H_ */