TDDC17AICourse/lab1/myvacuumagent.py

from lab1.liuvacuum import *

DEBUG_OPT_DENSEWORLDMAP = False

AGENT_STATE_UNKNOWN = 0
AGENT_STATE_WALL = 1
AGENT_STATE_CLEAR = 2
AGENT_STATE_DIRT = 3
AGENT_STATE_HOME = 4

AGENT_DIRECTION_NORTH = 0
AGENT_DIRECTION_EAST = 1
AGENT_DIRECTION_SOUTH = 2
AGENT_DIRECTION_WEST = 3

def direction_to_string(cdr):
    cdr %= 4
    return  "NORTH" if cdr == AGENT_DIRECTION_NORTH else\
            "EAST"  if cdr == AGENT_DIRECTION_EAST else\
            "SOUTH" if cdr == AGENT_DIRECTION_SOUTH else\
            "WEST" #if dir == AGENT_DIRECTION_WEST

"""
Internal state of a vacuum agent
"""
class MyAgentState:

    def __init__(self, width, height):

        # Initialize perceived world state
        self.world = [[AGENT_STATE_UNKNOWN for _ in range(height)] for _ in range(width)]
        self.world[1][1] = AGENT_STATE_HOME

        # Agent internal state
        self.last_action = ACTION_NOP
        self.direction = AGENT_DIRECTION_EAST
        self.pos_x = 1
        self.pos_y = 1

        # Metadata
        self.world_width = width
        self.world_height = height

    """
    Update perceived agent location and orientation
    """
    def update_position(self, bump):
        if not bump and self.last_action == ACTION_FORWARD:
            if self.direction == AGENT_DIRECTION_EAST:
                self.pos_x += 1
            elif self.direction == AGENT_DIRECTION_SOUTH:
                self.pos_y += 1
            elif self.direction == AGENT_DIRECTION_WEST:
                self.pos_x -= 1
            elif self.direction == AGENT_DIRECTION_NORTH:
                self.pos_y -= 1
        elif self.last_action == ACTION_TURN_LEFT:
            if self.direction == AGENT_DIRECTION_EAST:
                self.direction = AGENT_DIRECTION_NORTH
            elif self.direction == AGENT_DIRECTION_SOUTH:
                self.direction = AGENT_DIRECTION_EAST
            elif self.direction == AGENT_DIRECTION_WEST:
                self.direction = AGENT_DIRECTION_SOUTH
            elif self.direction == AGENT_DIRECTION_NORTH:
                self.direction = AGENT_DIRECTION_WEST
        elif self.last_action == ACTION_TURN_RIGHT:
            if self.direction == AGENT_DIRECTION_EAST:
                self.direction = AGENT_DIRECTION_SOUTH
            elif self.direction == AGENT_DIRECTION_SOUTH:
                self.direction = AGENT_DIRECTION_WEST
            elif self.direction == AGENT_DIRECTION_WEST:
                self.direction = AGENT_DIRECTION_NORTH
            elif self.direction == AGENT_DIRECTION_NORTH:
                self.direction = AGENT_DIRECTION_EAST

    """
    Update perceived or inferred information about a part of the world
    """
    def update_world(self, x, y, info):
        self.world[x][y] = info

    """
    Dumps a map of the world as the agent knows it
    """
    def print_world_debug(self):
        for y in range(self.world_height):
            for x in range(self.world_width):
                if self.world[x][y] == AGENT_STATE_UNKNOWN:
                    print("?" if DEBUG_OPT_DENSEWORLDMAP else " ? ", end="")
                elif self.world[x][y] == AGENT_STATE_WALL:
                    print("#" if DEBUG_OPT_DENSEWORLDMAP else " # ", end="")
                elif self.world[x][y] == AGENT_STATE_CLEAR:
                    print("." if DEBUG_OPT_DENSEWORLDMAP else " . ", end="")
                elif self.world[x][y] == AGENT_STATE_DIRT:
                    print("D" if DEBUG_OPT_DENSEWORLDMAP else " D ", end="")
                elif self.world[x][y] == AGENT_STATE_HOME:
                    print("H" if DEBUG_OPT_DENSEWORLDMAP else " H ", end="")

            print() # Newline
        print() # Delimiter post-print

