public void TestPriorityQueue()
        {
            int operationsCount = 100000;

            Random rand = new Random(0);

            PriorityQueue<double> queue = new PriorityQueue<double>();

            for (int op = 0; op < operationsCount; ++op)
            {
                int opType = rand.Next(0, 2);

                if (opType == 0) // Enqueue
                {
                    double item = (100.0 - 1.0) * rand.NextDouble() + 1.0;
                    queue.Enqueue(item);

                    Assert.IsTrue(queue.IsConsistent(), "Test fails after enqueue operation # " + op);
                }
                else // Dequeue
                {
                    if (queue.Count > 0)
                    {
                        double item = queue.Dequeue();
                        Assert.IsTrue(queue.IsConsistent(), "Test fails after dequeue operation # " + op);
                    }
                }
            }
        }

        public void CorrectPriorityTest2()
        {
            PriorityQueue<int> queue = new PriorityQueue<int>();

            queue.Enqueue(4);
            queue.Enqueue(1);
            queue.Enqueue(3);
            queue.Enqueue(2);
            queue.Enqueue(16);
            queue.Enqueue(9);
            queue.Enqueue(10);
            queue.Enqueue(14);
            queue.Enqueue(8);
            queue.Enqueue(7);

            Assert.AreEqual(10, queue.Count);
            Assert.AreEqual(16, queue.Dequeue());
            Assert.AreEqual(14, queue.Dequeue());
            Assert.AreEqual(10, queue.Dequeue());
            Assert.AreEqual(9, queue.Dequeue());
            Assert.AreEqual(8, queue.Dequeue());
            Assert.AreEqual(7, queue.Dequeue());
            Assert.AreEqual(4, queue.Dequeue());
            Assert.AreEqual(3, queue.Dequeue());
            Assert.AreEqual(2, queue.Dequeue());
            Assert.AreEqual(1, queue.Dequeue());
        }

    /* 1 Implement a class PriorityQueue<T> based
     * on the data structure "binary heap".
     * */
    static void Main(string[] args)
    {
        var heap = new Heap<int>();
        heap.Add(1);
        heap.Add(2);
        heap.Add(3);

        Debug.Assert(heap.SameContents(new[] { 1, 2, 3 }));
        Console.WriteLine(string.Join(",", heap));

        Debug.Assert(heap.ChopHead() == 3);
        Debug.Assert(heap.ChopHead() == 2);
        Debug.Assert(heap.ChopHead() == 1);
        Debug.Assert(heap.IsEmpty);

        // higher string means lower priority
        var pqueue = new PriorityQueue<string, string>((s1, s2) => -s1.CompareTo(s2));

        pqueue.Enqueue("18:00", "Buy food");
        pqueue.Enqueue("06:00", "Walk dog");
        pqueue.Enqueue("21:00", "Do homework");
        pqueue.Enqueue("09:00", "Go to work");
        pqueue.Enqueue("21:00", "Drink beer");

        Debug.Assert(pqueue.Count == 5);

        Debug.Assert(pqueue.Dequeue() == "Walk dog");
        Debug.Assert(pqueue.Dequeue() == "Go to work");
        Debug.Assert(pqueue.Dequeue() == "Buy food");
        Debug.Assert(new[] { "Do homework", "Drink beer" }.Contains(pqueue.Dequeue()));
        Debug.Assert(new[] { "Do homework", "Drink beer" }.Contains(pqueue.Dequeue()));
    }

    // Dijkstra's shortest paths algorithm, implemented
    // with priority queue. Running time: O(M * log M)
    // Learn more at: https://en.wikipedia.org/wiki/Dijkstra%27s_algorithm#Using_a_priority_queue
    public static void DijkstraAlgorithm(
        Dictionary<Node, List<Edge>> graph, Node sourceNode)
    {
        var queue = new PriorityQueue<Node>();

        foreach (var node in graph)
        {
            node.Key.Distance = double.PositiveInfinity;
        }
        sourceNode.Distance = 0.0d;
        queue.Enqueue(sourceNode);

        while (queue.Count != 0)
        {
            var currentNode = queue.Dequeue();

            if (double.IsPositiveInfinity(currentNode.Distance))
            {
                // All nodes processed --> algorithm finished
                break;
            }

            foreach (var childEdge in graph[currentNode])
            {
                var newDistance = currentNode.Distance + childEdge.Distance;
                if (newDistance < childEdge.Node.Distance)
                {
                    childEdge.Node.Distance = newDistance;
                    childEdge.Node.PreviousNode = currentNode;
                    queue.Enqueue(childEdge.Node);
                }
            }
        }
    }

    static void Main()
    {
        Console.WriteLine("Creating an instance of the custom PriorityQueue class and adding 8 integers to it in non-increasing randomized order.");
        var priorityQueue = new PriorityQueue<int>();
        priorityQueue.Enqueue(5);
        priorityQueue.Enqueue(3);
        priorityQueue.Enqueue(2);
        priorityQueue.Enqueue(5);
        priorityQueue.Enqueue(15);
        priorityQueue.Enqueue(6);
        priorityQueue.Enqueue(0);
        priorityQueue.Enqueue(-8);

        Console.WriteLine("Printing the queue:");
        PrintQueue(priorityQueue);

        Console.WriteLine("Dequeuing and displaying the first 5 elements");
        for (int i = 0; i < 5; i++)
        {
            Console.WriteLine("Dequeuing the number {0}", priorityQueue.Dequeue());
        }

        Console.WriteLine("Printing the queue after the removal:");
        PrintQueue(priorityQueue);

        Console.WriteLine("Adding 5 more integers to the queue in non-increasing randomized order.");
        priorityQueue.Enqueue(9);
        priorityQueue.Enqueue(20);
        priorityQueue.Enqueue(13);
        priorityQueue.Enqueue(2);
        priorityQueue.Enqueue(7);

        Console.WriteLine("Printing the queue after the addition:");
        PrintQueue(priorityQueue);
    }

    public static Path<Tile> FindPath(
        Tile start,
        Tile destination)
    {
        var closed = new HashSet<Tile>();
        var queue = new PriorityQueue<double, Path<Tile>>();
        queue.Enqueue(0, new Path<Tile>(start));

        while (!queue.IsEmpty)
        {
            var path = queue.Dequeue();

            if (closed.Contains(path.LastStep))
                continue;
            if (path.LastStep.Equals(destination))
                return path;

            closed.Add(path.LastStep);

            foreach (Tile n in path.LastStep.Neighbours)
            {
                double d = distance(path.LastStep, n);
                var newPath = path.AddStep(n, d);
                queue.Enqueue(newPath.TotalCost + estimate(n, destination), 
                    newPath);
            }
        }

        return null;
    }

        static ICollection<Node> PrimAlgorithm(Node startNode)
        {
            var mst = new List<Node>();
            PriorityQueue queue = new PriorityQueue();
            startNode.Used = true;

            mst.Add(startNode);
            foreach (var connection in neighbourhood[startNode])
            {
                queue.Enqueue(connection);
            }

            while (mst.Count != houses.Count)
            {
                var current = queue.Dequeue();

                current.ToNode.Used = true;
                mst.Add(current.ToNode);

                foreach (var connection in neighbourhood[current.ToNode])
                {
                    if (!connection.ToNode.Used)
                    {
                        queue.Enqueue(connection);
                    }
                }
            }

            return mst;
        }

    private static void Main()
    {
        var items = new[] { 2, 6, 3, 2, 1, 7, 4, 9, 5, 1, 8 };
        Console.WriteLine("Items: [{0}]", string.Join(", ", items));


        // Priority queue of integers, where a lower number means higher priority
        var queue = new PriorityQueue<int>();        

        // Add each item to the priority queue and 
        // check if the item with the highest priority is at the top of the queue
        var minItem = int.MaxValue;
        foreach (var item in items)
        {
            queue.Enqueue(item);
            minItem = Math.Min(item, minItem);            
            Debug.Assert(queue.Peek() == minItem);
        }

        // Now check if after each dequeue, the items come out ranked by priority
        var sorted = new List<int>();
        while (queue.Count > 0)
        {
            sorted.Add(queue.Dequeue());
        }

        // Items should be sorted in ascending order
        Console.WriteLine("Queue items: [{0}]", string.Join(", ", sorted));
    }

    public PriorityQueue<IHearable> getObjectsObservableBy(IHearing listener)
    {
        //Brute force approach = O(n)
        //For all hearing objects it's O(m*n) ~ Acceptable.
        PriorityQueue<IHearable> queue = new PriorityQueue<IHearable>(true);
        int i;

        IHearable target;
        for (i=0;i<hearableObjects.Count;++i)
        {
            double distanceSquared,priority; //Hopefully this will be optimized out.

            target = hearableObjects[i];

            //Calculates Distance^2
            distanceSquared = (listener.getLocation() - target.getLocation()).sqrMagnitude;

            //Store in variable for use as queue priority
            priority = target.getVolume().volumeFromDistanceSquared(distanceSquared).Intensity;

            //Put in queue if louder than hearing threshold.
            //Debug.Log(Volume.fromIntensity(priority).Decibels + " " + target.getGameObject().name);
            if (priority >= listener.getHearingThreshold().Intensity)
            {
                //Debug.Log(Volume.fromIntensity(priority).Decibels + " " + target.getGameObject().name);
                queue.enqueueWithPriority(target,priority);
            }
        }

        return queue;
    }

        public Route GetShortestPath(Vertex from, Vertex to)
        {
            ResetExploration();

            var queue = new PriorityQueue<double, Route>();

            queue.Enqueue(0, new Route { @from });
            while (!queue.IsEmpty)
            {
                var route = queue.Dequeue();
                if (route.Last().IsExplored)
                    continue;

                if (route.Last().Equals(to))
                    return route;

                route.Last().IsExplored = true;
                foreach (Edge n in route.Last().AdjacentEdges)
                {
                    var newRoute = route.Clone();
                    newRoute.Add(n.Tail);
                    queue.Enqueue(newRoute.Cost, newRoute);
                }
            }
            return null;
        }

    public static void DijkstraAlgorithm(Graph graph, Node source)
    {
        foreach (var node in graph)
        {
            node.MinDistance = double.PositiveInfinity;
        }

        source.MinDistance = 0;

        var pQueue = new PriorityQueue<Node>();
        pQueue.Enqueue(source);

        while (pQueue.Count != 0)
        {
            Node currentNode = pQueue.Dequeue();

            foreach (var neighbour in graph[currentNode.Id].Neighbors)
            {
                double newDist = currentNode.MinDistance + neighbour.Distance;

                if (newDist < neighbour.Node.MinDistance)
                {
                    neighbour.Node.MinDistance = newDist;
                    pQueue.Enqueue(neighbour.Node);
                }
            }
        }
    }

        public static void FindMinimalPaths(Dictionary<Node, List<Street>> graph, Node source)
        {
            PriorityQueue<Node> queue = new PriorityQueue<Node>();

            foreach (var node in graph)
            {
                if (source.ID != node.Key.ID)
                {
                    node.Key.DijkstraDistance = ulong.MaxValue;
                }
            }

            source.DijkstraDistance = 0;
            queue.Enqueue(source);

            while (queue.Count != 0)
            {
                Node currentNode = queue.Dequeue();

                foreach (var neighbour in graph[currentNode])
                {
                    ulong potDistance = currentNode.DijkstraDistance + neighbour.Distance;

                    if (potDistance < neighbour.Node.DijkstraDistance)
                    {
                        neighbour.Node.DijkstraDistance = potDistance;
                        // adds only modified elements in the queue
                        // thus reordering the queue after each iteration is avoided
                        queue.Enqueue(neighbour.Node);
                    }
                }
            }
        }

        static void Main(string[] args)
        {
            IQueue CommanderDispatcherMessageQueue = new PriorityQueue(30);
            IQueue DispatcherSerialMessageQueue = new PriorityQueue(30);
            //IQueue SerialStatusMessageQueue = new PriorityQueue(30);
               // IQueue StatusCommanderMessageQueue = new PriorityQueue(30);
            //IQueue MicrocontrollerCommanderMessageBox = new PriorityQueue(30);

            Thread dispatcher = new Thread(() => Dispatcher(CommanderDispatcherMessageQueue));
            //Thread serialManager = new Thread(() => SerialManager(DispatcherSerialMessageQueue, MicrocontrollerCommanderMessageBox));
            //Thread statusUpdater = new Thread(() => StatusUpdater(SerialStatusMessageQueue));
            //Thread commandSender = new Thread(() => CommandSender(MicrocontrollerCommanderMessageBox));

            RoverCameraFactory.GetInstance().Initialize(Properties.NetworkSettings.Default.OperatorIPAddress, Properties.NetworkSettings.Default.CameraBasePort);

            dispatcher.Start();
            //serialManager.Start();
            //statusUpdater.Start();
               // commandSender.Start();

            //Start the commands listener
            var commandsListener = new GuardedMessageListener(
                Properties.NetworkSettings.Default.CommandsPort,
                CommanderDispatcherMessageQueue,
                Properties.NetworkSettings.Default.OperatorIPAddress,
                new WatchDog());
            commandsListener.StartListening();
        }

        static void Main()
        {
            var bag = new PriorityQueue<Car>();

            var opel = new Car
            {
                Model = "Opel",
                Price = 1000
            };
            var mercedes = new Car
            {
                Model = "Mercedes",
                Price = 5000
            };

            var citroen = new Car
            {
                Model = "Citroen",
                Price = 3000
            };

            bag.Enqueue(opel);
            bag.Enqueue(mercedes);
            bag.Enqueue(citroen);

            while(bag.Count > 0)
            {
                var car = bag.Dequeue();
                Console.WriteLine("{0} -> {1}",car.Model, car.Price);
            }
        }

 static void Main(string[] args)
 {
     PriorityQueue queue = new PriorityQueue();
     queue.Enqueue("Name1", Priority.Low);
     queue.Enqueue("Name2", Priority.Low);
     queue.Enqueue("Name3", Priority.Low);
     queue.Enqueue("Name4", Priority.Low);
     queue.Enqueue("Name5", Priority.Low);
     queue.Enqueue("Name6", Priority.Low);
     queue.Enqueue("Name7", Priority.Low);
     queue.Enqueue("Name8", Priority.Low);
     queue.Enqueue("Name9", Priority.Low);
     Console.WriteLine("There are " + queue.Length + " people in line.");
     Console.WriteLine("Person in front: " + queue.PeekAtFrontName() + " with priority " + queue.PeekAtFrontPriority());
     queue.Dequeue();
     Console.WriteLine("Person in front: " + queue.PeekAtFrontName() + " with priority " + queue.PeekAtFrontPriority());
     queue.Dequeue();
     Console.WriteLine("Person in front: " + queue.PeekAtFrontName() + " with priority " + queue.PeekAtFrontPriority());
     queue.Enqueue("Name10", Priority.Medium);
     Console.WriteLine("Person in front: " + queue.PeekAtFrontName() + " with priority " + queue.PeekAtFrontPriority());
     queue.Enqueue("Name11", Priority.Medium);
     Console.WriteLine("Person in front: " + queue.PeekAtFrontName() + " with priority " + queue.PeekAtFrontPriority());
     queue.Dequeue();
     queue.Enqueue("Dad", Priority.High);
     Console.WriteLine("Person in front: " + queue.PeekAtFrontName() + " with priority " + queue.PeekAtFrontPriority());
 }

        public void TestEnqueueDequeue()
        {
            PriorityQueue<int> queue = new PriorityQueue<int>();
            queue.enqueueWithPriority (16,4);
            queue.enqueueWithPriority (14,5);
            queue.enqueueWithPriority (25,3);
            queue.enqueueWithPriority (17,4);

            Assert.AreEqual (queue.dequeue (), 14);
            Assert.AreEqual (queue.dequeue (), 16);
            Assert.AreEqual (queue.dequeue (), 17);
            Assert.AreEqual (queue.dequeue (), 25);

            queue.enqueueWithPriority(11,1);

            Assert.AreEqual (queue.dequeue (), 11);

            try {
                Console.Out.WriteLine("Attempting to dequeue an empty queue.");
                queue.dequeue();
                Console.Error.WriteLine("FAIL: System.InvalidOperationExcepti"+
                             "on was not thrown when dequeing an empty queue");
                Assert.IsTrue(false);
            } catch (System.InvalidOperationException e)
            {
                Console.Out.WriteLine("PASS: System.InvalidOperationException"+
                                            " thrown as expected: "+e.Message);
                Assert.IsTrue(true,"Exception thrown");
            }
        }

 /// <summary>Returns a synchronized wrapper around the queue.</summary>
 /// <param name="queue">The queue to be synchronized.</param>
 /// <returns>A synchronized priority queue.</returns>
 public static PriorityQueue Synchronize(PriorityQueue queue)
 {
     // Return the queue if it is already synchronized.  Otherwise, wrap it
     // with a synchronized wrapper.
     if (queue is SyncPriorityQueue) return queue;
     return new SyncPriorityQueue(queue);
 }

        public void PriorityQueue_Dequeue_NonRepeatedValues_Success()
        {
            // Arrange
            var priorityQueue = new PriorityQueue<int>();
            var values = new int[] { 10, 7, 6, 1, 2, 3, 5, 4, 9, 8 };
            var expected = "1,2,3,4,5,6,7,8,9,10";

            // Act
            foreach (var value in values)
            {
                priorityQueue.Enqueue(value);
            }

            var result = "";
            var index = 0;
            var lastIndex = priorityQueue.Count() - 1;

            while (!priorityQueue.IsEmpty())
            {
                var element = priorityQueue.Dequeue();

                result += index++ == lastIndex ?
                    string.Format("{0}", element.ToString()) : string.Format("{0},", element.ToString());
            }

            // Assert
            Assert.AreEqual(result, expected);
        }

 public void RandomTest()
 {
     PriorityQueue<int> TestObject = new PriorityQueue<int>();
     Utilities.Random.Random Rand = new Utilities.Random.Random();
     int Value=0;
     for (int x = 0; x < 10; ++x)
     {
         Value=Rand.Next();
         TestObject.Add(x, Value);
         Assert.Equal(Value, TestObject.Peek());
     }
     int HighestValue = TestObject.Peek();
     for (int x = 9; x >= 0; --x)
     {
         Value = Rand.Next();
         TestObject.Add(x, Value);
         Assert.Equal(HighestValue, TestObject.Peek());
     }
     int Count=0;
     foreach(int Priority in TestObject.Keys)
     {
         foreach(int Item in TestObject[Priority])
         {
             ++Count;
         }
     }
     Assert.Equal(20, Count);
 }

        public void WithPriority()
        {
            var priorityQueue = new PriorityQueue<string, int>(PriorityQueueType.Maximum);
            priorityQueue.Enqueue("dog", 2);
            priorityQueue.Enqueue("canary", 1);
            priorityQueue.Enqueue("cat", 3);

            int priority;

            // Peek gives us "cat"
            Assert.AreEqual("cat", priorityQueue.Dequeue(out priority));

            // With priority 3
            Assert.AreEqual(priority, 3);

            // Peek gives us "dog"
            Assert.AreEqual("dog", priorityQueue.Dequeue(out priority));

            // With priority 2
            Assert.AreEqual(priority, 2);

            // Peek gives us "canary"
            Assert.AreEqual("canary", priorityQueue.Dequeue(out priority));

            // With priority 1
            Assert.AreEqual(priority, 1);
        }