#include <limits>
#include "rheap.h"

namespace rheap {

	#define MIN(X, Y) ((X) < (Y) ? (X) : (Y))

	inline uint64_t ilog2(uint64_t x) {
		ASSERT(x > 0, "Log2 is undefined for non-positive arguments.");
		return (sizeof(uint64_t) << 3) - __builtin_clzll(x - 1);
	}


    node_t::node_t() {
        assign(-1, -1);
        pprev = pnext = NULL;
#ifdef DEBUG
        bucket = -1;
#endif
	}

    inline void node_t::assign(int64_t name, int64_t d) {
        this->name = name;
        this->d = d;
    }

	bucket_t::bucket_t() {
		u = -1;
		pnodes = new node_t;
		pnodes->pnext = pnodes->pprev = pnodes;
	}

	bucket_t::~bucket_t() {
		ASSERT(pnodes, "pnodes should always be present (prior to deletion)");
		delete pnodes;
	}

	inline void bucket_t::drop_nodes() {
		pnodes->pprev = pnodes->pnext = pnodes;
	}

	void bucket_t::insert(node_t *pnode) {
		MSG("\t\t- bucket inserts node {n:" << pnode->name << ", d:" << pnode->d << "}");
		pnode->pnext = pnodes;
		pnode->pprev = pnodes->pprev;
		pnodes->pprev->pnext = pnode;
		pnodes->pprev = pnode;
	}

	void bucket_t::remove(node_t *pnode) {
		ASSERT(!(pnode->pprev == pnode && pnode->pnext == pnode),
			"Can't kill the root of a doubly-linked list.");
		pnode->pprev->pnext = pnode->pnext;
		pnode->pnext->pprev = pnode->pprev;
#ifdef DEBUG
		pnode->pprev = pnode->pnext = NULL;
#endif
	}

	node_t* bucket_t::min() {
		ASSERT(!is_empty(), "Cannot extract from an empty bucket; use is_empty externally.");
		node_t *mind_node = pnodes->pnext;
		int64_t mind = mind_node->d;
		node_t *cursor = pnodes->pnext->pnext;
		while(cursor != pnodes) {
			if(cursor->d < mind) {
				mind = cursor->d;
				mind_node = cursor;
			}
			cursor = cursor->pnext;
		}
		return mind_node;
	}


	rheap_t::rheap_t(uint64_t nnodes, uint64_t max_cost) {
		ASSERT(nnodes > 0 && max_cost > 0, "Input must be non-negative");
		nitems = 0;
		pnodes = new node_t[nnodes];
		uint64_t C = nnodes * max_cost;
		nbuckets = ilog2(C + 1) + 2;
		pbuckets = new bucket_t[nbuckets + 1];
		pbuckets[0].u = -1;
		int64_t pot = 1;
		for(int64_t i = 1; i < nbuckets; ++i, pot <<= 1)
			pbuckets[i].u = pot - 1;
		pbuckets[nbuckets].u = C + 1;
	}

	rheap_t::~rheap_t() {
		delete [] pbuckets;
		delete [] pnodes;
	}

	void rheap_t::insert(int64_t name, int64_t d) {
		ASSERT(!contains(name), "Cannot reinsert a node (" << name << "," << d << ").");
		MSG("Inserting new node (" << name << "," << d << ")");
		node_t *pnode = &pnodes[name];
		pnode->assign(name, d);
		insert_node(nbuckets, pnode);
		++nitems;
	}

	void rheap_t::decrease_key(int64_t name, int64_t d_new) {
		ASSERT(pnodes && contains(name) && pnodes[name].d >= d_new,
				"Node " << name << " should exist; its d = "
					<< pnodes[name].d << " cannot decrease to " << d_new << ".")
		node_t *pnode = &pnodes[name];
		bucket_t::remove(pnode);
		pnode->d = d_new;
		insert_node(pnode->bucket - 1, pnode);
	}

	int64_t rheap_t::delete_min() {
		ASSERT(!is_empty(), "Cannot delete from an empty heap; use is_empty() externally.");

		int32_t ib;
		for(ib = 1; pbuckets[ib].is_empty(); ++ib);
		node_t *pmin_node = (ib == 1) ? pbuckets[1].pnodes->pnext : pbuckets[ib].min();
		int64_t min_node_name = pmin_node->name;
		int64_t dmin = pmin_node->d;
		MSG("Min node (" << min_node_name << "," << dmin << ") found in bucket " << ib << ".");

		pbuckets[ib].remove(pmin_node);
		pmin_node->name = -1;
		--nitems;

		uint64_t ub = pbuckets[ib].u;
		pbuckets[0].u = dmin - 1;
		pbuckets[1].u = dmin;
		uint64_t pot = 1, u_plus_sz = dmin;
		for(int64_t i = 2; i < ib; ++i) {
			u_plus_sz += pot;
			pbuckets[i].u = MIN(u_plus_sz, ub);
			pot <<= 1;
		}
		ASSERT(ib == 1 || pbuckets[ib - 1].u == pbuckets[ib].u, "Invalid new upper bound.");

        if(ib > 1) {
            MSG("Purging bucket " << ib);
            node_t *pnode = pbuckets[ib].pnodes->pnext;
            while(pnode != pbuckets[ib].pnodes) {
                MSG("Relocating node (" << pnode->name << "," << pnode->d << ").");
                node_t *pnext = pnode->pnext;
                insert_node(ib - 1, pnode);
                pnode = pnext;
            }
            pbuckets[ib].drop_nodes();
        }

		return min_node_name;
	}

	void rheap_t::insert_node(int64_t irmost_bucket, node_t *pnode) {
		ASSERT(irmost_bucket >= 0 && irmost_bucket <= nbuckets && pnode && pnode->d >= 0,
			"Invalid input for insert_node(" << irmost_bucket << ","
				<< "(" << pnode->name << "," << pnode->d << ") ).");
		MSG("\t- looking for bucket by u-bound rtl starting from " << irmost_bucket);
		int32_t ib;
        for(ib = irmost_bucket; pbuckets[ib].u >= pnode->d; --ib);
		++ib;
		ASSERT(ib >= 0 && ib <= nbuckets, "Invalid bucket index for node insertion.");
		MSG("\t- found bucket with index " << ib);

		pnode->bucket = ib;
		pbuckets[ib].insert(pnode);
	}

#ifdef DEBUG
	void rheap_t::dump() const {
		std::cout << "rheap_t: nbuckets = " << nbuckets << ", nitems = " << nitems << std::endl;
		for(int32_t i = 0; i <= nbuckets; ++i) {
			std::cout << "b" << i << "[u: " << pbuckets[i].u << "] ";
			const node_t *root = pbuckets[i].get_nodes();
			const node_t *node = root->pnext;
			while(node != root) {
				std::cout << "(n:" << node->name << ", d:" << node->d << ") ";
				node = node->pnext;
			}
			std::cout << std::endl;
		}
		std::cout << std::endl;
	}
#endif

}
