#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.Linq;

namespace SimSharp {
  /// <summary>
  /// A simplified priority queue implementation.  Is stable, auto-resizes, and thread-safe, at the cost of being slightly slower than
  /// FastPriorityQueue
  /// Methods tagged as O(1) or O(log n) are assuming there are no duplicates.  Duplicates may increase the algorithmic complexity.
  /// </summary>
  /// <remarks>
  /// Original sources from https://github.com/BlueRaja/High-Speed-Priority-Queue-for-C-Sharp
  /// </remarks>
  /// <typeparam name="TItem">The type to enqueue</typeparam>
  /// <typeparam name="TPriority">The priority-type to use for nodes.  Must extend IComparable&lt;TPriority&gt;</typeparam>
  public class SimplePriorityQueue<TItem, TPriority> : IEnumerable<TItem>
      where TPriority : IComparable<TPriority> {
    private class SimpleNode : GenericPriorityQueueNode<TPriority> {
      public TItem Data { get; private set; }

      public SimpleNode(TItem data) {
        Data = data;
      }
    }

    private const int INITIAL_QUEUE_SIZE = 64;
    private readonly GenericPriorityQueue<SimpleNode, TPriority> _queue;
    private readonly Dictionary<TItem, IList<SimpleNode>> _itemToNodesCache;
    private readonly IList<SimpleNode> _nullNodesCache;

    /// <summary>
    /// Instantiate a new Priority Queue
    /// </summary>
    public SimplePriorityQueue() : this(Comparer<TPriority>.Default) { }

    /// <summary>
    /// Instantiate a new Priority Queue
    /// </summary>
    /// <param name="comparer">The comparer used to compare TPriority values.  Defaults to Comparer&lt;TPriority&gt;.default</param>
    public SimplePriorityQueue(IComparer<TPriority> comparer) : this(comparer.Compare) { }

    /// <summary>
    /// Instantiate a new Priority Queue
    /// </summary>
    /// <param name="comparer">The comparison function to use to compare TPriority values</param>
    public SimplePriorityQueue(Comparison<TPriority> comparer) {
      _queue = new GenericPriorityQueue<SimpleNode, TPriority>(INITIAL_QUEUE_SIZE, comparer);
      _itemToNodesCache = new Dictionary<TItem, IList<SimpleNode>>();
      _nullNodesCache = new List<SimpleNode>();
    }

    /// <summary>
    /// Given an item of type T, returns the exist SimpleNode in the queue
    /// </summary>
    private SimpleNode GetExistingNode(TItem item) {
      if (item == null) {
        return _nullNodesCache.Count > 0 ? _nullNodesCache[0] : null;
      }

      IList<SimpleNode> nodes;
      if (!_itemToNodesCache.TryGetValue(item, out nodes)) {
        return null;
      }
      return nodes[0];
    }

    /// <summary>
    /// Adds an item to the Node-cache to allow for many methods to be O(1) or O(log n)
    /// </summary>
    private void AddToNodeCache(SimpleNode node) {
      if (node.Data == null) {
        _nullNodesCache.Add(node);
        return;
      }

      IList<SimpleNode> nodes;
      if (!_itemToNodesCache.TryGetValue(node.Data, out nodes)) {
        nodes = new List<SimpleNode>();
        _itemToNodesCache[node.Data] = nodes;
      }
      nodes.Add(node);
    }

    /// <summary>
    /// Removes an item to the Node-cache to allow for many methods to be O(1) or O(log n) (assuming no duplicates)
    /// </summary>
    private void RemoveFromNodeCache(SimpleNode node) {
      if (node.Data == null) {
        _nullNodesCache.Remove(node);
        return;
      }

      IList<SimpleNode> nodes;
      if (!_itemToNodesCache.TryGetValue(node.Data, out nodes)) {
        return;
      }
      nodes.Remove(node);
      if (nodes.Count == 0) {
        _itemToNodesCache.Remove(node.Data);
      }
    }

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


    /// <summary>
    /// Returns the head of the queue, without removing it (use Dequeue() for that).
    /// Throws an exception when the queue is empty.
    /// O(1)
    /// </summary>
    public TItem First {
      get {
        if (_queue.Count <= 0) {
          throw new InvalidOperationException("Cannot call .First on an empty queue");
        }

        return _queue.First.Data;
      }
    }

    /// <summary>
    /// Returns the top priority of the queue.
    /// Throws an exception when the queue is empty.
    /// O(1)
    /// </summary>
    public TPriority Peek {
      get {
        lock (_queue) {
          if (_queue.Count <= 0) {
            throw new InvalidOperationException("Cannot call .Peek on an empty queue");
          }
          return _queue.First.Priority;
        }
      }
    }

    /// <summary>
    /// Removes every node from the queue.
    /// O(n)
    /// </summary>
    public void Clear() {
      _queue.Clear();
      _itemToNodesCache.Clear();
      _nullNodesCache.Clear();
    }

    /// <summary>
    /// Returns whether the given item is in the queue.
    /// O(1)
    /// </summary>
    public bool Contains(TItem item) {
      if (item == null) {
        return _nullNodesCache.Count > 0;
      }
      return _itemToNodesCache.ContainsKey(item);
    }

    /// <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, throws an exception
    /// O(log n)
    /// </summary>
    public TItem Dequeue() {
      if (_queue.Count <= 0) {
        throw new InvalidOperationException("Cannot call Dequeue() on an empty queue");
      }

      SimpleNode node = _queue.Dequeue();
      RemoveFromNodeCache(node);
      return node.Data;
    }

    /// <summary>
    /// Enqueue the item with the given priority, without calling AddToNodeCache(node)
    /// </summary>
    /// <param name="item"></param>
    /// <param name="priority"></param>
    /// <returns></returns>
    private SimpleNode EnqueueNoCache(TItem item, TPriority priority) {
      SimpleNode node = new SimpleNode(item);
      if (_queue.Count == _queue.MaxSize) {
        _queue.Resize(_queue.MaxSize * 2 + 1);
      }
      _queue.Enqueue(node, priority);
      return node;
    }

    /// <summary>
    /// Enqueue a node to the priority queue.  Lower values are placed in front. Ties are broken by first-in-first-out.
    /// This queue automatically resizes itself, so there's no concern of the queue becoming 'full'.
    /// Duplicates and null-values are allowed.
    /// O(log n)
    /// </summary>
    public void Enqueue(TItem item, TPriority priority) {
      IList<SimpleNode> nodes;
      if (item == null) {
        nodes = _nullNodesCache;
      } else if (!_itemToNodesCache.TryGetValue(item, out nodes)) {
        nodes = new List<SimpleNode>();
        _itemToNodesCache[item] = nodes;
      }
      SimpleNode node = EnqueueNoCache(item, priority);
      nodes.Add(node);
    }

    /// <summary>
    /// Enqueue a node to the priority queue if it doesn't already exist.  Lower values are placed in front. Ties are broken by first-in-first-out.
    /// This queue automatically resizes itself, so there's no concern of the queue becoming 'full'.  Null values are allowed.
    /// Returns true if the node was successfully enqueued; false if it already exists.
    /// O(log n)
    /// </summary>
    public bool EnqueueWithoutDuplicates(TItem item, TPriority priority) {
      IList<SimpleNode> nodes;
      if (item == null) {
        if (_nullNodesCache.Count > 0) {
          return false;
        }
        nodes = _nullNodesCache;
      } else if (_itemToNodesCache.ContainsKey(item)) {
        return false;
      } else {
        nodes = new List<SimpleNode>();
        _itemToNodesCache[item] = nodes;
      }
      SimpleNode node = EnqueueNoCache(item, priority);
      nodes.Add(node);
      return true;
    }

    /// <summary>
    /// Removes an item from the queue.  The item does not need to be the head of the queue.  
    /// If the item is not in the queue, an exception is thrown.  If unsure, check Contains() first.
    /// If multiple copies of the item are enqueued, only the first one is removed. 
    /// O(log n)
    /// </summary>
    public void Remove(TItem item) {
      SimpleNode removeMe;
      IList<SimpleNode> nodes;
      if (item == null) {
        if (_nullNodesCache.Count == 0) {
          throw new InvalidOperationException("Cannot call Remove() on a node which is not enqueued: " + item);
        }
        removeMe = _nullNodesCache[0];
        nodes = _nullNodesCache;
      } else {
        if (!_itemToNodesCache.TryGetValue(item, out nodes)) {
          throw new InvalidOperationException("Cannot call Remove() on a node which is not enqueued: " + item);
        }
        removeMe = nodes[0];
        if (nodes.Count == 1) {
          _itemToNodesCache.Remove(item);
        }
      }
      _queue.Remove(removeMe);
      nodes.Remove(removeMe);
    }

    /// <summary>
    /// Call this method to change the priority of an item.
    /// Calling this method on a item not in the queue will throw an exception.
    /// If the item is enqueued multiple times, only the first one will be updated.
    /// (If your requirements are complex enough that you need to enqueue the same item multiple times <i>and</i> be able
    /// to update all of them, please wrap your items in a wrapper class so they can be distinguished).
    /// O(log n)
    /// </summary>
    public void UpdatePriority(TItem item, TPriority priority) {
      SimpleNode updateMe = GetExistingNode(item);
      if (updateMe == null) {
        throw new InvalidOperationException("Cannot call UpdatePriority() on a node which is not enqueued: " + item);
      }
      _queue.UpdatePriority(updateMe, priority);
    }

    /// <summary>
    /// Returns the priority of the given item.
    /// Calling this method on a item not in the queue will throw an exception.
    /// If the item is enqueued multiple times, only the priority of the first will be returned.
    /// (If your requirements are complex enough that you need to enqueue the same item multiple times <i>and</i> be able
    /// to query all their priorities, please wrap your items in a wrapper class so they can be distinguished).
    /// O(1)
    /// </summary>
    public TPriority GetPriority(TItem item) {
      SimpleNode findMe = GetExistingNode(item);
      if (findMe == null) {
        throw new InvalidOperationException("Cannot call GetPriority() on a node which is not enqueued: " + item);
      }
      return findMe.Priority;
    }

    #region Try* methods for multithreading
    /// Get the head of the queue, without removing it (use TryDequeue() for that).
    /// Useful for multi-threading, where the queue may become empty between calls to Contains() and First
    /// Returns true if successful, false otherwise
    /// O(1)
    public bool TryFirst(out TItem first) {
      if (_queue.Count <= 0) {
        first = default(TItem);
        return false;
      }

      first = _queue.First.Data;
      return true;
    }

    /// <summary>
    /// Removes the head of the queue (node with minimum priority; ties are broken by order of insertion), and sets it to first.
    /// Useful for multi-threading, where the queue may become empty between calls to Contains() and Dequeue()
    /// Returns true if successful; false if queue was empty
    /// O(log n)
    /// </summary>
    public bool TryDequeue(out TItem first) {
      if (_queue.Count <= 0) {
        first = default(TItem);
        return false;
      }

      SimpleNode node = _queue.Dequeue();
      first = node.Data;
      RemoveFromNodeCache(node);
      return true;
    }

    /// <summary>
    /// Attempts to remove an item from the queue.  The item does not need to be the head of the queue.  
    /// Useful for multi-threading, where the queue may become empty between calls to Contains() and Remove()
    /// Returns true if the item was successfully removed, false if it wasn't in the queue.
    /// If multiple copies of the item are enqueued, only the first one is removed. 
    /// O(log n)
    /// </summary>
    public bool TryRemove(TItem item) {
      SimpleNode removeMe;
      IList<SimpleNode> nodes;
      if (item == null) {
        if (_nullNodesCache.Count == 0) {
          return false;
        }
        removeMe = _nullNodesCache[0];
        nodes = _nullNodesCache;
      } else {
        if (!_itemToNodesCache.TryGetValue(item, out nodes)) {
          return false;
        }
        removeMe = nodes[0];
        if (nodes.Count == 1) {
          _itemToNodesCache.Remove(item);
        }
      }
      _queue.Remove(removeMe);
      nodes.Remove(removeMe);
      return true;
    }

    /// <summary>
    /// Call this method to change the priority of an item.
    /// Useful for multi-threading, where the queue may become empty between calls to Contains() and UpdatePriority()
    /// If the item is enqueued multiple times, only the first one will be updated.
    /// (If your requirements are complex enough that you need to enqueue the same item multiple times <i>and</i> be able
    /// to update all of them, please wrap your items in a wrapper class so they can be distinguished).
    /// Returns true if the item priority was updated, false otherwise.
    /// O(log n)
    /// </summary>
    public bool TryUpdatePriority(TItem item, TPriority priority) {
      SimpleNode updateMe = GetExistingNode(item);
      if (updateMe == null) {
        return false;
      }
      _queue.UpdatePriority(updateMe, priority);
      return true;
    }

    /// <summary>
    /// Attempt to get the priority of the given item.
    /// Useful for multi-threading, where the queue may become empty between calls to Contains() and GetPriority()
    /// If the item is enqueued multiple times, only the priority of the first will be returned.
    /// (If your requirements are complex enough that you need to enqueue the same item multiple times <i>and</i> be able
    /// to query all their priorities, please wrap your items in a wrapper class so they can be distinguished).
    /// Returns true if the item was found in the queue, false otherwise
    /// O(1)
    /// </summary>
    public bool TryGetPriority(TItem item, out TPriority priority) {
      SimpleNode findMe = GetExistingNode(item);
      if (findMe == null) {
        priority = default(TPriority);
        return false;
      }
      priority = findMe.Priority;
      return true;
    }
    #endregion

    public IEnumerator<TItem> GetEnumerator() {
      return _queue.Select(x => x.Data).GetEnumerator();
    }

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

    public bool IsValidQueue() {
      // Check all items in cache are in the queue
      foreach (IList<SimpleNode> nodes in _itemToNodesCache.Values) {
        foreach (SimpleNode node in nodes) {
          if (!_queue.Contains(node)) {
            return false;
          }
        }
      }

      // Check all items in queue are in cache
      foreach (SimpleNode node in _queue) {
        if (GetExistingNode(node.Data) == null) {
          return false;
        }
      }

      // Check queue structure itself
      return _queue.IsValidQueue();
    }
  }

