Added distance calculating and selecting directions

Traversal/BestFS.py

@@ -0,0 +1,72 @@
+from utilities import movement,check_moves
+from DataModels.House import House
+from DataModels.Container import Container
+from math import sqrt
+INF = float('Inf')
+
+
+def CalculateDistance(gc, goal):
+    result = sqrt(pow(goal[0]-gc[0],2)+pow(goal[1]-gc[1],2))
+    return result
+
+def BestFS(grid, available_movement, gc_moveset, houses_list, depth=0):
+
+    possible_goals = []
+    a = gc_moveset[-1][0]
+    b = gc_moveset[-1][1]
+    possible_goals.append([a+1,b])
+    possible_goals.append([a-1,b])
+    possible_goals.append([a,b+1])
+    possible_goals.append([a,b-1])
+    house_in_area = False
+    for location in possible_goals:
+        if location[0]>=0 and location[1]>=0:
+            try:
+                cell = grid[location[0]][location[1]]
+                if(type(cell) == House and cell.container.is_full and cell.unvisited):
+                    cell.unvisited = False
+                    house_in_area = True
+                    break
+            except:
+                continue
+    if(house_in_area):
+        xy = gc_moveset[-1]
+        gc_moveset.append("pick_garbage")
+        return (xy, gc_moveset)
+
+    if len(available_movement) == 0 or depth>30:
+        return
+
+    x,y = gc_moveset[-1]
+
+    print([x,y])
+    #calculate distance to the nearest object
+    min_distance_goal = ['-',INF]
+    for h in houses_list:
+        distance = CalculateDistance([a,b],h[1])
+        if(min_distance_goal[1] > distance):
+            min_distance_goal = [h[1], distance]
+            print(min_distance_goal)
+
+    #set preffered directions based on the closest object
+    preffered_directions = []
+    if(min_distance_goal[1] == 1):
+        preffered_directions.append("pick_garbage")
+    if(min_distance_goal[0][0] >= a):
+        preffered_directions.append("right")
+    if(min_distance_goal[0][0] <= a):
+        preffered_directions.append("left")
+    if(min_distance_goal[0][1] >= b):
+        preffered_directions.append("down")
+    if(min_distance_goal[0][1] <= b):
+        preffered_directions.append("up")
+    print(preffered_directions)
+    print("------------------------------")
+    for direction in available_movement:
+        x_next, y_next = movement(grid,x,y)[0][direction]
+        available_movement_next = check_moves(grid, x_next,y_next,direction)
+        gc_moveset_next = gc_moveset.copy()
+        gc_moveset_next.append([x_next,y_next])
+        result = BestFS(grid, available_movement_next, gc_moveset_next, houses_list, depth+1)
+        if result!= None:
+            return result