Merge branch '4-2d-movement-continuous' into 'main'

Resolve "2D movement continuous"

Closes #4

See merge request scs/cocosy/overcooked-simulator!2

overcooked_simulator/counters.py

+import numpy as np
+
+
+class Counter:
+    """Simple class for a counter at a specified position (center of counter). Can hold things on top."""
+
+    def __init__(self, pos: np.array):
+        self.pos = pos
+        self.occupied_by = None
+
+    def __repr__(self):
+        return f"Counter(pos:{str(self.pos)},holds:{self.occupied_by})"

overcooked_simulator/layouts/basic.layout

@@ -2,9 +2,9 @@ EEEEEEEEEEE
 ECCCCCCCCCE
 ECEEEEEEECE
 ECEEEEEEECE
-ECEEEEEEECE
-ECEEEEEEECE
-ECEEEEEEECE
+ECEEEEEEEEE
+ECEEEEEEEEE
+ECEEEEEEEEE
 ECEEEEEEECE
 ECEEEEEEECE
 ECCCCCCCCCE

overcooked_simulator/layouts/empty.layout

+E
\ No newline at end of file

overcooked_simulator/main.py

 from overcooked_simulator.player import Player
 import sys
+from pathlib import Path
+
+from overcooked_simulator.player import Player
+from overcooked_simulator.pygame_gui import PyGameGUI
 from overcooked_simulator.simulation_runner import Simulator
-from overcooked_simulator.overcooked_environment import Environment
 
-def main():
-    simulator = Simulator(Environment, 300)
 
+def main():
+    simulator = Simulator(Path("overcooked_simulator/layouts/basic.layout"), 300)
     simulator.register_player(Player("p1", [100, 200]))
     simulator.register_player(Player("p2", [200, 100]))
 
+    gui = PyGameGUI(simulator)
+
     simulator.start()
 
-    print(simulator.get_state())
+    gui.start_pygame()
 
     simulator.stop()
     sys.exit()

overcooked_simulator/overcooked_environment.py

 from __future__ import annotations
+
 from typing import TYPE_CHECKING
 
 if TYPE_CHECKING:
     from overcooked_simulator.player import Player
 from pathlib import Path
 import numpy as np
-
-
-class Counter:
-    """Simple class for a counter at a specified position (center of counter). Can hold things on top.
-    """
-    def __init__(self, pos: np.array):
-        self.pos = pos
-        self.occupied_by = None
-
-    def __repr__(self):
-        return f"Counter(pos:{str(self.pos)},holds:{self.occupied_by})"
+from scipy.spatial import distance_matrix
+from overcooked_simulator.counters import Counter
 
 
 class Action:
-    """Action class, specifies player, action type and action itself.
-    """
+    """Action class, specifies player, action type and action itself."""
+
     def __init__(self, player, act_type, action):
         self.player = player
         self.act_type = act_type
-        assert self.act_type in ["movement", "pickup", "interact"], "Unknown action type"
+        assert self.act_type in [
+            "movement",
+            "pickup",
+            "interact",
+        ], "Unknown action type"
         self.action = action
 
 
 class Environment:
     """Environment class which handles the game logic for the overcooked-inspired environment.
 
-        Handles player movement, collision-detection, counters, cooking processes, recipes, incoming orders, time.
-        """
-    def __init__(self):
-        self.players = {}
-        self.counter_side_length = 40
-        self.layout_path = Path("overcooked_simulator/layouts/basic.layout")
-        self.counters = self.create_counters(self.layout_path)
-        self.score = 0
+    Handles player movement, collision-detection, counters, cooking processes, recipes, incoming orders, time.
+    # TODO Abstract base class for different environments
+    """
+
+    def __init__(self, layout_path):
+        self.players: dict[str, Player] = {}
+        self.counter_side_length: float = 40
+        self.layout_path: Path = layout_path
+        self.counters: list[Counter] = self.create_counters(self.layout_path)
+        self.score: int = 0
+        self.world_width: int = 800
+        self.world_height: int = 600
 
     def create_counters(self, layout_file: Path):
         """Creates layout of kitchen counters in the environment based on layout file.
@@ -64,8 +64,7 @@ class Environment:
                     counters.append(counter)
                     current_x += self.counter_side_length
                 elif character == "E":
