def test_is_a_robot_on_position_on_maze(self):
        maze_cells = np.array([[0, 0, 0, 0, 0]])
        maze = Maze(maze_cells)
        robots = {
            "A": (0, 2),
        }
        goals = queue.Queue()
        goals.put(Goal("A", (0, 1)))
        rr = RobotReboot(maze, goals)

        rr.set_robots(robots)
        self.assertTrue(rr.is_a_robot_on((0, 2)))
        self.assertFalse(rr.is_a_robot_on((0, 0)))
    def test_state(self):
        maze_cells = np.array([
            [Maze.EMPTY, Maze.S, Maze.EMPTY, Maze.E, Maze.EMPTY],
            [Maze.EMPTY, Maze.EMPTY, Maze.EMPTY, Maze.EMPTY, Maze.EMPTY],
            [Maze.N, Maze.N, Maze.EMPTY, Maze.EMPTY, Maze.EMPTY],
            [Maze.EMPTY, Maze.EMPTY, Maze.S, Maze.EMPTY, Maze.EMPTY],
            [Maze.E, Maze.E, Maze.EMPTY, Maze.EMPTY, Maze.EMPTY]
        ])
        maze = Maze(maze_cells)
        robots = {
            "A": (0, 2),
            "B": (0, 2),
            "C": (4, 2)
        }
        goals = queue.Queue()
        # Order here matters, first robot is B, next robot is A, last robot is C
        goals.put(Goal("B", (3, 4)))
        goals.put(Goal("A", (0, 0)))
        goals.put(Goal("C", (0, 0)))

        rr = RobotReboot(maze, goals)
        rr.set_robots(robots)

        obs = rr.state()
        rows, cols, layers = obs.shape

        self.assertEqual(obs.shape, (5, 5, 4))
        np.testing.assert_equal(obs[:, :, 0], maze_cells)
        # Checking robots on each layer
        self.assertEqual(obs[0, 2, 1], RobotReboot.ROBOT)
        self.assertEqual(obs[0, 2, 2], RobotReboot.ROBOT)
        self.assertEqual(obs[4, 2, 3], RobotReboot.ROBOT)
        # Checking goal on the target robot
        self.assertEqual(obs[3, 4, 1], RobotReboot.GOAL)

        for i in range(rows):
            for j in range(cols):
                for layer in range(1, layers):
                    if not rr.is_a_robot_on((i, j)) and rr.goal.cell != (i, j) and layer != 2:
                        self.assertEqual(obs[i, j, layer], 0)