#pragma warning disable 162
#pragma warning disable 429
#define DECREASE_KEY

namespace Pathfinding {
	/// <summary>
	/// Binary heap implementation.
	/// Binary heaps are really fast for ordering nodes in a way that
	/// makes it possible to get the node with the lowest F score.
	/// Also known as a priority queue.
	///
	/// This has actually been rewritten as a 4-ary heap
	/// for performance, but it's the same principle.
	///
	/// See: http://en.wikipedia.org/wiki/Binary_heap
	/// See: https://en.wikipedia.org/wiki/D-ary_heap
	/// </summary>
	public class BinaryHeap {
		/// <summary>Number of items in the tree</summary>
		public int numberOfItems;

		/// <summary>The tree will grow by at least this factor every time it is expanded</summary>
		public float growthFactor = 2;

		/// <summary>
		/// Number of children of each node in the tree.
		/// Different values have been tested and 4 has been empirically found to perform the best.
		/// See: https://en.wikipedia.org/wiki/D-ary_heap
		/// </summary>
		const int D = 4;

		/// <summary>
		/// Sort nodes by G score if there is a tie when comparing the F score.
		/// Disabling this will improve pathfinding performance with around 2.5%
		/// but may break ties between paths that have the same length in a less
		/// desirable manner (only relevant for grid graphs).
		/// </summary>
		const bool SortGScores = true;

		public const ushort NotInHeap = 0xFFFF;

		/// <summary>Internal backing array for the heap</summary>
		private Tuple[] heap;

		/// <summary>True if the heap does not contain any elements</summary>
		public bool isEmpty {
			get {
				return numberOfItems <= 0;
			}
		}

		/// <summary>Item in the heap</summary>
		private struct Tuple {
			public PathNode node;
			public uint F;

			public Tuple (uint f, PathNode node) {
				this.F = f;
				this.node = node;
			}
		}

		/// <summary>
		/// Rounds up v so that it has remainder 1 when divided by D.
		/// I.e it is of the form n*D + 1 where n is any non-negative integer.
		/// </summary>
		static int RoundUpToNextMultipleMod1 (int v) {
			// I have a feeling there is a nicer way to do this
			return v + (4 - ((v-1) % D)) % D;
		}

		/// <summary>Create a new heap with the specified initial capacity</summary>
		public BinaryHeap (int capacity) {
			// Make sure the size has remainder 1 when divided by D
			// This allows us to always guarantee that indices used in the Remove method
			// will never throw out of bounds exceptions
			capacity = RoundUpToNextMultipleMod1(capacity);

			heap = new Tuple[capacity];
			numberOfItems = 0;
		}

		/// <summary>Removes all elements from the heap</summary>
		public void Clear () {
#if DECREASE_KEY
			// Clear all heap indices
			// This is important to avoid bugs
			for (int i = 0; i < numberOfItems; i++) {
				heap[i].node.heapIndex = NotInHeap;
			}
#endif

			numberOfItems = 0;
		}

		internal PathNode GetNode (int i) {
			return heap[i].node;
		}

		internal void SetF (int i, uint f) {
			heap[i].F = f;
		}

		/// <summary>Expands to a larger backing array when the current one is too small</summary>
		void Expand () {
			// 65533 == 1 mod 4 and slightly smaller than 1<<16 = 65536
			int newSize = System.Math.Max(heap.Length+4, System.Math.Min(65533, (int)System.Math.Round(heap.Length*growthFactor)));

			// Make sure the size has remainder 1 when divided by D
			// This allows us to always guarantee that indices used in the Remove method
			// will never throw out of bounds exceptions
			newSize = RoundUpToNextMultipleMod1(newSize);

			// Check if the heap is really large
			// Also note that heaps larger than this are not supported
			// since PathNode.heapIndex is a ushort and can only store
			// values up to 65535 (NotInHeap = 65535 is reserved however)
			if (newSize > (1<<16) - 2) {
				throw new System.Exception("Binary Heap Size really large (>65534). A heap size this large is probably the cause of pathfinding running in an infinite loop. ");
			}

			var newHeap = new Tuple[newSize];
			heap.CopyTo(newHeap, 0);
#if ASTARDEBUG
			UnityEngine.Debug.Log("Resizing binary heap to "+newSize);
#endif
			heap = newHeap;
		}

		/// <summary>Adds a node to the heap</summary>
		public void Add (PathNode node) {
			if (node == null) throw new System.ArgumentNullException("node");

#if DECREASE_KEY
			// Check if node is already in the heap
			if (node.heapIndex != NotInHeap) {
				DecreaseKey(heap[node.heapIndex], node.heapIndex);
				return;
			}
#endif

			if (numberOfItems == heap.Length) {
				Expand();
			}

			DecreaseKey(new Tuple(0, node), (ushort)numberOfItems);
			numberOfItems++;
		}

