public void Enqueue(NodeV5 node)
        {
            if (node == null)
            {
                throw new ArgumentNullException(nameof(node));
            }

            int newIndex = this.binaryHeap.Count;
            this.binaryHeap.Add(node);
            this.hashSet.Add(node);

            while (newIndex > 0)
            {
                int parentIndex = (newIndex - 1) / 2;
                if (this.binaryHeap[parentIndex].Cost <= node.Cost)
                {
                    break;
                }

                this.binaryHeap[newIndex] = this.binaryHeap[parentIndex];
                newIndex = parentIndex;
            }

            this.binaryHeap[newIndex] = node;
        }
 private static IEnumerable<Step> GetPathFrom(NodeV5 goalNode1)
 {
     for (NodeV5 current = goalNode1; current.ParentNode != null && current.PreviousStep != null; current = current.ParentNode)
     {
         yield return current.PreviousStep.Value;
     }
 }
        public bool Contains(NodeV5 node)
        {
            if (node == null)
            {
                throw new ArgumentNullException(nameof(node));
            }

            return this.hashSet.Contains(node);
        }