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, gc_moveset, object_list):
    print(gc_moveset)
    x, y = gc_moveset[-1][0], gc_moveset[-1][1]
    available_movement = check_moves(grid, x, y)

    if(len(object_list) == 0): 
        return gc_moveset


    decision_stack = []

    constraint = 100
    while(len(object_list) > 0 and constraint > 0):

        print("================")
        print("iteracja: "+str(100-constraint))
        print("GC: "+str([x,y]))
        print(object_list)

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

        #check if can pick garbage
        if(min_distance_goal[1] == 1):
            gc_moveset.append("pick_garbage")
            decision_stack = []
            #remove object from goals list
            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]])
            #if that was the last object, return 
            if(len(object_list)==0):
                break
            #look for a new goal
            available_movement = check_moves(grid, x, y)
            min_distance_goal = CalculateDistance([x,y], object_list)
            print(min_distance_goal)
            print(min_distance_goal[0])
        
        #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))
        
        #if agent finds more than 1 optimal route
        possible_routes = len([i for i in available_movement if i in preffered_directions ])
        print("Preferred to available count: "+str(possible_routes))

        #HOTFIX
        if(possible_routes > 1):
            if(len(decision_stack) > 0):
                if(decision_stack[0] == [x,y]):
                    preffered_directions.pop(1)
            else:        
                decision_stack = [[x,y]]
                preffered_directions.pop(0)
        print("Decision stack: "+str(decision_stack))

        """ if(possible_routes > 1):
            for move in available_movement:
                if (move in preffered_directions):
                    x_next, y_next = movement(grid, x, y)[0][move]
                    route = BestFS(grid, [x_next,y_next], houses_list, [[x_next, y_next]], check_moves(grid, x_next, y_next, move), "House", depth + 1)
                    print("DIRECTION: "+str(move)+", GIVEN "+str(len(houses_list))+" HOUSES, RECURSION ON DEPTH "+str(depth+1)+" returned "+str(route))
                    if (route == None):
                        break
                    if (route.count("pick_garbage") - 1 == len(houses_list)):
                        print(str(route.count("pick_garbage"))+" / "+str(len(houses_list)))
                        print("ROUTE ADDED")
                        gc_moveset.extend(route)
                        break
            break """

        #if got no available moves but still has goals to visit
        if(len(available_movement) == 0):
            available_movement = check_moves(grid, x, y)

        #selecting next move based on preferences (starting with the worst option)
        if(len(available_movement)>0):
            next_move = available_movement[0] #pick any
            for move in available_movement:
                if (move not in discouraged_directions): #pick any not discouraged move if possible
                    next_move = move
                    break
            for move in preffered_directions:
                if(move in available_movement): #pick any preferred move if possible
                    next_move = move
                    break
            print("Next move: "+str(next_move))
            x_next, y_next = movement(grid, x, y)[0][next_move]
            print("Next moving to "+str(x_next)+" "+str(y_next))
            gc_moveset.append([x_next,y_next])
            x, y = x_next, y_next
            available_movement = check_moves(grid, x, y, next_move)
        print("------------------------------")
                
        constraint -= 1

    return gc_moveset, [x,y]