  /// <summary>
  /// A simplified priority queue implementation.  Is stable, auto-resizes, and thread-safe, at the cost of being slightly slower than
  /// FastPriorityQueue
  /// Methods tagged as O(1) or O(log n) are assuming there are no duplicates.  Duplicates may increase the algorithmic complexity.
  /// </summary>
  /// <typeparam name="TItem">The type to enqueue</typeparam>
  public class SimplePriorityQueue<TItem> : IEnumerable<TItem> where TItem : IComparable<TItem> {
    private class SimpleNode : GenericPriorityQueueNode, IComparable<SimpleNode>, IComparable {
      public TItem Data { get; private set; }
      private Comparison<TItem> _comparer;

      public SimpleNode(TItem data, Comparison<TItem> comparer) {
        Data = data;
        _comparer = comparer;
      }

      public int CompareTo(SimpleNode other) {
        if (other == null) return 1;
        return _comparer(Data, other.Data);
      }

      public int CompareTo(object obj) {
        if (obj is SimpleNode other) return CompareTo(other);
        if (obj == null) return 1;
        throw new ArgumentException("can only compare to objects of type SimpleNode.");
      }
    }

    private const int INITIAL_QUEUE_SIZE = 64;
    private readonly GenericPriorityQueue<SimpleNode> _queue;
    private readonly Dictionary<TItem, IList<SimpleNode>> _itemToNodesCache;
    private readonly IList<SimpleNode> _nullNodesCache;
    private readonly Comparison<TItem> _comparer;

