#region License
/*
The MIT License (MIT)

Copyright (c) 2013 Daniel "BlueRaja" Pflughoeft

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
 */
#endregion

using System;
using System.Collections;
using System.Collections.Generic;
using System.Runtime.CompilerServices;

namespace SimSharp {
  /// <summary>
  /// A copy of StablePriorityQueue which also has generic explicit priority-type
  /// </summary>
  /// <remarks>
  /// Original sources from https://github.com/BlueRaja/High-Speed-Priority-Queue-for-C-Sharp
  /// </remarks>
  /// <typeparam name="TItem">The values in the queue.  Must extend the GenericPriorityQueue class</typeparam>
  /// <typeparam name="TPriority">The priority-type.  Must extend IComparable&lt;TPriority&gt;</typeparam>
  public sealed class GenericPriorityQueue<TItem, TPriority> : IEnumerable<TItem>
      where TItem : GenericPriorityQueueNode<TPriority>
      where TPriority : IComparable<TPriority> {
    private int _numNodes;
    private TItem[] _nodes;
    private long _numNodesEverEnqueued;
    private readonly Comparison<TPriority> _comparer;

    /// <summary>
    /// Instantiate a new Priority Queue
    /// </summary>
    /// <param name="maxNodes">The max nodes ever allowed to be enqueued (going over this will cause undefined behavior)</param>
    public GenericPriorityQueue(int maxNodes) : this(maxNodes, Comparer<TPriority>.Default) { }

    /// <summary>
    /// Instantiate a new Priority Queue
    /// </summary>
    /// <param name="maxNodes">The max nodes ever allowed to be enqueued (going over this will cause undefined behavior)</param>
    /// <param name="comparer">The comparer used to compare TPriority values.</param>
    public GenericPriorityQueue(int maxNodes, IComparer<TPriority> comparer) : this(maxNodes, comparer.Compare) { }

    /// <summary>
    /// Instantiate a new Priority Queue
    /// </summary>
    /// <param name="maxNodes">The max nodes ever allowed to be enqueued (going over this will cause undefined behavior)</param>
    /// <param name="comparer">The comparison function to use to compare TPriority values</param>
    public GenericPriorityQueue(int maxNodes, Comparison<TPriority> comparer) {
#if DEBUG
      if (maxNodes <= 0) {
        throw new InvalidOperationException("New queue size cannot be smaller than 1");
      }
#endif

      _numNodes = 0;
      _nodes = new TItem[maxNodes + 1];
      _numNodesEverEnqueued = 0;
      _comparer = comparer;
    }

    /// <summary>
    /// Returns the number of nodes in the queue.
    /// O(1)
    /// </summary>
    public int Count {
      get {
        return _numNodes;
      }
    }

    /// <summary>
    /// Returns the maximum number of items that can be enqueued at once in this queue.  Once you hit this number (ie. once Count == MaxSize),
    /// attempting to enqueue another item will cause undefined behavior.  O(1)
    /// </summary>
    public int MaxSize {
      get {
        return _nodes.Length - 1;
      }
    }

    /// <summary>
    /// Removes every node from the queue.
    /// O(n) (So, don't do this often!)
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public void Clear() {
      Array.Clear(_nodes, 1, _numNodes);
      _numNodes = 0;
    }

    /// <summary>
    /// Returns (in O(1)!) whether the given node is in the queue.  O(1)
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public bool Contains(TItem node) {
#if DEBUG
      if (node == null) {
        throw new ArgumentNullException("node");
      }
      if (node.QueueIndex < 0 || node.QueueIndex >= _nodes.Length) {
        throw new InvalidOperationException("node.QueueIndex has been corrupted. Did you change it manually? Or add this node to another queue?");
      }
#endif

      return (_nodes[node.QueueIndex] == node);
    }

    /// <summary>
    /// Enqueue a node to the priority queue.  Lower values are placed in front. Ties are broken by first-in-first-out.
    /// If the queue is full, the result is undefined.
    /// If the node is already enqueued, the result is undefined.
    /// O(log n)
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public void Enqueue(TItem node, TPriority priority) {
#if DEBUG
      if (node == null) {
        throw new ArgumentNullException("node");
      }
      if (_numNodes >= _nodes.Length - 1) {
        throw new InvalidOperationException("Queue is full - node cannot be added: " + node);
      }
      if (Contains(node)) {
        throw new InvalidOperationException("Node is already enqueued: " + node);
      }
#endif

      node.Priority = priority;
      _numNodes++;
      _nodes[_numNodes] = node;
      node.QueueIndex = _numNodes;
      node.InsertionIndex = _numNodesEverEnqueued++;
      CascadeUp(node);
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    private void CascadeUp(TItem node) {
      //aka Heapify-up
      int parent;
      if (node.QueueIndex > 1) {
        parent = node.QueueIndex >> 1;
        TItem parentNode = _nodes[parent];
        if (HasHigherPriority(parentNode, node))
          return;

        //Node has lower priority value, so move parent down the heap to make room
        _nodes[node.QueueIndex] = parentNode;
        parentNode.QueueIndex = node.QueueIndex;

        node.QueueIndex = parent;
      } else {
        return;
      }
      while (parent > 1) {
        parent >>= 1;
        TItem parentNode = _nodes[parent];
        if (HasHigherPriority(parentNode, node))
          break;

        //Node has lower priority value, so move parent down the heap to make room
        _nodes[node.QueueIndex] = parentNode;
        parentNode.QueueIndex = node.QueueIndex;

        node.QueueIndex = parent;
      }
      _nodes[node.QueueIndex] = node;
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    private void CascadeDown(TItem node) {
      //aka Heapify-down
      int finalQueueIndex = node.QueueIndex;
      int childLeftIndex = 2 * finalQueueIndex;

      // If leaf node, we're done
      if (childLeftIndex > _numNodes) {
        return;
      }

      // Check if the left-child is higher-priority than the current node
      int childRightIndex = childLeftIndex + 1;
      TItem childLeft = _nodes[childLeftIndex];
      if (HasHigherPriority(childLeft, node)) {
        // Check if there is a right child. If not, swap and finish.
        if (childRightIndex > _numNodes) {
          node.QueueIndex = childLeftIndex;
          childLeft.QueueIndex = finalQueueIndex;
          _nodes[finalQueueIndex] = childLeft;
          _nodes[childLeftIndex] = node;
          return;
        }
        // Check if the left-child is higher-priority than the right-child
        TItem childRight = _nodes[childRightIndex];
        if (HasHigherPriority(childLeft, childRight)) {
          // left is highest, move it up and continue
          childLeft.QueueIndex = finalQueueIndex;
          _nodes[finalQueueIndex] = childLeft;
          finalQueueIndex = childLeftIndex;
        } else {
          // right is even higher, move it up and continue
          childRight.QueueIndex = finalQueueIndex;
          _nodes[finalQueueIndex] = childRight;
          finalQueueIndex = childRightIndex;
        }
      }
      // Not swapping with left-child, does right-child exist?
      else if (childRightIndex > _numNodes) {
        return;
      } else {
        // Check if the right-child is higher-priority than the current node
        TItem childRight = _nodes[childRightIndex];
        if (HasHigherPriority(childRight, node)) {
          childRight.QueueIndex = finalQueueIndex;
          _nodes[finalQueueIndex] = childRight;
          finalQueueIndex = childRightIndex;
        }
        // Neither child is higher-priority than current, so finish and stop.
        else {
          return;
        }
      }

      while (true) {
        childLeftIndex = 2 * finalQueueIndex;

        // If leaf node, we're done
        if (childLeftIndex > _numNodes) {
          node.QueueIndex = finalQueueIndex;
          _nodes[finalQueueIndex] = node;
          break;
        }

        // Check if the left-child is higher-priority than the current node
        childRightIndex = childLeftIndex + 1;
        childLeft = _nodes[childLeftIndex];
        if (HasHigherPriority(childLeft, node)) {
          // Check if there is a right child. If not, swap and finish.
          if (childRightIndex > _numNodes) {
            node.QueueIndex = childLeftIndex;
            childLeft.QueueIndex = finalQueueIndex;
            _nodes[finalQueueIndex] = childLeft;
            _nodes[childLeftIndex] = node;
            break;
          }
          // Check if the left-child is higher-priority than the right-child
          TItem childRight = _nodes[childRightIndex];
          if (HasHigherPriority(childLeft, childRight)) {
            // left is highest, move it up and continue
            childLeft.QueueIndex = finalQueueIndex;
            _nodes[finalQueueIndex] = childLeft;
            finalQueueIndex = childLeftIndex;
          } else {
            // right is even higher, move it up and continue
            childRight.QueueIndex = finalQueueIndex;
            _nodes[finalQueueIndex] = childRight;
            finalQueueIndex = childRightIndex;
          }
        }
        // Not swapping with left-child, does right-child exist?
        else if (childRightIndex > _numNodes) {
          node.QueueIndex = finalQueueIndex;
          _nodes[finalQueueIndex] = node;
          break;
        } else {
          // Check if the right-child is higher-priority than the current node
          TItem childRight = _nodes[childRightIndex];
          if (HasHigherPriority(childRight, node)) {
            childRight.QueueIndex = finalQueueIndex;
            _nodes[finalQueueIndex] = childRight;
            finalQueueIndex = childRightIndex;
          }
          // Neither child is higher-priority than current, so finish and stop.
          else {
            node.QueueIndex = finalQueueIndex;
            _nodes[finalQueueIndex] = node;
            break;
          }
        }
      }
    }

    /// <summary>
    /// Returns true if 'higher' has higher priority than 'lower', false otherwise.
    /// Note that calling HasHigherPriority(node, node) (ie. both arguments the same node) will return false
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    private bool HasHigherPriority(TItem higher, TItem lower) {
      var cmp = _comparer(higher.Priority, lower.Priority);
      return (cmp < 0 || (cmp == 0 && higher.InsertionIndex < lower.InsertionIndex));
    }

    /// <summary>
    /// Removes the head of the queue (node with minimum priority; ties are broken by order of insertion), and returns it.
    /// If queue is empty, result is undefined
    /// O(log n)
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public TItem Dequeue() {
#if DEBUG
      if (_numNodes <= 0) {
        throw new InvalidOperationException("Cannot call Dequeue() on an empty queue");
      }

      if (!IsValidQueue()) {
        throw new InvalidOperationException("Queue has been corrupted (Did you update a node priority manually instead of calling UpdatePriority()?" +
                                            "Or add the same node to two different queues?)");
      }
#endif

      TItem returnMe = _nodes[1];
      //If the node is already the last node, we can remove it immediately
      if (_numNodes == 1) {
        _nodes[1] = null;
        _numNodes = 0;
        return returnMe;
      }

      //Swap the node with the last node
      TItem formerLastNode = _nodes[_numNodes];
      _nodes[1] = formerLastNode;
      formerLastNode.QueueIndex = 1;
      _nodes[_numNodes] = null;
      _numNodes--;

      //Now bubble formerLastNode (which is no longer the last node) down
      CascadeDown(formerLastNode);
      return returnMe;
    }

    /// <summary>
    /// Resize the queue so it can accept more nodes.  All currently enqueued nodes are remain.
    /// Attempting to decrease the queue size to a size too small to hold the existing nodes results in undefined behavior
    /// O(n)
    /// </summary>
    public void Resize(int maxNodes) {
#if DEBUG
      if (maxNodes <= 0) {
        throw new InvalidOperationException("Queue size cannot be smaller than 1");
      }

      if (maxNodes < _numNodes) {
        throw new InvalidOperationException("Called Resize(" + maxNodes + "), but current queue contains " + _numNodes + " nodes");
      }
#endif

      TItem[] newArray = new TItem[maxNodes + 1];
      int highestIndexToCopy = Math.Min(maxNodes, _numNodes);
      Array.Copy(_nodes, newArray, highestIndexToCopy + 1);
      _nodes = newArray;
    }

    /// <summary>
    /// Returns the head of the queue, without removing it (use Dequeue() for that).
    /// If the queue is empty, behavior is undefined.
    /// O(1)
    /// </summary>
    public TItem First {
      get {
#if DEBUG
        if (_numNodes <= 0) {
          throw new InvalidOperationException("Cannot call .First on an empty queue");
        }
#endif

        return _nodes[1];
      }
    }

    /// <summary>
    /// This method must be called on a node every time its priority changes while it is in the queue.  
    /// <b>Forgetting to call this method will result in a corrupted queue!</b>
    /// Calling this method on a node not in the queue results in undefined behavior
    /// O(log n)
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public void UpdatePriority(TItem node, TPriority priority) {
#if DEBUG
      if (node == null) {
        throw new ArgumentNullException("node");
      }
      if (!Contains(node)) {
        throw new InvalidOperationException("Cannot call UpdatePriority() on a node which is not enqueued: " + node);
      }
#endif

      node.Priority = priority;
      OnNodeUpdated(node);
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    private void OnNodeUpdated(TItem node) {
      //Bubble the updated node up or down as appropriate
      int parentIndex = node.QueueIndex >> 1;

      if (parentIndex > 0 && HasHigherPriority(node, _nodes[parentIndex])) {
        CascadeUp(node);
      } else {
        //Note that CascadeDown will be called if parentNode == node (that is, node is the root)
        CascadeDown(node);
      }
    }

    /// <summary>
    /// Removes a node from the queue.  The node does not need to be the head of the queue.  
    /// If the node is not in the queue, the result is undefined.  If unsure, check Contains() first
    /// O(log n)
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public void Remove(TItem node) {
#if DEBUG
      if (node == null) {
        throw new ArgumentNullException("node");
      }
      if (!Contains(node)) {
        throw new InvalidOperationException("Cannot call Remove() on a node which is not enqueued: " + node);
      }
#endif

      //If the node is already the last node, we can remove it immediately
      if (node.QueueIndex == _numNodes) {
        _nodes[_numNodes] = null;
        _numNodes--;
        return;
      }

      //Swap the node with the last node
      TItem formerLastNode = _nodes[_numNodes];
      _nodes[node.QueueIndex] = formerLastNode;
      formerLastNode.QueueIndex = node.QueueIndex;
      _nodes[_numNodes] = null;
      _numNodes--;

      //Now bubble formerLastNode (which is no longer the last node) up or down as appropriate
      OnNodeUpdated(formerLastNode);
    }

    public IEnumerator<TItem> GetEnumerator() {
      IEnumerable<TItem> e = new ArraySegment<TItem>(_nodes, 1, _numNodes);
      return e.GetEnumerator();
    }

    IEnumerator IEnumerable.GetEnumerator() {
      return GetEnumerator();
    }

    /// <summary>
    /// <b>Should not be called in production code.</b>
    /// Checks to make sure the queue is still in a valid state.  Used for testing/debugging the queue.
    /// </summary>
    public bool IsValidQueue() {
      for (int i = 1; i < _nodes.Length; i++) {
        if (_nodes[i] != null) {
          int childLeftIndex = 2 * i;
          if (childLeftIndex < _nodes.Length && _nodes[childLeftIndex] != null && HasHigherPriority(_nodes[childLeftIndex], _nodes[i]))
            return false;

          int childRightIndex = childLeftIndex + 1;
          if (childRightIndex < _nodes.Length && _nodes[childRightIndex] != null && HasHigherPriority(_nodes[childRightIndex], _nodes[i]))
            return false;
        }
      }
      return true;
    }
  }

  /// <summary>
  /// A copy of StablePriorityQueue where the items themselves contain an implicit priority.
  /// </summary>
  /// <remarks>
  /// Either <typeparamref name="TItem"/> implements IComparable&lt;typeparamref name="TItem"/>&gt; or
  /// a custom comparer must be provided.
  /// </remarks>
  /// <typeparam name="TItem">The values in the queue. Must extend the GenericPriorityQueueNode class</typeparam>
  public sealed class GenericPriorityQueue<TItem> : IEnumerable<TItem>
      where TItem : GenericPriorityQueueNode {
    private int _numNodes;
    private TItem[] _nodes;
    private long _numNodesEverEnqueued;
    private readonly Comparison<TItem> _comparer;

    /// <summary>
    /// Instantiate a new Priority Queue
    /// </summary>
    /// <param name="maxNodes">The max nodes ever allowed to be enqueued (going over this will cause undefined behavior)</param>
    public GenericPriorityQueue(int maxNodes) : this(maxNodes, Comparer<TItem>.Default) { }

    /// <summary>
    /// Instantiate a new Priority Queue
    /// </summary>
    /// <param name="maxNodes">The max nodes ever allowed to be enqueued (going over this will cause undefined behavior)</param>
    /// <param name="comparer">The comparer used to compare TPriority values.</param>
    public GenericPriorityQueue(int maxNodes, IComparer<TItem> comparer) : this(maxNodes, comparer.Compare) { }

    /// <summary>
    /// Instantiate a new Priority Queue
    /// </summary>
    /// <param name="maxNodes">The max nodes ever allowed to be enqueued (going over this will cause undefined behavior)</param>
    /// <param name="comparer">The comparison function to use to compare TPriority values</param>
    public GenericPriorityQueue(int maxNodes, Comparison<TItem> comparer) {
#if DEBUG
      if (maxNodes <= 0) {
        throw new InvalidOperationException("New queue size cannot be smaller than 1");
      }
#endif

      _numNodes = 0;
      _nodes = new TItem[maxNodes + 1];
      _numNodesEverEnqueued = 0;
      _comparer = comparer;
    }

    /// <summary>
    /// Returns the number of nodes in the queue.
    /// O(1)
    /// </summary>
    public int Count {
      get {
        return _numNodes;
      }
    }

    /// <summary>
    /// Returns the maximum number of items that can be enqueued at once in this queue.  Once you hit this number (ie. once Count == MaxSize),
    /// attempting to enqueue another item will cause undefined behavior.  O(1)
    /// </summary>
    public int MaxSize {
      get {
        return _nodes.Length - 1;
      }
    }

    /// <summary>
    /// Removes every node from the queue.
    /// O(n) (So, don't do this often!)
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public void Clear() {
      Array.Clear(_nodes, 1, _numNodes);
      _numNodes = 0;
    }

    /// <summary>
    /// Returns (in O(1)!) whether the given node is in the queue.  O(1)
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public bool Contains(TItem node) {
#if DEBUG
      if (node == null) {
        throw new ArgumentNullException("node");
      }
      if (node.QueueIndex < 0 || node.QueueIndex >= _nodes.Length) {
        throw new InvalidOperationException("node.QueueIndex has been corrupted. Did you change it manually? Or add this node to another queue?");
      }
#endif

      return (_nodes[node.QueueIndex] == node);
    }

    /// <summary>
    /// Enqueue a node to the priority queue.  Lower values are placed in front. Ties are broken by first-in-first-out.
    /// If the queue is full, the result is undefined.
    /// If the node is already enqueued, the result is undefined.
    /// O(log n)
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public void Enqueue(TItem node) {
#if DEBUG
      if (node == null) {
        throw new ArgumentNullException("node");
      }
      if (_numNodes >= _nodes.Length - 1) {
        throw new InvalidOperationException("Queue is full - node cannot be added: " + node);
      }
      if (Contains(node)) {
        throw new InvalidOperationException("Node is already enqueued: " + node);
      }
#endif
      
      _numNodes++;
      _nodes[_numNodes] = node;
      node.QueueIndex = _numNodes;
      node.InsertionIndex = _numNodesEverEnqueued++;
      CascadeUp(node);
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    private void CascadeUp(TItem node) {
      //aka Heapify-up
      int parent;
      if (node.QueueIndex > 1) {
        parent = node.QueueIndex >> 1;
        TItem parentNode = _nodes[parent];
        if (HasHigherPriority(parentNode, node))
          return;

        //Node has lower priority value, so move parent down the heap to make room
        _nodes[node.QueueIndex] = parentNode;
        parentNode.QueueIndex = node.QueueIndex;

        node.QueueIndex = parent;
      } else {
        return;
      }
      while (parent > 1) {
        parent >>= 1;
        TItem parentNode = _nodes[parent];
        if (HasHigherPriority(parentNode, node))
          break;

        //Node has lower priority value, so move parent down the heap to make room
        _nodes[node.QueueIndex] = parentNode;
        parentNode.QueueIndex = node.QueueIndex;

        node.QueueIndex = parent;
      }
      _nodes[node.QueueIndex] = node;
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    private void CascadeDown(TItem node) {
      //aka Heapify-down
      int finalQueueIndex = node.QueueIndex;
      int childLeftIndex = 2 * finalQueueIndex;

      // If leaf node, we're done
      if (childLeftIndex > _numNodes) {
        return;
      }

      // Check if the left-child is higher-priority than the current node
      int childRightIndex = childLeftIndex + 1;
      TItem childLeft = _nodes[childLeftIndex];
      if (HasHigherPriority(childLeft, node)) {
        // Check if there is a right child. If not, swap and finish.
        if (childRightIndex > _numNodes) {
          node.QueueIndex = childLeftIndex;
          childLeft.QueueIndex = finalQueueIndex;
          _nodes[finalQueueIndex] = childLeft;
          _nodes[childLeftIndex] = node;
          return;
        }
        // Check if the left-child is higher-priority than the right-child
        TItem childRight = _nodes[childRightIndex];
        if (HasHigherPriority(childLeft, childRight)) {
          // left is highest, move it up and continue
          childLeft.QueueIndex = finalQueueIndex;
          _nodes[finalQueueIndex] = childLeft;
          finalQueueIndex = childLeftIndex;
        } else {
          // right is even higher, move it up and continue
          childRight.QueueIndex = finalQueueIndex;
          _nodes[finalQueueIndex] = childRight;
          finalQueueIndex = childRightIndex;
        }
      }
      // Not swapping with left-child, does right-child exist?
      else if (childRightIndex > _numNodes) {
        return;
      } else {
        // Check if the right-child is higher-priority than the current node
        TItem childRight = _nodes[childRightIndex];
        if (HasHigherPriority(childRight, node)) {
          childRight.QueueIndex = finalQueueIndex;
          _nodes[finalQueueIndex] = childRight;
          finalQueueIndex = childRightIndex;
        }
        // Neither child is higher-priority than current, so finish and stop.
        else {
          return;
        }
      }

      while (true) {
        childLeftIndex = 2 * finalQueueIndex;

        // If leaf node, we're done
        if (childLeftIndex > _numNodes) {
          node.QueueIndex = finalQueueIndex;
          _nodes[finalQueueIndex] = node;
          break;
        }

        // Check if the left-child is higher-priority than the current node
        childRightIndex = childLeftIndex + 1;
        childLeft = _nodes[childLeftIndex];
        if (HasHigherPriority(childLeft, node)) {
          // Check if there is a right child. If not, swap and finish.
          if (childRightIndex > _numNodes) {
            node.QueueIndex = childLeftIndex;
            childLeft.QueueIndex = finalQueueIndex;
            _nodes[finalQueueIndex] = childLeft;
            _nodes[childLeftIndex] = node;
            break;
          }
          // Check if the left-child is higher-priority than the right-child
          TItem childRight = _nodes[childRightIndex];
          if (HasHigherPriority(childLeft, childRight)) {
            // left is highest, move it up and continue
            childLeft.QueueIndex = finalQueueIndex;
            _nodes[finalQueueIndex] = childLeft;
            finalQueueIndex = childLeftIndex;
          } else {
            // right is even higher, move it up and continue
            childRight.QueueIndex = finalQueueIndex;
            _nodes[finalQueueIndex] = childRight;
            finalQueueIndex = childRightIndex;
          }
        }
        // Not swapping with left-child, does right-child exist?
        else if (childRightIndex > _numNodes) {
          node.QueueIndex = finalQueueIndex;
          _nodes[finalQueueIndex] = node;
          break;
        } else {
          // Check if the right-child is higher-priority than the current node
          TItem childRight = _nodes[childRightIndex];
          if (HasHigherPriority(childRight, node)) {
            childRight.QueueIndex = finalQueueIndex;
            _nodes[finalQueueIndex] = childRight;
            finalQueueIndex = childRightIndex;
          }
          // Neither child is higher-priority than current, so finish and stop.
          else {
            node.QueueIndex = finalQueueIndex;
            _nodes[finalQueueIndex] = node;
            break;
          }
        }
      }
    }

    /// <summary>
    /// Returns true if 'higher' has higher priority than 'lower', false otherwise.
    /// Note that calling HasHigherPriority(node, node) (ie. both arguments the same node) will return false
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    private bool HasHigherPriority(TItem higher, TItem lower) {
      var cmp = _comparer(higher, lower);
      return (cmp < 0 || (cmp == 0 && higher.InsertionIndex < lower.InsertionIndex));
    }

    /// <summary>
    /// Removes the head of the queue (node with minimum priority; ties are broken by order of insertion), and returns it.
    /// If queue is empty, result is undefined
    /// O(log n)
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public TItem Dequeue() {
#if DEBUG
      if (_numNodes <= 0) {
        throw new InvalidOperationException("Cannot call Dequeue() on an empty queue");
      }

      if (!IsValidQueue()) {
        throw new InvalidOperationException("Queue has been corrupted (Did you update a node priority manually instead of calling UpdatePriority()?" +
                                            "Or add the same node to two different queues?)");
      }
#endif

      TItem returnMe = _nodes[1];
      //If the node is already the last node, we can remove it immediately
      if (_numNodes == 1) {
        _nodes[1] = null;
        _numNodes = 0;
        return returnMe;
      }

      //Swap the node with the last node
      TItem formerLastNode = _nodes[_numNodes];
      _nodes[1] = formerLastNode;
      formerLastNode.QueueIndex = 1;
      _nodes[_numNodes] = null;
      _numNodes--;

      //Now bubble formerLastNode (which is no longer the last node) down
      CascadeDown(formerLastNode);
      return returnMe;
    }

    /// <summary>
    /// Resize the queue so it can accept more nodes.  All currently enqueued nodes are remain.
    /// Attempting to decrease the queue size to a size too small to hold the existing nodes results in undefined behavior
    /// O(n)
    /// </summary>
    public void Resize(int maxNodes) {
#if DEBUG
      if (maxNodes <= 0) {
        throw new InvalidOperationException("Queue size cannot be smaller than 1");
      }

      if (maxNodes < _numNodes) {
        throw new InvalidOperationException("Called Resize(" + maxNodes + "), but current queue contains " + _numNodes + " nodes");
      }
#endif

      TItem[] newArray = new TItem[maxNodes + 1];
      int highestIndexToCopy = Math.Min(maxNodes, _numNodes);
      Array.Copy(_nodes, newArray, highestIndexToCopy + 1);
      _nodes = newArray;
    }

    /// <summary>
    /// Returns the head of the queue, without removing it (use Dequeue() for that).
    /// If the queue is empty, behavior is undefined.
    /// O(1)
    /// </summary>
    public TItem First {
      get {
#if DEBUG
        if (_numNodes <= 0) {
          throw new InvalidOperationException("Cannot call .First on an empty queue");
        }
#endif

        return _nodes[1];
      }
    }

    /// <summary>
    /// This method must be called on a node every time its priority changes while it is in the queue.  
    /// <b>Forgetting to call this method will result in a corrupted queue!</b>
    /// Calling this method on a node not in the queue results in undefined behavior
    /// O(log n)
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public void UpdatePriority(TItem node) {
#if DEBUG
      if (node == null) {
        throw new ArgumentNullException("node");
      }
      if (!Contains(node)) {
        throw new InvalidOperationException("Cannot call UpdatePriority() on a node which is not enqueued: " + node);
      }
#endif
      
      OnNodeUpdated(node);
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    private void OnNodeUpdated(TItem node) {
      //Bubble the updated node up or down as appropriate
      int parentIndex = node.QueueIndex >> 1;

      if (parentIndex > 0 && HasHigherPriority(node, _nodes[parentIndex])) {
        CascadeUp(node);
      } else {
        //Note that CascadeDown will be called if parentNode == node (that is, node is the root)
        CascadeDown(node);
      }
    }

    /// <summary>
    /// Removes a node from the queue.  The node does not need to be the head of the queue.  
    /// If the node is not in the queue, the result is undefined.  If unsure, check Contains() first
    /// O(log n)
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public void Remove(TItem node) {
#if DEBUG
      if (node == null) {
        throw new ArgumentNullException("node");
      }
      if (!Contains(node)) {
        throw new InvalidOperationException("Cannot call Remove() on a node which is not enqueued: " + node);
      }
#endif

      //If the node is already the last node, we can remove it immediately
      if (node.QueueIndex == _numNodes) {
        _nodes[_numNodes] = null;
        _numNodes--;
        return;
      }

      //Swap the node with the last node
      TItem formerLastNode = _nodes[_numNodes];
      _nodes[node.QueueIndex] = formerLastNode;
      formerLastNode.QueueIndex = node.QueueIndex;
      _nodes[_numNodes] = null;
      _numNodes--;

      //Now bubble formerLastNode (which is no longer the last node) up or down as appropriate
      OnNodeUpdated(formerLastNode);
    }

    public IEnumerator<TItem> GetEnumerator() {
      IEnumerable<TItem> e = new ArraySegment<TItem>(_nodes, 1, _numNodes);
      return e.GetEnumerator();
    }

    IEnumerator IEnumerable.GetEnumerator() {
      return GetEnumerator();
    }

    /// <summary>
    /// <b>Should not be called in production code.</b>
    /// Checks to make sure the queue is still in a valid state.  Used for testing/debugging the queue.
    /// </summary>
    public bool IsValidQueue() {
      for (int i = 1; i < _nodes.Length; i++) {
        if (_nodes[i] != null) {
          int childLeftIndex = 2 * i;
          if (childLeftIndex < _nodes.Length && _nodes[childLeftIndex] != null && HasHigherPriority(_nodes[childLeftIndex], _nodes[i]))
            return false;

          int childRightIndex = childLeftIndex + 1;
          if (childRightIndex < _nodes.Length && _nodes[childRightIndex] != null && HasHigherPriority(_nodes[childRightIndex], _nodes[i]))
            return false;
        }
      }
      return true;
    }
  }
}