#pragma once
#include<stdlib.h>
#include<string.h>

static inline size_t parent_index(size_t idx){
	return (idx - 1) / 2;
}

static inline size_t right_child_index(size_t idx){
	return (idx * 2) + 2;
}

static inline size_t left_child_index(size_t idx){
	return (idx * 2) + 1;
}

template<typename T>
class binary_heap {
	T* storage = 0;
	size_t item_count = 0;
	size_t storage_size = 0;

	void swap_elements(size_t idxa, size_t idxb){
		char temp_spot[sizeof(T)];
		memcpy(temp_spot, &storage[idxa], sizeof(T));
		memcpy(&storage[idxa], &storage[idxb], sizeof(T));
		memcpy(&storage[idxb], temp_spot, sizeof(T));
	}

public:
	void insert(const T& new_item){
		if(!storage){
			storage_size = 20;
			storage = (T*)malloc(sizeof(T) * storage_size);
		}
		if(storage_size == item_count){
			storage_size *= 2;
			storage = (T*)realloc(storage, sizeof(T) * storage_size);
		}
		new (&storage[item_count]) T(new_item);
		item_count++;
		sift_up(item_count - 1);
	}
	void sift_up(size_t idx){
		if(!idx)
			return;
		if(storage[idx] > storage[parent_index(idx)]) {
			swap_elements(idx, parent_index(idx));
			sift_up(parent_index(idx));
		}
	}
	void sift_down(size_t idx){
		if(left_child_index(idx) >= item_count)
			return; // We didn't have any children
		size_t gci = left_child_index(idx);
		if(right_child_index(idx) < item_count && storage[right_child_index(idx)] > storage[gci])
			gci = right_child_index(idx);
		if(storage[gci] > storage[idx]){
			swap_elements(idx, gci);
			sift_down(gci);
		}
	}
	bool empty(){
		return !item_count;
	}
	// What if there aren't any items?
	T pop(){
		T retval(storage[0]);
		storage[0].~T();
		memcpy(&storage[0], &storage[item_count - 1], sizeof(T));
		item_count--;
		sift_down(0);
		return retval;
	}
	// Could also be called cancel
	void remove(const T& item){
		for(size_t i = 0; i < item_count; i++){
			if(storage[i] == item){
				// What about the destructor for the item we're removing?
				memcpy(&storage[i], &storage[item_count - 1], sizeof(T));
				item_count--;
				sift_down(i);
				return; // This assumes no duplicates
			}
		}
	}
	T& top(){
		return storage[0];
	}
	~binary_heap(){
		/* delete [] would have called the destructors
		 * We used free, which does NOT call the destructors
		 */
		for(size_t i = 0; i < item_count; i++){
			storage[i].~T();
		}
		free(storage);
	}
};