-                    pass
-
+                    current_x += self.counter_side_length
             current_y += self.counter_side_length
         return counters
 
@@ -87,15 +86,34 @@ class Environment:
         elif action.act_type == "interact":
             self.perform_interact(player)
 
-    def get_closest_counter(self, player: Player):
-        """Determines the closest counter in the environment of a player.
+    def get_closest_counter(self, point: np.array):
+        """Determines the closest counter for a given 2d-coordinate point in the env.
 
         Args:
-            player: The player for which to find the closest counter
+            point: The point in the env for which to find the closest counter
 
-        Returns: The closest counter for the given player.
+        Returns: The closest counter for the given point.
         """
-        pass
+        counter_distances = distance_matrix(
+            [point], [counter.pos for counter in self.counters]
+        )[0]
+        closest_counter_idx = np.argmin(counter_distances)
+        return self.counters[closest_counter_idx]
+
+    def get_facing_counter(self, player: Player):
+        """Determines the counter which the player is looking at.
+        Adds a multiple of the player facing direction onto the player position and finds the closest
+        counter for that point.
+
+        Args:
+            player: The player for which to find the facing counter.
+
+        Returns:
+
+        """
+        facing_point = player.pos + (player.facing_direction * player.interaction_range)
+        facing_counter = self.get_closest_counter(facing_point)
+        return facing_counter
 
     def perform_pickup(self, player: Player):
         """Performs the game action corresponding to picking up an item
@@ -118,15 +136,45 @@ class Environment:
         """
         pass
 
-    def perform_movement(self, player: Player, action):
+    def perform_movement(self, player: Player, move_vector: np.array):
         """Moves a player in the direction specified in the action.action. If the player collides with a
         counter or other player through this movement, then they are not moved.
+        (The extended code with the two ifs is for sliding movement at the counters, which feels a bit smoother.
+        This happens, when the player moves diagonally against the counters or world boundary.
+        This just checks if the single axis party of the movement could move the player and does so at a lower rate.)
+
+        The movement action is a unit 2d vector.
 
         Args:
             player: The player to move.
-            action: The Action which now contains a unit-2d-vector of the movement direction
+            move_vector: The movement vector which is a unit-2d-vector of the movement direction
         """
-        pass
+        old_pos = player.pos.copy()
+
+        step = move_vector * player.move_dist
+        player.move(step)
+        if self.detect_collision(player):
+            player.move_abs(old_pos)
+
+            old_pos = player.pos.copy()
+
+            step_sliding = step.copy()
+            step_sliding[0] = 0
+            player.move(step_sliding * 0.5)
+            player.turn(step)
+
+            if self.detect_collision(player):
+                player.move_abs(old_pos)
+
+                old_pos = player.pos.copy()
+
+                step_sliding = step.copy()
+                step_sliding[1] = 0
+                player.move(step_sliding * 0.5)
+                player.turn(step)
+
+                if self.detect_collision(player):
+                    player.move_abs(old_pos)
 
     def detect_collision(self, player: Player):
         """Detect collisions between the player and other players or counters.
@@ -135,13 +183,85 @@ class Environment:
             player: The player for which to check collisions.
 
         Returns: True if the player is intersecting with any object in the environment.