"""
Vacuum agent
"""
class MyVacuumAgent(Agent):
    """
    Init function, everything here is execute once at the beginning
    """
    def __init__(self, world_width, world_height, log):
        super().__init__(self.execute)
        self.initial_random_actions = 10
        self.iteration_counter = 1000
        self.state = MyAgentState(world_width, world_height)
        self.log = log

        # Arrived variable for the goto function
        self.arrived = True
        # Arrived variable for the go_to_with_pathfinder function
        self.arrived_destination = True
        # If the agent has finished the cleaning
        self.finished = False

        # Goto coordinates for the goto function
        self.goto_x = 1
        self.goto_y = 1

        # Goto coordinates for the go_to_with_pathfinder function
        self.destination_x = 1
        self.destination_y = 1

        # If we need to re-calculate our destination
        self.reset_destination = True

        # Different list in agent memory
        self.waypoint = []                  # Memory for next waypoint coordinates
        self.slist = []                     # List of waypoint to go at the agent destination

        # At the beginning, we assume that the zone is surrounded by wall
        for i in range(0, world_width):
            self.state.update_world(i, 0, AGENT_STATE_WALL)
            self.state.update_world(i, world_height - 1, AGENT_STATE_WALL)
        for j in range(1, world_height - 1):
            self.state.update_world(0, j, AGENT_STATE_WALL)
            self.state.update_world(world_width - 1, j, AGENT_STATE_WALL)

    """
    Function which move the agent to a ramdom start position
    """
    def move_to_random_start_position(self, bump):
        action = random()

        self.initial_random_actions -= 1
        self.state.update_position(bump)
        self.log("Actual direction: " + direction_to_string(self.state.direction))

        if action < 0.1666666:   # 1/6 chance
            self.state.last_action = ACTION_TURN_LEFT
            return ACTION_TURN_LEFT
        elif action < 0.3333333: # 1/6 chance
            self.state.last_action = ACTION_TURN_RIGHT
            return ACTION_TURN_RIGHT
        else:                    # 4/6 chance
            self.state.last_action = ACTION_FORWARD
            return ACTION_FORWARD

    """
    A basic "go to" function which does not consider wall
    """
    def goto(self, bump, dirt):
        self.state.update_position(bump)
        self.log("Actual direction: " + direction_to_string(self.state.direction))
        self.log("Next waypoint: {}, {}".format(self.goto_x, self.goto_y))

        if dirt:
            self.state.update_world(self.state.pos_x, self.state.pos_y, AGENT_STATE_DIRT)
            self.log("DIRT -> choosing SUCK action!")
            self.state.last_action = ACTION_SUCK
            return ACTION_SUCK
        else:
            self.state.update_world(self.state.pos_x, self.state.pos_y, AGENT_STATE_CLEAR)

        if (self.goto_x == self.state.pos_x and self.goto_y == self.state.pos_y) or bump:
            self.arrived = True
            self.log("Arrived to the target point")
            self.state.last_action = ACTION_NOP
            return ACTION_NOP

        if abs(self.state.pos_x - self.goto_x) >= abs(self.state.pos_y - self.goto_y):
            if self.state.pos_x - self.goto_x > 0:          # Need to go WEST
                if self.state.direction == AGENT_DIRECTION_WEST:
                    return ACTION_FORWARD
                elif self.state.direction == AGENT_DIRECTION_SOUTH:
                    return ACTION_TURN_RIGHT
                else:
                    return ACTION_TURN_LEFT
            else:                                           # Need to go EAST
                if self.state.direction == AGENT_DIRECTION_EAST:
                    return ACTION_FORWARD
                elif self.state.direction == AGENT_DIRECTION_NORTH:
                    return ACTION_TURN_RIGHT
                else:
                    return ACTION_TURN_LEFT
        else:
            if self.state.pos_y - self.goto_y > 0:          # Need to go NORTH
                if self.state.direction == AGENT_DIRECTION_NORTH:
                    return ACTION_FORWARD
                elif self.state.direction == AGENT_DIRECTION_WEST:
                    return ACTION_TURN_RIGHT
                else:
                    return ACTION_TURN_LEFT
            else:                                           # Need to go SOUTH
                if self.state.direction == AGENT_DIRECTION_SOUTH:
                    return ACTION_FORWARD
                elif self.state.direction == AGENT_DIRECTION_EAST:
                    return ACTION_TURN_RIGHT
                else:
                    return ACTION_TURN_LEFT

    """
    A pathfinding algorithm, based on the BFS algorithm
    """
    def pathfinding(self, target_x, target_y):

        # Class for node used in the search
        class Node:
            def __init__(self, parent=None, x=-1, y=-1):
                self.x = x
                self.y = y
                self.parent = parent

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

        # Start and end nodes
        start = Node(None, self.state.pos_x, self.state.pos_y)
        end = Node(None, target_x, target_y)

        # List containing the nodes to explore (By order of priority)
        queue = [start]
        # List of explored nodes
        explored = []
        # List which will be returned by the program containing
        # the coordinates to follow to reach the targeted point
        slist = []

        # While there is node to explore
        while queue:
            # If the queue is to big, we assume that there is a problem
            if len(queue) > 200:
                break

            # We take the first node in the queue
            node = queue.pop(0)
            # If it is the destination
            if node == end:
                print("Trying to go at : {}; {}".format(target_x, target_y))
                print("Solution found : ")
                n = node
                # We return the path found
                while n.parent:
                    print("x: {}, y: {}".format(n.x, n.y))
                    slist.append([n.x, n.y])
                    n = n.parent
                # and stop the processing
                break

            # If we never explored this node
            elif node not in explored:
                explored.append(node)

                # For each adjacent node, if it is not a wall,
                # we add it to the queue
                if self.state.world[node.x - 1][node.y] != AGENT_STATE_WALL:
                    queue.append(Node(node, node.x-1, node.y))
                if self.state.world[node.x + 1][node.y] != AGENT_STATE_WALL:
                    queue.append(Node(node, node.x+1, node.y))
                if self.state.world[node.x][node.y - 1] != AGENT_STATE_WALL:
                    queue.append(Node(node, node.x, node.y - 1))
                if self.state.world[node.x][node.y + 1] != AGENT_STATE_WALL:
                    queue.append(Node(node, node.x, node.y+1))

        # If we don't find any solution
        if not slist:
            # We consider that the place in inaccessible
            self.state.update_world(end.x, end.y, AGENT_STATE_WALL)
            self.log("Find a inaccessible place, marking as a wall ##")
            print("Overload! Inaccessible place!")
        return slist

    """
    A "go to" function which implement the pathfinding algorithm using the goto function
    """
    def go_to_with_pathfinder(self, bump, dirt):
        # If we start or if we need to reset the path
        if self.reset_destination:
            print("Launching the pathfinding algorithm.")
            self.slist = self.pathfinding(self.destination_x, self.destination_y)

        # If we have a path, we follow it
        if self.slist:
            # If the agent move correctly the previous time
            if not bump:
                self.waypoint = self.slist.pop()
            self.goto_x = self.waypoint[0]
            self.goto_y = self.waypoint[1]
            self.arrived = False
            action = self.goto(bump, dirt)
            self.state.last_action = action
            self.reset_destination = False
            return action
        # else (whether we arrived or we don't find a path), we assume
        # that we will not go further
        else:
            self.reset_destination = True
            self.arrived_destination = True
            self.state.last_action = ACTION_NOP
            return ACTION_NOP
        pass

    """
    The execute function, which is call at each turn
    """
    def execute(self, percept):

        # If the agent has finished, we do nothing
        if self.finished:
            return ACTION_NOP

        ###########################
        # DO NOT MODIFY THIS CODE #
        ###########################

        bump = percept.attributes["bump"]
        dirt = percept.attributes["dirt"]
        home = percept.attributes["home"]

        # Move agent to a randomly chosen initial position
        if self.initial_random_actions > 0:
            self.log("Moving to random start position ({} steps left)".format(self.initial_random_actions))
            return self.move_to_random_start_position(bump)

        # Finalize randomization by properly updating position (without subsequently changing it)
        elif self.initial_random_actions == 0:
            self.initial_random_actions -= 1
            self.state.update_position(bump)
            self.state.last_action = ACTION_SUCK
            self.log("Processing percepts after position randomization")
            return ACTION_SUCK

        ########################
        # START MODIFYING HERE #
        ########################

        # Logging position and orientation
        self.log("Position: ({}, {})\t\tDirection: {}".format(self.state.pos_x, self.state.pos_y,
                                                              direction_to_string(self.state.direction)))

        if bump:
            # Not arrived, but this way we force our agent to re-calculate its path
            self.arrived = True
            self.reset_destination = True
            # Get an xy-offset pair based on where the agent is facing
            offset = [(0, -1), (1, 0), (0, 1), (-1, 0)][self.state.direction]

            # Mark the tile at the offset from the agent as a wall (since the agent bumped into it)
            self.state.update_world(self.state.pos_x + offset[0], self.state.pos_y + offset[1], AGENT_STATE_WALL)

        # While we aren't arrived, we refer to the goto function
        if not self.arrived:
            action = self.goto(bump, dirt)
            self.state.last_action = action
            return action
        # While we aren't arrived, we refer to the go_to_with_pathfinder function
        elif not self.arrived_destination:
            return self.go_to_with_pathfinder(bump, dirt)

        # Max iterations for the agent
        if self.iteration_counter < 1:
            if self.iteration_counter == 0:
                self.iteration_counter -= 1
                self.log("Iteration counter is now 0. Halting!")
                self.log("Performance: {}".format(self.performance))
            return ACTION_NOP
        self.iteration_counter -= 1

        # Track position of agent
        self.state.update_position(bump)

        # Update perceived state of current tile
        if dirt:
            self.state.update_world(self.state.pos_x, self.state.pos_y, AGENT_STATE_DIRT)
        else:
            self.state.update_world(self.state.pos_x, self.state.pos_y, AGENT_STATE_CLEAR)

        # Debug
        self.state.print_world_debug()

        # Variables used to determine the closest unknown place
        closest_unk_range = self.state.world_height + self.state.world_width + 1
        closest_x = 1
        closest_y = 1
        find_new = False

        # Determination of the nearest unknown location
        for i in range(self.state.world_width):
            for j in range(self.state.world_height):
                if self.state.world[i][j] == AGENT_STATE_UNKNOWN and (
                        abs(self.state.pos_x - i) + abs(self.state.pos_y - j)) < closest_unk_range:
                    find_new = True
                    closest_unk_range = (abs(self.state.pos_x - i) + abs(self.state.pos_y - j))
                    closest_x = i
                    closest_y = j

        # If we find a new place to go
        if find_new:
            self.log("Going to unknown places : ({};{})".format(closest_x, closest_y))
            self.destination_x = closest_x
            self.destination_y = closest_y
            return self.go_to_with_pathfinder(bump, dirt)
        # If not, we have finished, we go home
        if home:
            self.log("Finished !")
            self.log("Performance: {}".format(self.performance))
            self.finished = True
        # Finally we are at home
        else:
            self.log("Job done, going back home.")
            self.destination_x = 1
            self.destination_y = 1
            return self.go_to_with_pathfinder(bump, dirt)

        # Useless
        self.state.last_action = ACTION_NOP
        return ACTION_NOP