from utilities import movement,check_moves
from DataModels.House import House
from DataModels.Container import Container
from config import GRID_WIDTH, GRID_HEIGHT
from math import sqrt
INF = float('Inf')


def CalculateDistance(gc, object_list):
    min_distance_goal = ['-',INF]
    for h in object_list:
        distance = sqrt(pow(h[1][0]-gc[0],2)+pow(h[1][1]-gc[1],2))
        if(min_distance_goal[1] > distance):
            min_distance_goal = [h[1], distance]
    return min_distance_goal

def BestFS(grid, available_movement, gc_moveset, object_list, depth = 0):
    
    x, y = gc_moveset[-1][0], gc_moveset[-1][1]

    #if depth exceeded, return
    if(depth > 15 or len(available_movement) == 0):
        return 

    #calculate distance to the nearest object
    min_distance_goal = CalculateDistance([x,y], object_list)
    print(depth,min_distance_goal)

    if(min_distance_goal[1] == 1):
        gc_moveset.append("pick_garbage")
        cell = grid[min_distance_goal[0][0]][min_distance_goal[0][1]]
        print("***"+str([cell,min_distance_goal[0]])+"***")
        object_list.remove([cell,min_distance_goal[0]])
        return([x, y], gc_moveset, object_list)    

    #set preffered directions based on the closest object
    preffered_directions = []
    discouraged_directions = []

    if(min_distance_goal[0][0] > x):
        preffered_directions.append("right")
    if(min_distance_goal[0][0] < x):
        preffered_directions.append("left")
    if(min_distance_goal[0][1] > y):
        preffered_directions.append("down")
    if(min_distance_goal[0][1] < y):
        preffered_directions.append("up")

    if(len(preffered_directions) == 1):
        discouraged_directions.append(movement(grid, x, y)[1][preffered_directions[0]])

    """print("Preferred: "+str(preffered_directions))
    print("Discouraged: "+str(discouraged_directions))
    print("Available: "+str(available_movement))"""
    
    #sort available moves according to preferences
    sorted = [o for o in preffered_directions if o in available_movement]
    for o in sorted:
        available_movement.remove(o)
    sorted.extend([o for o in available_movement if o not in discouraged_directions])
    for o in sorted:
        if(o in available_movement):
            available_movement.remove(o)
    sorted.extend(available_movement)
    available_movement = sorted.copy()

    print("After sorting: "+str(available_movement))

    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, object_list, depth + 1)
        print(type(result))
        if result!= None:
            return result