+        """
+        return (
+                self.detect_player_collision(player)
+                or self.detect_collision_counters(player)
+                or self.detect_collision_world_bounds(player)
+        )
+
+    def detect_player_collision(self, player: Player):
+        """Detects collisions between the queried player and other players.
+        A player is modelled as a circle. Collision is detected if the distance between the players is smaller
+        than the sum of the radius's.
+
+        Args:
+            player: The player to check collisions with other players for.
+
+        Returns: True if the player collides with other players, False if not.
 
         """
-        pass
+        other_players = filter(lambda p: p.name != player.name, self.players.values())
+
+        def collide(p):
+            return np.linalg.norm(player.pos - p.pos) <= (player.radius + p.radius)
+
+        return any(map(collide, other_players))
+
+    def detect_collision_counters(self, player: Player):
+        """Checks for collisions of the queried player with each counter.
+
+        Args:
+            player:  The player to check collisions with counters for.
+
+        Returns: True if the player collides with any counter, False if not.
+
+        """
+        return any(
+            map(
+                lambda counter: self.detect_collision_player_counter(player, counter),
+                self.counters,
+            )
+        )
+
+    def detect_collision_player_counter(self, player: Player, counter: Counter):
+        """Checks if the player and counter collide (overlap).
+        A counter is modelled as a rectangle (square actually), a player is modelled as a circle.
+        The distance of the player position (circle center) and the counter rectangle is calculated, if it is
+        smaller than the player radius, a collision is detected.
+        TODO: Efficiency improvement by checking only nearest counters? Quadtree...?
+
+        Args:
+            player: The player to check the collision for.
+            counter: The counter to check the collision for.
+
+        Returns: True if player and counter overlap, False if not.
+
+        """
+        size = self.counter_side_length
+        cx, cy = player.pos
+        dx = max(np.abs(cx - counter.pos[0]) - size / 2, 0)
+        dy = max(np.abs(cy - counter.pos[1]) - size / 2, 0)
+        distance = np.linalg.norm([dx, dy])
+        return distance < player.radius
+
+    def detect_collision_world_bounds(self, player: Player):
+        """Checks for detections of the player and the world bounds.
+
+        Args:
+            player: The player which to not let escape the world.
+
+        Returns: True if the player touches the world bounds, False if not.
+        """
+        collisions_lower = any((player.pos - player.radius) < 0)
+        collisions_upper = any(
+            (player.pos + player.radius) > [self.world_width, self.world_height]
+        )
+        return collisions_lower or collisions_upper
 
     def step(self):
         """Performs a step of the environment. Affects time based events such as cooking or cutting things, orders
-        and timelimits.
+        and time limits.
         """
         pass
 
@@ -151,9 +271,7 @@ class Environment:
         Returns: Dict of lists of the current relevant game objects.
 
         """
-        return {"players": self.players,
-                "counters": self.counters,
-                "score": self.score}
+        return {"players": self.players, "counters": self.counters, "score": self.score}
 
     def get_state_json(self):
         """Get the current state of the game environment as a json-like nested dictionary.

overcooked_simulator/player.py

 import numpy as np
-from overcooked_simulator.overcooked_environment import Counter
+
+from overcooked_simulator.counters import Counter
 
 
 class Player:
@@ -8,12 +9,15 @@ class Player:
     This class handles interactions with counters and objects.
 
     """
+
     def __init__(self, name, pos):
         self.name = name
         self.pos = np.array(pos, dtype=float)
         self.holding = None
 
+        self.radius = 18
         self.move_dist = 5
+        self.interaction_range = 50
         self.facing_direction = np.array([0, 1])
 
     def move(self, movement: np.array):
@@ -23,7 +27,9 @@ class Player:
         Args:
             movement: 2D-Vector of length 1
         """
-        pass
+        self.pos += movement
+        if np.linalg.norm(movement) != 0:
+            self.turn(movement)
 
     def move_abs(self, new_pos: np.array):
         """Overwrites the player location by the new_pos 2d-vector. Absolute movement.
@@ -32,7 +38,7 @@ class Player:
         Args:
             new_pos: 2D-Vector of the new player position.
         """
-        pass
+        self.pos = new_pos
 
     def turn(self, direction: np.array):
         """Turns the player in the given direction. Overwrites the facing_direction by a given 2d-vector.
@@ -41,7 +47,8 @@ class Player:
         Args:
             direction: 2D-Vector of the direction for the player to face.
         """
-        pass
+        if np.linalg.norm(direction) != 0:
+            self.facing_direction = direction / np.linalg.norm(direction)
 
     def pick_action(self, counter: Counter):
         """Performs the pickup-action with the counter. Handles the logic of what the player is currently holding,

overcooked_simulator/pygame_gui.py

