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
+          return if index
-            @edge_nodes[index] = node
+
-          else
+          @edge_nodes.push(node)
-            @edge_nodes.push(node)
-          end
         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

lib/cyberarm_engine/pathfinding/graph_searches/breadth_first_search.rb

@@ -1,11 +1,11 @@
 module CyberarmEngine
   module Pathfinding
     class BreadthFirstSearch < GraphSearch
-      def search(graph:, start:, goal: nil)
+      def self.search(graph:, start:, goal: nil)
         frontier = []
         came_from = {}
 
-        frontier.put(start)
+        frontier.push(start)
         came_from[start.key] = start
 
         while(current = frontier.shift)
@@ -15,7 +15,7 @@ module CyberarmEngine
           graph.neighbors(current).each do |neighbor|
             next if came_from[neighbor.key]
 
-            frontier.put(neighbor)
+            frontier.push(neighbor)
             came_from[neighbor.key] = current
           end
         end

lib/cyberarm_engine/pathfinding/graphs/grid.rb

@@ -1,10 +1,9 @@
 module CyberarmEngine
   module Pathfinding
     class Grid < Graph
-
       def add_node(x:, y:, z: 0, cost: 0, replace: false)
         key = format(Node::KEY_FORMAT, x, y, z)
-        node = @node[key]
+        node = @nodes[key]
 
         raise "Node at: #{key} already exists! (#{node})" if node && !replace
 
@@ -12,6 +11,7 @@ module CyberarmEngine
         @nodes[key] = node
 
         update_node_edge_nodes(node)
+        node
       end
 
       def remove_node(node)
@@ -35,12 +35,18 @@ module CyberarmEngine
         front = @nodes[format(Node::KEY_FORMAT, x, y, z - 1)]
         back = @nodes[format(Node::KEY_FORMAT, x, y, z + 1)]
 
+        possible_edge_nodes = [left, right, top, bottom, front, back]
+
         if remove_node
-          [left, right, top, bottom, front, back].remove_edge_node(node)
+          possible_edge_nodes.each { |n| n&.remove_edge_node(node) }
         else
-          [left, right, top, bottom, front, back].add_edge_node(node)
+          possible_edge_nodes.each { |n| n&.add_edge_node(node) }
         end
       end
+
+      def update_edge_nodes
+        @nodes.each { |k, n| update_node_edge_nodes(n) }
+      end
     end
   end
 end