		void DecreaseKey (Tuple node, ushort index) {
			// This is where 'obj' is in the binary heap logically speaking
			// (for performance reasons we don't actually store it there until
			// we know the final index, that's just a waste of CPU cycles)
			int bubbleIndex = index;
			// Update F value, it might have changed since the node was originally added to the heap
			uint nodeF = node.F = node.node.F;
			uint nodeG = node.node.G;

			while (bubbleIndex != 0) {
				// Parent node of the bubble node
				int parentIndex = (bubbleIndex-1) / D;

				if (nodeF < heap[parentIndex].F || (SortGScores && nodeF == heap[parentIndex].F && nodeG > heap[parentIndex].node.G)) {
					// Swap the bubble node and parent node
					// (we don't really need to store the bubble node until we know the final index though
					// so we do that after the loop instead)
					heap[bubbleIndex] = heap[parentIndex];
#if DECREASE_KEY
					heap[bubbleIndex].node.heapIndex = (ushort)bubbleIndex;
#endif
					bubbleIndex = parentIndex;
				} else {
					break;
				}
			}

			heap[bubbleIndex] = node;
#if DECREASE_KEY
			node.node.heapIndex = (ushort)bubbleIndex;
#endif
		}

		/// <summary>Returns the node with the lowest F score from the heap</summary>
		public PathNode Remove () {
			PathNode returnItem = heap[0].node;

#if DECREASE_KEY
			returnItem.heapIndex = NotInHeap;
#endif

			numberOfItems--;
			if (numberOfItems == 0) return returnItem;

			// Last item in the heap array
			var swapItem = heap[numberOfItems];
			var swapItemG = swapItem.node.G;

			int swapIndex = 0, parent;

			// Trickle upwards
			while (true) {
				parent = swapIndex;
				uint swapF = swapItem.F;
				int pd = parent * D + 1;

				// If this holds, then the indices used
				// below are guaranteed to not throw an index out of bounds
				// exception since we choose the size of the array in that way
				if (pd <= numberOfItems) {
					// Loading all F scores here instead of inside the if statements
					// reduces data dependencies and improves performance
					uint f0 = heap[pd+0].F;
					uint f1 = heap[pd+1].F;
					uint f2 = heap[pd+2].F;
					uint f3 = heap[pd+3].F;

					// The common case is that all children of a node are present
					// so the first comparison in each if statement below
					// will be extremely well predicted so it is essentially free
					// (I tried optimizing for the common case, but it didn't affect performance at all
					// at the expense of longer code, the CPU branch predictor is really good)

					if (pd+0 < numberOfItems && (f0 < swapF || (SortGScores && f0 == swapF && heap[pd+0].node.G < swapItemG))) {
						swapF = f0;
						swapIndex = pd+0;
					}

					if (pd+1 < numberOfItems && (f1 < swapF || (SortGScores && f1 == swapF && heap[pd+1].node.G < (swapIndex == parent ? swapItemG : heap[swapIndex].node.G)))) {
						swapF = f1;
						swapIndex = pd+1;
					}

					if (pd+2 < numberOfItems && (f2 < swapF || (SortGScores && f2 == swapF && heap[pd+2].node.G < (swapIndex == parent ? swapItemG : heap[swapIndex].node.G)))) {
						swapF = f2;
						swapIndex = pd+2;
					}

					if (pd+3 < numberOfItems && (f3 < swapF || (SortGScores && f3 == swapF && heap[pd+3].node.G < (swapIndex == parent ? swapItemG : heap[swapIndex].node.G)))) {
						swapIndex = pd+3;
					}
				}

				// One if the parent's children are smaller or equal, swap them
				// (actually we are just pretenting we swapped them, we hold the swapData
				// in local variable and only assign it once we know the final index)
				if (parent != swapIndex) {
					heap[parent] = heap[swapIndex];
#if DECREASE_KEY
					heap[parent].node.heapIndex = (ushort)parent;
#endif
				} else {
					break;
				}
			}

			// Assign element to the final position
			heap[swapIndex] = swapItem;
#if DECREASE_KEY
			swapItem.node.heapIndex = (ushort)swapIndex;
#endif

			// For debugging
			// Validate ();

			return returnItem;
		}

		void Validate () {
			for (int i = 1; i < numberOfItems; i++) {
				int parentIndex = (i-1)/D;
				if (heap[parentIndex].F > heap[i].F) {
					throw new System.Exception("Invalid state at " + i + ":" +  parentIndex + " ( " + heap[parentIndex].F + " > " + heap[i].F + " ) ");
				}
#if DECREASE_KEY
				if (heap[i].node.heapIndex != i) {
					throw new System.Exception("Invalid heap index");
				}
#endif
			}
		}

		/// <summary>
		/// Rebuilds the heap by trickeling down all items.
		/// Usually called after the hTarget on a path has been changed
		/// </summary>
		public void Rebuild () {
#if ASTARDEBUG
			int changes = 0;
#endif

			for (int i = 2; i < numberOfItems; i++) {
				int bubbleIndex = i;
				var node = heap[i];
				uint nodeF = node.F;
				while (bubbleIndex != 1) {
					int parentIndex = bubbleIndex / D;

					if (nodeF < heap[parentIndex].F) {
						heap[bubbleIndex] = heap[parentIndex];
#if DECREASE_KEY
						heap[bubbleIndex].node.heapIndex = (ushort)bubbleIndex;
#endif

						heap[parentIndex] = node;
#if DECREASE_KEY
						heap[parentIndex].node.heapIndex = (ushort)parentIndex;
#endif

						bubbleIndex = parentIndex;
#if ASTARDEBUG
						changes++;
#endif
					} else {
						break;
					}
				}
			}

#if ASTARDEBUG
			UnityEngine.Debug.Log("+++ Rebuilt Heap - "+changes+" changes +++");
#endif
		}
	}
}