+import numpy as np
+import pygame
+
+from overcooked_simulator.overcooked_environment import Action
+from overcooked_simulator.simulation_runner import Simulator
+
+WHITE = (255, 255, 255)
+GREY = (190, 190, 190)
+BLACK = (0, 0, 0)
+COUNTERCOLOR = (240, 240, 240)
+LIGHTGREY = (220, 220, 220)
+GREEN = (0, 255, 0)
+RED = (255, 0, 0)
+BLUE = (0, 0, 255)
+YELLOW = (255, 255, 0)
+BACKGROUND_COLOR = GREY
+BACKGROUND_LINES_COLOR = (200, 200, 200)
+
+
+class PlayerKeyset:
+    """Set of keyboard keys for controlling a player.
+    First four keys are for movement,
+    5th key is for interacting with counters.
+    6th key ist for picking up things or dropping them.
+    """
+
+    def __init__(self, keys: list[pygame.key]):
+        self.player_keys = keys
+        self.move_vectors = [[-1, 0], [1, 0], [0, -1], [0, 1]]
+        self.key_to_movement = {
+            key: vec for (key, vec) in zip(self.player_keys[:-2], self.move_vectors)
+        }
+        self.interact_key = self.player_keys[-2]
+        self.pickup_key = self.player_keys[-1]
+
+
+class PyGameGUI:
+    """Visualisation of the overcooked environmnent and reading keyboard inputs using pygame."""
+
+    def __init__(self, simulator: Simulator):
+        self.FPS = 60
+        self.simulator = simulator
+        self.counter_size = self.simulator.env.counter_side_length
+        self.window_width, self.window_height = (
+            simulator.env.world_width,
+            simulator.env.world_height,
+        )
+
+        self.GET_CONTINOUS_INTERACT_AND_PICKUP = False
+
+        keys1 = [
+            pygame.K_LEFT,
+            pygame.K_RIGHT,
+            pygame.K_UP,
+            pygame.K_DOWN,
+            pygame.K_SPACE,
+            pygame.K_i,
+        ]
+        keys2 = [pygame.K_a, pygame.K_d, pygame.K_w, pygame.K_s, pygame.K_f, pygame.K_e]
+        self.player_keysets: list[PlayerKeyset] = [
+            PlayerKeyset(keys1),
+            PlayerKeyset(keys2),
+        ]
+
+    def send_action(self, action: Action):
+        """Sends an action to the game environment.
+
+        Args:
+            action: The action to be sent. Contains the player, action type and move direction if action is a movement.
+        """
+        self.simulator.enter_action(action)
+
+    def handle_keys(self):
+        """Handles keyboard inputs. Sends action for the respective players. When a key is held down, every frame
+        an action is sent in this function.
+        """
+        keys = pygame.key.get_pressed()
+        for player_idx, keyset in enumerate(self.player_keysets):
+            relevant_keys = [keys[k] for k in keyset.player_keys]
+            if any(relevant_keys[:-2]):
+                move_vec = np.zeros(2)
+                for idx, pressed in enumerate(relevant_keys[:-2]):
+                    if pressed:
+                        move_vec += keyset.move_vectors[idx]
+                if np.linalg.norm(move_vec) != 0:
+                    move_vec = move_vec / np.linalg.norm(move_vec)
+
+                action = Action(f"p{player_idx + 1}", "movement", move_vec)
+                self.send_action(action)
+            if self.GET_CONTINOUS_INTERACT_AND_PICKUP:
+                if relevant_keys[-2]:
+                    action = Action(f"p{player_idx + 1}", "interact", "interact")
+                    self.send_action(action)
+                if relevant_keys[-1]:
+                    action = Action(f"p{player_idx + 1}", "pickup", "pickup")
+                    self.send_action(action)
+
+    def handle_interact_single_send(self, event):
+        """Handles key events. Here when a key is held down, only one action is sent. (This can be
+        switched by the GET_CONTINOUS_INTERACT_AND_PICKUP flag)
+
+        Args:
+            event: Pygame event for extracting the key.
+        """
+        for player_idx, keyset in enumerate(self.player_keysets):
+            if event.key == keyset.pickup_key:
+                action = Action(f"p{player_idx + 1}", "pickup", "pickup")
+                self.send_action(action)
+            elif event.key == keyset.interact_key:
+                action = Action(f"p{player_idx + 1}", "interact", "interact")
+                self.send_action(action)
+
+    def draw_background(self):
+        """Visualizes a game background."""
+        block_size = self.counter_size // 2  # Set the size of the grid block
+        for x in range(0, self.window_width, block_size):
+            for y in range(0, self.window_height, block_size):
+                rect = pygame.Rect(x, y, block_size, block_size)
+                pygame.draw.rect(self.screen, BACKGROUND_LINES_COLOR, rect, 1)
+
+    def draw_players(self, state):
+        """Visualizes the players as circles with a triangle for the facing diretion.
+
+        Args:
+            state: The game state returned by the environment.
+        """
+        for player in state["players"].values():
+            pos = player.pos
+            size = player.radius
+            color1 = RED if player.name == "p1" else GREEN
+            color2 = WHITE
+
+            rect = pygame.Rect(pos[0] - (size / 2), pos[1] - (size / 2), size, size)
+            pygame.draw.circle(self.screen, color2, pos, size)
+            pygame.draw.rect(self.screen, color1, rect)
+
+            facing = player.facing_direction
+
+            pygame.draw.polygon(
+                self.screen,
+                BLUE,
+                (
+                    (pos[0] + (facing[1] * 5), pos[1] - (facing[0] * 5)),
+                    (pos[0] - (facing[1] * 5), pos[1] + (facing[0] * 5)),
+                    player.pos + (facing * 20),
+                ),
+            )
+
+    def draw_counters(self, state):
+        """Visualizes the counters in the environment.
+
+        Args:
+            state: The game state returned by the environment.
+        """
+        for idx, counter in enumerate(state["counters"]):
+            counter_rect_outline = pygame.Rect(
+                counter.pos[0] - (self.counter_size / 2),
+                counter.pos[1] - (self.counter_size / 2),
+                self.counter_size,
+                self.counter_size,
+            )
+
+            pygame.draw.rect(self.screen, COUNTERCOLOR, counter_rect_outline)
+
+    def draw(self, state):
+        """Main visualization function.
+
+        Args:
+            state: The game state returned by the environment.
+        """
+        self.screen.fill(BACKGROUND_COLOR)
+        self.draw_background()
+
+        self.draw_counters(state)
+        self.draw_players(state)
+
+        pygame.display.flip()
+
+    def start_pygame(self):
+        """Starts pygame and the gui loop. Each frame the gamestate is visualized and keyboard inputs are read."""
+        pygame.init()
+        pygame.font.init()
+
+        self.screen = pygame.display.set_mode((self.window_width, self.window_height))
+        pygame.display.set_caption("Simple Overcooked Simulator")
+        self.screen.fill(BACKGROUND_COLOR)
+
+        clock = pygame.time.Clock()
+
+        # Game loop
+        running = True
+        while running:
+            for event in pygame.event.get():
+                if event.type == pygame.QUIT:
+                    running = False
+                if not self.GET_CONTINOUS_INTERACT_AND_PICKUP:
+                    if event.type == pygame.KEYDOWN:
+                        self.handle_interact_single_send(event)
+
+            self.handle_keys()
+            clock.tick(self.FPS)
+            state = self.simulator.get_state()
+            self.draw(state)
+
+        pygame.quit()