    /// <summary>
    /// Instantiate a new Priority Queue
    /// </summary>
    public SimplePriorityQueue() : this(Comparer<TItem>.Default) { }

    /// <summary>
    /// Instantiate a new Priority Queue
    /// </summary>
    /// <param name="comparer">The comparer used to compare TPriority values.  Defaults to Comparer&lt;TPriority&gt;.default</param>
    public SimplePriorityQueue(IComparer<TItem> comparer) : this(comparer.Compare) { }

    /// <summary>
    /// Instantiate a new Priority Queue
    /// </summary>
    /// <param name="comparer">The comparison function to use to compare TPriority values</param>
    public SimplePriorityQueue(Comparison<TItem> comparer) {
      _comparer = comparer;
      // the inner priority queue will always use the default comparer which in turn makes use of the specified comparer
      _queue = new GenericPriorityQueue<SimpleNode>(INITIAL_QUEUE_SIZE, Comparer<SimpleNode>.Default);
      _itemToNodesCache = new Dictionary<TItem, IList<SimpleNode>>();
      _nullNodesCache = new List<SimpleNode>();
    }

    /// <summary>
    /// Given an item of type T, returns the exist SimpleNode in the queue
    /// </summary>
    private SimpleNode GetExistingNode(TItem item) {
      if (item == null) {
        return _nullNodesCache.Count > 0 ? _nullNodesCache[0] : null;
      }

      IList<SimpleNode> nodes;
      if (!_itemToNodesCache.TryGetValue(item, out nodes)) {
        return null;
      }
      return nodes[0];
    }

