Fix up some errors with BFS and pathfinding grid

lib/cyberarm_engine/pathfinding/graph.rb

@@ -12,7 +12,7 @@ module CyberarmEngine
           @z = z
           @cost = cost
 
-          @key = format(KEY_FORMAT, @x, @y, @z)
+          @key = format(KEY_FORMAT, @x, @y, @z).freeze
 
           @edge_nodes = []
         end
@@ -20,11 +20,9 @@ module CyberarmEngine
         def add_edge_node(node)
           index = @edge_nodes.find_index { |n| n.key == node.key }
 
-          if index
-            @edge_nodes[index] = node
-          else
-            @edge_nodes.push(node)
-          end
+          return if index
+
+          @edge_nodes.push(node)
         end
 
         def remove_edge_node(node)
@@ -34,12 +32,30 @@ module CyberarmEngine
 
           @edge_nodes.delete(index)
         end
+
+        def inspect
+          format "#<%<class>s:%<id>#018x @key=%<key>s @cost=%<cost>s @edge_nodes=%<edge_nodes>s>",
+                 class: self.class,
+                 id: object_id << 1,
+                 key: key.inspect,
+                 cost: cost.inspect,
+                 edge_nodes: edge_nodes.map { |n| n&.key }
+        end
       end
 
       def initialize(nodes: {})
         @nodes = nodes
       end
 
+      def node(key)
+        @nodes[key]
+      end
+
+      def node_at(x, y, z = 0)
+        key = format(Node::KEY_FORMAT, x, y, z)
+        @nodes[key]
+      end
+
       def neighbors(node)
         node.edge_nodes
       end