import queue

from domain.commands.vacuum_move_command import VacuumMoveCommand
from domain.world import World


class State:
    def __init__(self, x, y, direction=(1, 0)):
        self.x = x
        self.y = y
        self.direction = direction

    def __hash__(self):
        return hash((self.x, self.y))

    def __eq__(self, other):
        return (self.x == other.x and self.y == other.y
                and self.direction == other.direction)


class Node:
    def __init__(self, state: State):
        self.state = state
        self.parent = None
        self.action = None


def action_sequence(node: Node):
    actions = []
    while node.parent:
        actions.append(node.action)
        node = node.parent
    actions.reverse()
    return actions


class TemplateBFS:
    def __init__(self, world: World, start_state: State, goal_state: State):
        self.world = world
        self.start_state = start_state
        self.goal_state = goal_state
        self.fringe = queue.Queue()
        self.enqueued_states = set()
        self.explored = set()
        self.actions = []

    def search(self):
        self.fringe.put(Node(self.start_state))

        while self.fringe:
            elem = self.fringe.get()
            if elem.state == self.goal_state:
                self.actions = action_sequence(elem)
                return True
            self.explored.add(elem.state)

            for (action, state) in self.successors(elem.state):
                if state in self.explored or state in self.enqueued_states:
                    continue
                next_node = Node(state)
                next_node.action = action
                next_node.parent = elem
                self.fringe.put(next_node)
                self.enqueued_states.add(state)

        return False

    def successors(self, state: State):
        new_successors = [
            # ("GO", State(state.x + state.direction[0], state.y + state.direction[1], state.direction)),
            # rotate right
            ("RR", State(state.x, state.y, (-state.direction[1], state.direction[0]))),
            # rotate left
            ("RL", State(state.x, state.y, (state.direction[1], -state.direction[0]))),
        ]
        if self.world.accepted_move(state.x + state.direction[0], state.y + state.direction[1]):
            new_successors.append(("GO", State(state.x + state.direction[0], state.y + state.direction[1], state.direction)))
        return new_successors