#include "spq.h"
#include "../../debug.h"

namespace spq {

bucket_t::bucket_t() {
	_d = 0;
	_nnodes = 0;
	_heapind = -1;
	_pnodes = new node_t;
	_pnodes->_name = -1;
	_pnodes->_pprev = _pnodes->_pnext = _pnodes;
}

bucket_t::~bucket_t() {
	ASSERT(_pnodes, "Head node is absent.");
	delete _pnodes;
}

void bucket_t::add_node(node_t *pnode) {
	ASSERT(pnode, "Node is NULL");
	ASSERT(!pnode->owned(), "Node still owned by bucket d=" << pnode->_pbucket->d()
		<< " cannot be added to bucket d=" << _d);
	ASSERT(pnode->_pnext != pnode && pnode->_pprev != pnode, "Unable to add a head node.");
	pnode->_pbucket = this;
	pnode->_pnext = _pnodes;
	pnode->_pprev = _pnodes->_pprev;
	_pnodes->_pprev->_pnext = pnode;
	_pnodes->_pprev = pnode;
	++_nnodes;
}

void bucket_t::remove_node(node_t *pnode) {
	ASSERT(pnode, "Node is NULL");
	ASSERT(pnode->_pbucket == this, "Bucket " << _d << " does not own node " << pnode->_name << ".");
	ASSERT(pnode->_pprev != pnode && pnode->_pnext != pnode, "Can't remove head node.");
	pnode->_pprev->_pnext = pnode->_pnext;
	pnode->_pnext->_pprev = pnode->_pprev;
	pnode->_pbucket = NULL;
	--_nnodes;
}

node_t *bucket_t::disown_nodes() {
	ASSERT(_pnodes, "Invalid head.");
	if(is_empty())
		return NULL;
	else {
		node_t *first = _pnodes->_pnext;
		_pnodes->_pprev->_pnext = first->_pprev = NULL;
		for(node_t *cursor = first; cursor; cursor = cursor->_pnext)
			cursor->_pbucket = NULL;
		_pnodes->_pprev = _pnodes->_pnext = _pnodes;
		_nnodes = 0;
		return first;
	}
}

spq_t::spq_t(uint64_t nnodes, uint64_t maxcost) {
	ASSERT(nnodes > 0, "# of nodes should be positive.");
	ASSERT(maxcost > 0, "Maxcost should be positive");

	_nitems = 0;

	const uint64_t map_nbuckets = nnodes;
	const double map_max_load_factor = maxcost;
	_d2pbucket.max_load_factor(map_max_load_factor);
	_d2pbucket.rehash(map_nbuckets);

	const uint64_t max_nbuckets_alive = maxcost + 1;
	_heap.reserve(max_nbuckets_alive + 1);
	_heap.push_back(0);

	_pnodes = new node_t[nnodes];
	for(uint64_t i = 0; i < nnodes; ++i) {
		_pnodes[i]._name = i;
		_pnodes[i]._pbucket = NULL;
#ifdef DEBUG
		_pnodes[i]._pprev = NULL;
		_pnodes[i]._pnext = NULL;
#endif
	}
}

spq_t::~spq_t() {
	delete [] _pnodes;

	for(uint64_t i = 1; i < _heap.size(); ++i) {
		uint64_t d = _heap[i];
		bucket_t *pbucket = _d2pbucket[d];
		ASSERT(pbucket, i << "'th bucket[d=" << d << "] should be present.");
		delete pbucket;
	}
}

void spq_t::insert(int64_t name, uint64_t d) {
	ASSERT(name >= 0, "Invalid node name.");
	node_t *pnode = &_pnodes[name];
	ASSERT(!pnode->owned(), "Cannot reinsert: inserted node should not be owned.");
	bucket_t *pbucket = find_bucket_ensure_exists(d);
	pbucket->add_node(pnode);
	++_nitems;
}

void spq_t::decrease_key(int64_t name, uint64_t dnew) {
	ASSERT(name >= 0, "Invalid node name.");
	ASSERT(!is_empty(), "Queue is empty.");
	node_t *pnode = &_pnodes[name];
	ASSERT(pnode->owned(), "Node should exist to decrease its key.");
	ASSERT(pnode->_pbucket->d() >= dnew, "Cannot decrease: node " << name << " has invalid new key " << dnew << ".");
	bucket_t *pbucket = pnode->_pbucket;
	pbucket->remove_node(pnode);
	if(pbucket->is_empty())
		delete_bucket(pbucket);
	pbucket = find_bucket_ensure_exists(dnew);
	pbucket->add_node(pnode);
}

node_t *spq_t::delete_mins(uint64_t &dmin) {
	ASSERT(!is_empty(), "Cannot delete from an empty queue.");
	bucket_t *pmin_bucket = _d2pbucket[_heap[1]];
	dmin = pmin_bucket->d();
	_nitems -= pmin_bucket->nnodes();
	node_t *pnodes = pmin_bucket->disown_nodes();
	delete_bucket(pmin_bucket);
	return pnodes;
}

void spq_t::delete_bucket(bucket_t *pbucket) {
	ASSERT(pbucket, "Bucket is NULL.");
	ASSERT(pbucket->heapind() >= 0, "Invalid heap index.");
	uint64_t d = pbucket->d();
	heap_delete_at(pbucket->heapind());
	_d2pbucket.erase(d);
	delete pbucket;	
}

bucket_t *spq_t::find_bucket_ensure_exists(uint64_t d) {
	ASSERT(d >= 0, "Invalid d.");
	bucket_t *pbucket;
	if(!_d2pbucket.count(d)) {
		pbucket = new bucket_t;
		pbucket->d(d);
		_d2pbucket[d] = pbucket;
		heap_insert(d);
	} else
		pbucket = _d2pbucket[d];
	return pbucket;
}

void spq_t::heap_insert(uint64_t d) {
	_heap.push_back(d);
	_d2pbucket[d]->heapind(_heap.size() - 1);
	heap_siftup(_heap.size() - 1);
}

void spq_t::heap_delete_at(uint64_t i) {
	ASSERT(i >= 1 && i < _heap.size(), "Invalid index for deletion.");
	_heap[i] = _heap[_heap.size() - 1];
	_d2pbucket[_heap[i]]->heapind(i);
	_heap.pop_back();
	if(i < _heap.size())
		heap_siftdown(i);
}

void spq_t::heap_siftup(uint64_t i) {
	ASSERT(i >= 1 && i < _heap.size(), "Invalid input for heap-siftup.");
	uint64_t ival = _heap[i];
	while(i > 1 && ival < _heap[i >> 1]) {
		_heap[i] = _heap[i >> 1];
		_d2pbucket[_heap[i]]->heapind(i);
		i >>= 1;
	}
	_heap[i] = ival;
	_d2pbucket[ival]->heapind(i);
	ASSERT_MINHEAP(_heap);
}

void spq_t::heap_siftdown(uint64_t i) {
	ASSERT(i >= 1 && i < _heap.size(), "Invalid heap state or index to sift down from.");
	uint64_t ival = _heap[i];
	while((i << 1) < _heap.size()) {
		uint64_t ileft = i << 1;
		uint64_t iright = ileft + 1;
		uint64_t imin = (iright > _heap.size() || _heap[ileft] <= _heap[iright]) ? ileft : iright;
		if(ival > _heap[imin]) {
			_heap[i] = _heap[imin];
			_d2pbucket[_heap[i]]->heapind(i);
			i = imin;
		} else
			break;
	}
	_heap[i] = ival;
	_d2pbucket[ival]->heapind(i);
	ASSERT_MINHEAP(_heap);
}

#ifdef DEBUG

void spq_t::dump() {
	std::cout << "sqp_t {\n\tnitems: " << _nitems << "\n\tbuckets:";
	if(_heap.size() <= 1)
		std::cout << " n/a";
	std::cout << '\n';
	for(uint64_t i = 1; i < _heap.size(); ++i) {
		uint64_t d = _heap[i];
		bucket_t *pbucket = _d2pbucket[d];
		std::cout << "\t\t[d=" << d << ",hind=" << pbucket->heapind() << "]";
		node_t *pnode = pbucket->pnodes()->_pnext;
		while(pnode != pbucket->pnodes()) {
			std::cout << ' ' << pnode->_name;
			pnode = pnode->_pnext;
		}
		std::cout << '\n';
	}
	std::cout << "}\n";
}

#endif

}