    /// <summary>
    /// Adds an item to the Node-cache to allow for many methods to be O(1) or O(log n)
    /// </summary>
    private void AddToNodeCache(SimpleNode node) {
      if (node.Data == null) {
        _nullNodesCache.Add(node);
        return;
      }

      IList<SimpleNode> nodes;
      if (!_itemToNodesCache.TryGetValue(node.Data, out nodes)) {
        nodes = new List<SimpleNode>();
        _itemToNodesCache[node.Data] = nodes;
      }
      nodes.Add(node);
    }

    /// <summary>
    /// Removes an item to the Node-cache to allow for many methods to be O(1) or O(log n) (assuming no duplicates)
    /// </summary>
    private void RemoveFromNodeCache(SimpleNode node) {
      if (node.Data == null) {
        _nullNodesCache.Remove(node);
        return;
      }

      IList<SimpleNode> nodes;
      if (!_itemToNodesCache.TryGetValue(node.Data, out nodes)) {
        return;
      }
      nodes.Remove(node);
      if (nodes.Count == 0) {
        _itemToNodesCache.Remove(node.Data);
      }
    }

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


    /// <summary>
    /// Returns the head of the queue, without removing it (use Dequeue() for that).
    /// Throws an exception when the queue is empty.
    /// O(1)
    /// </summary>
    public TItem First {
      get {
        if (_queue.Count <= 0) {
          throw new InvalidOperationException("Cannot call .First on an empty queue");
        }

        return _queue.First.Data;
      }
    }