overcooked_simulator/simulation_runner.py

-from threading import Thread
 import time
+from threading import Thread
+
 from overcooked_simulator.overcooked_environment import Environment, Action
 from overcooked_simulator.player import Player
 
@@ -17,79 +18,77 @@ class Simulator(Thread):
       sim.start()
     """
 
-    def __init__(self, env_class: type, frequency: int):
+    def __init__(self, env_layout_path, frequency: int):
         self.finished: bool = False
 
         self.step_frequency: int = frequency
         self.prefered_sleeptime_ns: float = 1e9 / self.step_frequency
-
-        self.env = env_class()
+        self.env: Environment = Environment(env_layout_path)
 
         super().__init__()
 
     def step(self):
-        """One simulation step of the environment.
-            """
+        """One simulation step of the environment."""
         self.env.step()
 
     def enter_action(self, action: Action):
         """Takes an action and executes it in the environment.
 
-            Args:
-                action (Action): The action object to be executed.
-            """
+        Args:
+            action (Action): The action object to be executed.
+        """
         self.env.perform_action(action)
 
     def get_state(self):
         """Get the current gamestate as python objects.
 
-            Returns:
-                The current state of the game. Currently as dict with lists of environment objects.
-            """
+        Returns:
+            The current state of the game. Currently as dict with lists of environment objects.
+        """
 
         return self.env.get_state()
 
     def get_state_json(self):
         """Get the current gamestate in json-like dict.
 
-            Returns:
-                The gamestate encoded in a json style nested dict.
-            """
+        Returns:
+            The gamestate encoded in a json style nested dict.
+        """
 
         return self.env.get_state_json()
 
     def register_player(self, player: Player):
