/***********************************************************************
 * Software License Agreement (BSD License)
 *
 * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
 * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
 *
 * THE BSD LICENSE
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *************************************************************************/

#ifndef HEAP_H
#define HEAP_H


#include <algorithm>
using namespace std;

namespace cvflann
{

/**
 * Priority Queue Implementation
 *
 * The priority queue is implemented with a heap.  A heap is a complete
 * (full) binary tree in which each parent is less than both of its
 * children, but the order of the children is unspecified.
 * Note that a heap uses 1-based indexing to allow for power-of-2
 * location of parents and children.  We ignore element 0 of Heap array.
 */
template <typename T>
class Heap {

	/**
	* Storage array for the heap.
	* Type T must be comparable.
	*/
	T* heap;
    int length;

	/**
	 * Number of element in the heap
	 */
	int count;



public:
	/**
	 * Constructor.
	 *
	 * Params:
	 *     size = heap size
	 */

	Heap(int size)
	{
        length = size+1;
		heap = new T[length];  // heap uses 1-based indexing
		count = 0;
	}


	/**
	 * Destructor.
	 *
	 */
	~Heap()
	{
		delete[] heap;
	}

	/**
	 *
	 * Returns: heap size
	 */
	int size()
	{
		return count;
	}

	/**
	 * Tests if the heap is empty
	 *
	 * Returns: true is heap empty, false otherwise
	 */
	bool empty()
	{
		return size()==0;
	}

	/**
	 * Clears the heap.
	 */
	void clear()
	{
		count = 0;
	}


	/**
	 * Insert a new element in the heap.
	 *
	 * We select the next empty leaf node, and then keep moving any larger
	 * parents down until the right location is found to store this element.
	 *
	 * Params:
	 *     value = the new element to be inserted in the heap
	 */
	void insert(T value)
	{
		/* If heap is full, then return without adding this element. */
		if (count == length-1) {
			return;
		}

		int loc = ++(count);   /* Remember 1-based indexing. */

		/* Keep moving parents down until a place is found for this node. */
		int par = loc / 2;                 /* Location of parent. */
		while (par > 0  && value < heap[par]) {
			heap[loc] = heap[par];     /* Move parent down to loc. */
			loc = par;
			par = loc / 2;
		}
		/* Insert the element at the determined location. */
		heap[loc] = value;
	}



	/**
 	 * Returns the node of minimum value from the heap (top of the heap).
	 *
	 * Params:
	 *     value = out parameter used to return the min element
	 * Returns: false if heap empty
	 */
	bool popMin(T& value)
	{
		if (count == 0) {
 			return false;
		}

		/* Switch first node with last. */
		swap(heap[1],heap[count]);

		count -= 1;
		heapify(1);      /* Move new node 1 to right position. */

		value = heap[count + 1];
		return true;  /* Return old last node. */
	}


	/**
	 * Reorganizes the heap (a parent is smaller than its children)
	 * starting with a node.
	 *
	 * Params:
	 *     parent = node form which to start heap reorganization.
	 */
	void heapify(int parent)
	{
		int minloc = parent;

		/* Check the left child */
		int left = 2 * parent;
		if (left <= count && heap[left] < heap[parent]) {
			minloc = left;
		}

		/* Check the right child */
		int right = left + 1;
		if (right <= count && heap[right] < heap[minloc]) {
			minloc = right;
		}

		/* If a child was smaller, than swap parent with it and Heapify. */
		if (minloc != parent) {
			swap(heap[parent],heap[minloc]);
			heapify(minloc);
		}
	}

};

}

#endif //HEAP_H