    /// <summary>
    /// Removes every node from the queue.
    /// O(n)
    /// </summary>
    public void Clear() {
      _queue.Clear();
      _itemToNodesCache.Clear();
      _nullNodesCache.Clear();
    }

    /// <summary>
    /// Returns whether the given item is in the queue.
    /// O(1)
    /// </summary>
    public bool Contains(TItem item) {
      if (item == null) {
        return _nullNodesCache.Count > 0;
      }
      return _itemToNodesCache.ContainsKey(item);
    }

    /// <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, throws an exception
    /// O(log n)
    /// </summary>
    public TItem Dequeue() {
      if (_queue.Count <= 0) {
        throw new InvalidOperationException("Cannot call Dequeue() on an empty queue");
      }

      SimpleNode node = _queue.Dequeue();
      RemoveFromNodeCache(node);
      return node.Data;
    }

    /// <summary>
    /// Enqueue the item with the given priority, without calling AddToNodeCache(node)
    /// </summary>
    /// <param name="item"></param>
    /// <returns></returns>
    private SimpleNode EnqueueNoCache(TItem item) {
      SimpleNode node = new SimpleNode(item, _comparer);
      if (_queue.Count == _queue.MaxSize) {
        _queue.Resize(_queue.MaxSize * 2 + 1);
      }
      _queue.Enqueue(node);
      return node;
    }