-        print(f"Added player {player.name} to the game.")
         """Adds a player to the environment.
 
-            Args:
-                player: The player to be added.
-            """
+        Args:
+            player: The player to be added.
+        """
+        print(f"Added player {player.name} to the game.")
 
         self.env.players[player.name] = player
 
     def register_players(self, players: list[Player]):
         """Registers multiple players from a list
 
-            Args:
-                players: List of players to be added.
-            """
+        Args:
+            players: List of players to be added.
+        """
 
         for p in players:
             self.register_player(p)
 
     def run(self):
-        """Starts the simulator thread. Runs in a loop until stopped.
-            """
+        """Starts the simulator thread. Runs in a loop until stopped."""
         overslept_in_ns = 0
 
         while not self.finished:
-
             step_start = time.time_ns()
             self.step()
             step_duration = time.time_ns() - step_start
 
-            time_to_sleep_ns = self.prefered_sleeptime_ns - (step_duration + overslept_in_ns)
+            time_to_sleep_ns = self.prefered_sleeptime_ns - (
+                    step_duration + overslept_in_ns
+            )
 
             sleep_start = time.time_ns()
             time.sleep(max(time_to_sleep_ns / 1e9, 0))
@@ -97,7 +96,6 @@ class Simulator(Thread):
             overslept_in_ns = sleep_function_duration - time_to_sleep_ns
 
     def stop(self):
-        """Stops the simulator
-        """
+        """Stops the simulator"""
         print("Stopping the simulation.")
         self.finished = True

setup.py

@@ -4,39 +4,41 @@
 
 from setuptools import setup, find_packages
 
-with open('README.md') as readme_file:
+with open("README.md") as readme_file:
     readme = readme_file.read()
 
-with open('CHANGELOG.md') as history_file:
+with open("CHANGELOG.md") as history_file:
     history = history_file.read()
 
-requirements = ["numpy", "pygame", "scipy"]
+requirements = ["numpy", "pygame", "scipy", "pytest>=3"]
 
-test_requirements = ['pytest>=3', ]
+test_requirements = [
+    "pytest>=3",
+]
 
 setup(
     author="Annika Österdiekhoff, Dominik Battefeld, Fabian Heinrich, Florian Schröder",
-    author_email='florian.schroeder@uni-bielefeld.de',
-    python_requires='>=3.10',
+    author_email="florian.schroeder@uni-bielefeld.de",
+    python_requires=">=3.10",
     classifiers=[
-        'Development Status :: 2 - Pre-Alpha',
-        'Intended Audience :: Developers',
-        'License :: OSI Approved :: MIT License',
-        'Natural Language :: English',
-        'Programming Language :: Python :: 3',
-        'Programming Language :: Python :: 3.10',
+        "Development Status :: 2 - Pre-Alpha",
+        "Intended Audience :: Developers",
+        "License :: OSI Approved :: MIT License",
+        "Natural Language :: English",
+        "Programming Language :: Python :: 3",
+        "Programming Language :: Python :: 3.10",
     ],
     description="The real-time overcooked simulation for a cognitive cooperative system",
     install_requires=requirements,
     license="MIT license",
-    long_description=readme + '\n\n' + history,
+    long_description=readme + "\n\n" + history,
     include_package_data=True,
-    keywords=['aaambos', 'overcooked_simulator'],
-    name='overcooked_simulator',
-    packages=find_packages(include=['overcooked_simulator', 'overcooked_simulator.*']),
-    test_suite='tests',
+    keywords=["aaambos", "overcooked_simulator"],
+    name="overcooked_simulator",
+    packages=find_packages(include=["overcooked_simulator", "overcooked_simulator.*"]),
+    test_suite="tests",
     tests_require=test_requirements,
-    url='https://gitlab.ub.uni-bielefeld.de/scs/cocosy/overcooked-simulator',
-    version='0.1.0',
+    url="https://gitlab.ub.uni-bielefeld.de/scs/cocosy/overcooked-simulator",
+    version="0.1.0",
     zip_safe=False,
 )

tests/test_start.py

-from overcooked_simulator.simulation_runner import Simulator
-from overcooked_simulator.player import Player
-from overcooked_simulator.overcooked_environment import Environment, Action
-import numpy as np
 import time