    /// <summary>
    /// Enqueue a node to the priority queue.  Lower values are placed in front. Ties are broken by first-in-first-out.
    /// This queue automatically resizes itself, so there's no concern of the queue becoming 'full'.
    /// Duplicates and null-values are allowed.
    /// O(log n)
    /// </summary>
    public void Enqueue(TItem item) {
      IList<SimpleNode> nodes;
      if (item == null) {
        nodes = _nullNodesCache;
      } else if (!_itemToNodesCache.TryGetValue(item, out nodes)) {
        nodes = new List<SimpleNode>();
        _itemToNodesCache[item] = nodes;
      }
      SimpleNode node = EnqueueNoCache(item);
      nodes.Add(node);
    }

    /// <summary>
    /// Enqueue a node to the priority queue if it doesn't already exist.  Lower values are placed in front. Ties are broken by first-in-first-out.
    /// This queue automatically resizes itself, so there's no concern of the queue becoming 'full'.  Null values are allowed.
    /// Returns true if the node was successfully enqueued; false if it already exists.
    /// O(log n)
    /// </summary>
    public bool EnqueueWithoutDuplicates(TItem item) {
      IList<SimpleNode> nodes;
      if (item == null) {
        if (_nullNodesCache.Count > 0) {
          return false;
        }
        nodes = _nullNodesCache;
      } else if (_itemToNodesCache.ContainsKey(item)) {
        return false;
      } else {
        nodes = new List<SimpleNode>();
        _itemToNodesCache[item] = nodes;
      }
      SimpleNode node = EnqueueNoCache(item);
      nodes.Add(node);
      return true;
    }

    /// <summary>
    /// Removes an item from the queue.  The item does not need to be the head of the queue.  
    /// If the item is not in the queue, an exception is thrown.  If unsure, check Contains() first.
    /// If multiple copies of the item are enqueued, only the first one is removed. 
    /// O(log n)
    /// </summary>
    public void Remove(TItem item) {
      SimpleNode removeMe;
      IList<SimpleNode> nodes;
      if (item == null) {
        if (_nullNodesCache.Count == 0) {
          throw new InvalidOperationException("Cannot call Remove() on a node which is not enqueued: " + item);
        }
        removeMe = _nullNodesCache[0];
        nodes = _nullNodesCache;
      } else {
        if (!_itemToNodesCache.TryGetValue(item, out nodes)) {
          throw new InvalidOperationException("Cannot call Remove() on a node which is not enqueued: " + item);
        }
        removeMe = nodes[0];
        if (nodes.Count == 1) {
          _itemToNodesCache.Remove(item);
        }
      }
      _queue.Remove(removeMe);
      nodes.Remove(removeMe);
    }

    /// <summary>
    /// Call this method to change the priority of an item.
    /// Calling this method on a item not in the queue will throw an exception.
    /// If the item is enqueued multiple times, only the first one will be updated.
    /// (If your requirements are complex enough that you need to enqueue the same item multiple times <i>and</i> be able
    /// to update all of them, please wrap your items in a wrapper class so they can be distinguished).
    /// O(log n)
    /// </summary>
    public void UpdatePriority(TItem item) {
      SimpleNode updateMe = GetExistingNode(item);
      if (updateMe == null) {
        throw new InvalidOperationException("Cannot call UpdatePriority() on a node which is not enqueued: " + item);
      }
      _queue.UpdatePriority(updateMe);
    }

    #region Try* methods for multithreading
    /// Get the head of the queue, without removing it (use TryDequeue() for that).
    /// Useful for multi-threading, where the queue may become empty between calls to Contains() and First
    /// Returns true if successful, false otherwise
    /// O(1)
    public bool TryFirst(out TItem first) {
      if (_queue.Count <= 0) {
        first = default(TItem);
        return false;
      }

      first = _queue.First.Data;
      return true;
    }

    /// <summary>
    /// Removes the head of the queue (node with minimum priority; ties are broken by order of insertion), and sets it to first.
    /// Useful for multi-threading, where the queue may become empty between calls to Contains() and Dequeue()
    /// Returns true if successful; false if queue was empty
    /// O(log n)
    /// </summary>
    public bool TryDequeue(out TItem first) {
      if (_queue.Count <= 0) {
        first = default(TItem);
        return false;
      }

      SimpleNode node = _queue.Dequeue();
      first = node.Data;
      RemoveFromNodeCache(node);
      return true;
    }

    /// <summary>
    /// Attempts to remove an item from the queue.  The item does not need to be the head of the queue.  
    /// Useful for multi-threading, where the queue may become empty between calls to Contains() and Remove()
    /// Returns true if the item was successfully removed, false if it wasn't in the queue.
    /// If multiple copies of the item are enqueued, only the first one is removed. 
    /// O(log n)
    /// </summary>
    public bool TryRemove(TItem item) {
      SimpleNode removeMe;
      IList<SimpleNode> nodes;
      if (item == null) {
        if (_nullNodesCache.Count == 0) {
          return false;
        }
        removeMe = _nullNodesCache[0];
        nodes = _nullNodesCache;
      } else {
        if (!_itemToNodesCache.TryGetValue(item, out nodes)) {
          return false;
        }
        removeMe = nodes[0];
        if (nodes.Count == 1) {
          _itemToNodesCache.Remove(item);
        }
      }
      _queue.Remove(removeMe);
      nodes.Remove(removeMe);
      return true;
    }

    /// <summary>
    /// Call this method to change the priority of an item.
    /// Useful for multi-threading, where the queue may become empty between calls to Contains() and UpdatePriority()
    /// If the item is enqueued multiple times, only the first one will be updated.
    /// (If your requirements are complex enough that you need to enqueue the same item multiple times <i>and</i> be able
    /// to update all of them, please wrap your items in a wrapper class so they can be distinguished).
    /// Returns true if the item priority was updated, false otherwise.
    /// O(log n)
    /// </summary>
    public bool TryUpdatePriority(TItem item) {
      SimpleNode updateMe = GetExistingNode(item);
      if (updateMe == null) {
        return false;
      }
      _queue.UpdatePriority(updateMe);
      return true;
    }
    #endregion

    public IEnumerator<TItem> GetEnumerator() {
      return _queue.Select(x => x.Data).GetEnumerator();
    }

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

    public bool IsValidQueue() {
      // Check all items in cache are in the queue
      foreach (IList<SimpleNode> nodes in _itemToNodesCache.Values) {
        foreach (SimpleNode node in nodes) {
          if (!_queue.Contains(node)) {
            return false;
          }
        }
      }

      // Check all items in queue are in cache
      foreach (SimpleNode node in _queue) {
        if (GetExistingNode(node.Data) == null) {
          return false;
        }
      }

      // Check queue structure itself
      return _queue.IsValidQueue();
    }
  }
}