+from pathlib import Path
 
-def test_player_registration():
+import numpy as np
+
+from overcooked_simulator.counters import Counter
+from overcooked_simulator.overcooked_environment import Action
+from overcooked_simulator.player import Player
+from overcooked_simulator.simulation_runner import Simulator
+
+try:
+    with open("../overcooked_simulator/layouts/basic.layout", "r") as textfile:
+        pass
+    layouts_folder = Path("../overcooked_simulator/layouts")
+except FileNotFoundError as e:
+    layouts_folder = Path("overcooked_simulator/layouts")
 
-    sim = Simulator(Environment, 200)
+
+def test_player_registration():
+    sim = Simulator(layouts_folder / "basic.layout", 200)
     p1 = Player("player1", np.array([0, 0]))
     sim.register_player(p1)
 
@@ -19,12 +30,13 @@ def test_player_registration():
     assert len(sim.env.players) == 2, "Wrong number of players"
 
     p3 = Player("player2", np.array([100, 100]))
-    sim.register_player(p2) # same player name
+    sim.register_player(p2)  # same player name
     assert len(sim.env.players) == 2, "Wrong number of players"
 
     sim.start()
     sim.stop()
 
+
 def test_simulator_frequency():
     class TestEnv:
         def __init__(self):
@@ -33,10 +45,11 @@ def test_simulator_frequency():
         def step(self):
             self.c += 1
 
-    frequency = 1000
-    running_time_seconds = 8
+    frequency = 2000
+    running_time_seconds = 3
 
-    sim = Simulator(TestEnv, frequency)
+    sim = Simulator(layouts_folder / "basic.layout", frequency)
+    sim.env = TestEnv()  # Overwrite environment with a simple counting env
 
     sim.start()
     time.sleep(running_time_seconds)
@@ -44,6 +57,57 @@ def test_simulator_frequency():
 
     print(sim.env.c)
     accepted_tolerance = 0.02
-    lower = frequency * running_time_seconds * (1-accepted_tolerance)
-    upper = frequency * running_time_seconds * (1+accepted_tolerance)
-    assert sim.env.c > lower and sim.env.c < upper, "Timing error in the environment at 1000hz"
+    lower = frequency * running_time_seconds * (1 - accepted_tolerance)
+    upper = frequency * running_time_seconds * (1 + accepted_tolerance)
+    assert lower < sim.env.c < upper, "Timing error in the environment at 1000hz"
+
+
+def test_movement():
+    sim = Simulator(layouts_folder / "empty.layout", 200)
+    player_name = "p1"
+    start_pos = np.array([50, 50])
+    player1 = Player(player_name, start_pos)
+    sim.register_player(player1)
+    move_direction = np.array([1, 0])
+    move_action = Action(player_name, "movement", move_direction)
+    do_moves_number = 6
+    for i in range(do_moves_number):
+        sim.enter_action(move_action)
+
+    expected = start_pos + do_moves_number * (move_direction * player1.move_dist)
+
+    assert (
+            np.linalg.norm(expected - sim.env.players[player_name].pos) == 0
+    ), "Should be here?"
+
+
+def test_collision_detection():
+    sim = Simulator(layouts_folder / "empty.layout", 200)
+    counter_pos = np.array([50, 100])
+    counter = Counter(counter_pos)
+    sim.env.counters = [counter]
+    player1 = Player("p1", np.array([50, 50]))
+    player2 = Player("p2", np.array([50, 200]))
+    sim.register_player(player1)
+    sim.register_player(player2)  # same player name
+
+    sim.start()
+    assert not sim.env.detect_collision_counters(player1), "Should not collide"
+    assert not sim.env.detect_player_collision(player1), "Should not collide yet."
+
+    assert not sim.env.detect_collision(player1), "Does not collide yet."
+
+    player1.move_abs(counter_pos)
+    assert sim.env.detect_collision_counters(
+        player1
+    ), "Player and counter at same pos. Not detected."
+    player2.move_abs(counter_pos)
+    assert sim.env.detect_player_collision(
+        player1
+    ), "Players at same pos. Not detected."
+
+    player1.move_abs(np.array([0, 0]))
+    assert sim.env.detect_collision_world_bounds(
+        player1
+    ), "Player collides with world bounds."
+    sim.stop()