# coding=utf-8
# Copyright 2021 The Google Research Authors.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

"""Implements the multi-agent version of the Grid and MultiGridEnv classes.

Note that at each step, the environment expects an array of actions equal to the
number of agents with which the class was initialized. Similarly, it will return
an array of observations, and an array of rewards.

In the competitive version, as soon as one agent finds the goal, the game is
over.

In the non-competitive case, all episodes have a fixed length based on the
maximum number of steps. To avoid issues with some agents finishing early and
therefore requiring support for non-scalar step types, if an agent finishes
before the step deadlie it will be respawned in a new location. To make the
single-agent case comparable to this design, it should also run for a fixed
number of steps and allow the agent to find the goal as many times as possible
within this step budget.

Unlike Minigrid, Multigrid does not include the string text of the 'mission'
with each observation.
"""
import math

import gym
import gym_minigrid.minigrid as minigrid
import gym_minigrid.rendering as rendering
import numpy as np

from . import window

# Map of color names to RGB values
AGENT_COLOURS = [
    np.array([60, 182, 234]),  # Blue
    np.array([229, 52, 52]),  # Red
    np.array([144, 32, 249]),  # Purple
    np.array([69, 196, 60]),  # Green
    np.array([252, 227, 35]),  # Yellow
]

class WorldObj(minigrid.WorldObj):
  """Override MiniGrid base class to deal with Agent objects."""

  def __init__(self, obj_type, color=None):
    assert obj_type in minigrid.OBJECT_TO_IDX, obj_type
    self.type = obj_type
    if color:
      assert color in minigrid.COLOR_TO_IDX, color
      self.color = color

    self.contains = None

    # Initial position of the object
    self.init_pos = None

    # Current position of the object
    self.cur_pos = None

  @staticmethod
  def decode(type_idx, color_idx, state):
    """Create an object from a 3-tuple state description."""

    obj_type = minigrid.IDX_TO_OBJECT[type_idx]
    if obj_type != 'agent':
      color = minigrid.IDX_TO_COLOR[color_idx]

    if obj_type == 'empty' or obj_type == 'unseen':
      return None

    if obj_type == 'wall':
      v = minigrid.Wall(color)
    elif obj_type == 'floor':
      v = minigrid.Floor(color)
    elif obj_type == 'ball':
      v = minigrid.Ball(color)
    elif obj_type == 'key':
      v = minigrid.Key(color)
    elif obj_type == 'box':
      v = minigrid.Box(color)
    elif obj_type == 'door':
      # State, 0: open, 1: closed, 2: locked
      is_open = state == 0
      is_locked = state == 2
      v = Door(color, is_open, is_locked)
    elif obj_type == 'goal':
      v = minigrid.Goal()
    elif obj_type == 'lava':
      v = minigrid.Lava()
    elif obj_type == 'agent':
      v = Agent(color_idx, state)
    else:
      assert False, "unknown object type in decode '%s'" % obj_type

    return v


class Door(minigrid.Door):
  """Extends minigrid Door class to multiple agents possibly carrying keys."""

  def toggle(self, env, pos, carrying):
    # If the player has the right key to open the door
    if self.is_locked:
      if isinstance(carrying, minigrid.Key) and carrying.color == self.color:
        self.is_locked = False
        self.is_open = True
        return True
      return False

    self.is_open = not self.is_open
    return True


class Agent(WorldObj):
  """Class to represent other agents existing in the world."""

  def __init__(self, agent_id, state):
    super(Agent, self).__init__('agent')
    self.agent_id = agent_id
    self.dir = state

  def can_contain(self):
    """Can this contain another object?"""
    return True

  def encode(self):
    """Encode the a description of this object as a 3-tuple of integers."""
    return (minigrid.OBJECT_TO_IDX[self.type], self.agent_id, self.dir)

  def render(self, img):
    tri_fn = rendering.point_in_triangle(
        (0.12, 0.19),
        (0.87, 0.50),
        (0.12, 0.81),
    )

    # Rotate the agent based on its direction
    tri_fn = rendering.rotate_fn(
        tri_fn, cx=0.5, cy=0.5, theta=0.5 * math.pi * self.dir)
    color = AGENT_COLOURS[self.agent_id]
    rendering.fill_coords(img, tri_fn, color)


class Grid(minigrid.Grid):
  """Extends Grid class, overrides some functions to cope with multi-agent case."""

  @classmethod
  def render_tile(cls,
                  obj,
                  highlight=None,
                  tile_size=minigrid.TILE_PIXELS,
                  subdivs=3,
                  cell_type=None):
    """Render a tile and cache the result."""
    # Hash map lookup key for the cache
    if isinstance(highlight, list):
      key = (tuple(highlight), tile_size)
    else:
      key = (highlight, tile_size)
    key = obj.encode() + key if obj else key

    if key in cls.tile_cache:
      return cls.tile_cache[key]

    img = np.zeros(
        shape=(tile_size * subdivs, tile_size * subdivs, 3), dtype=np.uint8)

    # Draw the grid lines (top and left edges)
    rendering.fill_coords(img, rendering.point_in_rect(0, 0.031, 0, 1),
                          (100, 100, 100))
    rendering.fill_coords(img, rendering.point_in_rect(0, 1, 0, 0.031),
                          (100, 100, 100))

    if obj is not None and obj.type != 'agent':
      obj.render(img)

    # Highlight the cell if needed (do not highlight walls)
    if highlight and not (cell_type is not None and cell_type == 'wall'):
      if isinstance(highlight, list):
        for a, agent_highlight in enumerate(highlight):
          if agent_highlight:
            rendering.highlight_img(img, color=AGENT_COLOURS[a])
      else:
        # Default highlighting for agent's partially observed views
        rendering.highlight_img(img)

    # Render agents after highlight to avoid highlighting agent triangle (the
    # combination of colours makes it difficult to ID agent)
    if obj is not None and obj.type == 'agent':
      obj.render(img)

    # Downsample the image to perform supersampling/anti-aliasing
    img = rendering.downsample(img, subdivs)

    # Cache the rendered tile
    cls.tile_cache[key] = img

    return img

  def render(self,
             tile_size,
             highlight_mask=None):
    """Render this grid at a given scale.

    Args:
      tile_size: Tile size in pixels.
      highlight_mask: An array of binary masks, showing which part of the grid
        should be highlighted for each agent. Can also be used in partial
        observation for single agent, which must be handled differently.

    Returns:
      An image of the rendered Grid.
    """
    if highlight_mask is None:
      highlight_mask = np.zeros(shape=(self.width, self.height), dtype=np.bool)

    # Compute the total grid size
    width_px = self.width * tile_size
    height_px = self.height * tile_size

    img = np.zeros(shape=(height_px, width_px, 3), dtype=np.uint8)

    # Render the grid
    for y in range(0, self.height):
      for x in range(0, self.width):
        cell = self.get(x, y)
        cell_type = cell.type if cell else None

        if isinstance(highlight_mask, list):
          # Figure out highlighting for each agent
          n_agents = len(highlight_mask)
          highlights = [highlight_mask[a][x, y] for a in range(n_agents)]
        else:
          highlights = highlight_mask[x, y]

        tile_img = Grid.render_tile(
            cell,
            highlight=highlights,
            tile_size=tile_size,
            cell_type=cell_type,
        )

        ymin = y * tile_size
        ymax = (y + 1) * tile_size
        xmin = x * tile_size
        xmax = (x + 1) * tile_size
        img[ymin:ymax, xmin:xmax, :] = tile_img

    return img


  def set_encoding(self, encoding, multigrid_env=None):
    assert tuple(encoding.shape[:2]) == (self.height, self.width)

    for i in range(self.height):
      for j in range(self.width):
        v = WorldObj.decode(*encoding[i,j,:])
        if isinstance(v, Agent):
          v.agent_id = 0

          if multigrid_env:
            multigrid_env.agent_start_pos = np.array((i,j), dtype=np.int64)

        elif isinstance(v, minigrid.Goal):
          if multigrid_env:
            multigrid_env.goal_pos = np.array((i,j), dtype=np.int64)
          
        self.set(i, j, v)

  @staticmethod
  def decode(array):
    """Decode an array grid encoding back into a grid."""
    width, height, channels = array.shape
    assert channels == 3

    vis_mask = np.ones(shape=(width, height), dtype=np.bool)

    grid = Grid(width, height)
    for i in range(width):
      for j in range(height):
        type_idx, color_idx, state = array[i, j]
        v = WorldObj.decode(type_idx, color_idx, state)
        grid.set(i, j, v)
        vis_mask[i, j] = (type_idx != minigrid.OBJECT_TO_IDX['unseen'])

    return grid, vis_mask

  def rotate_left(self):
    """Rotate the grid counter-clockwise, including agents within it."""
    grid = Grid(self.height, self.width)

    for i in range(self.width):
      for j in range(self.height):
        v = self.get(i, j)

        # Directions are relative to the agent so must be modified
        if v is not None and v.type == 'agent':
          # Make a new agent so original grid isn't modified
          v = Agent(v.agent_id, v.dir)
          v.dir -= 1
          if v.dir < 0:
            v.dir += 4

        grid.set(j, grid.height - 1 - i, v)

    return grid

  def slice(self, top_x, top_y, width, height, agent_pos=None):
    """Get a subset of the grid for agents' partial observations."""

    grid = Grid(width, height)

    for j in range(0, height):
      for i in range(0, width):
        x = top_x + i
        y = top_y + j

        if x >= 0 and x < self.width and \
           y >= 0 and y < self.height:
          v = self.get(x, y)
        else:
          v = minigrid.Wall()

        grid.set(i, j, v)

    return grid


class MultiGridEnv(minigrid.MiniGridEnv):
  """2D grid world game environment with multi-agent support."""

  def __init__(
      self,
      grid_size=None,
      width=None,
      height=None,
      max_steps=100,
      see_through_walls=False,
      seed=52,
      agent_view_size=7,
      n_agents=3,
      competitive=False,
      fixed_environment=False,
      minigrid_mode=False
  ):
    """Constructor for multi-agent gridworld environment generator.

    Args:
      grid_size: Number of tiles for the width and height of the square grid.
      width: Number of tiles across grid width.
      height: Number of tiles in height of grid.
      max_steps: Number of environment steps before the episode end (max
        episode length).
      see_through_walls: True if agents can see through walls.
      seed: Random seed used in generating environments.
      agent_view_size: Number of tiles in the agent's square, partially
        observed view of the world.
      n_agents: The number of agents playing in the world.
      competitive: If True, as soon as one agent locates the goal, the episode
        ends for all agents. If False, if one agent locates the goal it is
        respawned somewhere else in the grid, and the episode continues until
        max_steps is reached.
      fixed_environment: If True, will use the same random seed each time the
        environment is generated, so it will remain constant / be the same
        environment each time.
      minigrid_mode: Set to True to maintain backwards compatibility with
        minigrid in the single agent case.
    """
    # Can't set both grid_size and width/height
    if grid_size:
      assert width is None and height is None
      width = grid_size
      height = grid_size

    # Set the number of agents
    self.n_agents = n_agents

    # If competitive, only one agent is allowed to reach the goal.
    self.competitive = competitive

    if self.n_agents == 1:
      self.competitive = True

    # Action enumeration for this environment
    self.actions = MultiGridEnv.Actions

    # Number of cells (width and height) in the agent view
    self.agent_view_size = agent_view_size

    # Range of possible rewards
    self.reward_range = (0, 1)

    # Compute observation and action spaces
    # Direction always has an extra dimension for tf-agents compatibility
    self.direction_obs_space = gym.spaces.Box(
        low=0, high=3, shape=(self.n_agents,), dtype='uint8')

    # Maintain for backwards compatibility with minigrid.
    self.minigrid_mode = minigrid_mode
    if self.minigrid_mode:
      msg = 'Backwards compatibility with minigrid only possible with 1 agent'
      assert self.n_agents == 1, msg

      # Single agent case
      # Actions are discrete integer values
      self.action_space = gym.spaces.Discrete(len(self.actions))
      # Images have three dimensions
      self.image_obs_space = gym.spaces.Box(
          low=0,
          high=255,
          shape=(self.agent_view_size, self.agent_view_size, 3),
          dtype='uint8')
    else:
      # First dimension of all observations is the agent ID
      self.action_space = gym.spaces.Box(low=0, high=len(self.actions)-1,
                                         shape=(self.n_agents,), dtype='int64')

      self.image_obs_space = gym.spaces.Box(
          low=0,
          high=255,
          shape=(self.n_agents, self.agent_view_size, self.agent_view_size, 3),
          dtype='uint8')

    # Observations are dictionaries containing an encoding of the grid and the
    # agent's direction
    self.observation_space = gym.spaces.Dict(
        {'image': self.image_obs_space,
         'direction': self.direction_obs_space})

    # Window to use for human rendering mode
    self.window = None

    # Environment configuration
    self.width = width
    self.height = height
    self.max_steps = max_steps
    self.see_through_walls = see_through_walls

    # Current position and direction of the agent
    self.agent_pos = [None] * self.n_agents
    self.agent_dir = [None] * self.n_agents

    # Maintain a done variable for each agent
    self.done = [False] * self.n_agents

    # Initialize the RNG
    self.seed(seed=seed)
    self.fixed_environment = fixed_environment

    # Initialize the state
    self.reset()

  def seed(self, seed):
    super().seed(seed=seed)
    self.seed_value = seed
    return [seed]

  def reset(self):
    if self.fixed_environment:
      self.seed(self.seed_value)

    # Current position and direction of the agent
    self.agent_pos = [None] * self.n_agents
    self.agent_dir = [None] * self.n_agents
    self.done = [False] * self.n_agents

    # Generate the grid. Will be random by default, or same environment if
    # 'fixed_environment' is True.
    self._gen_grid(self.width, self.height)

    # These fields should be defined by _gen_grid
    for a in range(self.n_agents):
      assert self.agent_pos[a] is not None
      assert self.agent_dir[a] is not None

      # Check that the agent doesn't overlap with an object
      start_cell = self.grid.get(*self.agent_pos[a])
      assert (start_cell.type == 'agent' or
              start_cell is None or start_cell.can_overlap())

    # Item picked up, being carried, initially nothing
    self.carrying = [None] * self.n_agents

    # Step count since episode start
    self.step_count = 0

    # Return first observation
    obs = self.gen_obs()

    return obs

  def __str__(self):
    """Produce a pretty string of the environment's grid along with the agent.

    A grid cell is represented by 2-character string, the first one for
    the object and the second one for the color.

    Returns:
      String representation of the grid.
    """

    # Map of object types to short string
    obj_to_str = {
        'wall': 'W',
        'floor': 'F',
        'door': 'D',
        'key': 'K',
        'ball': 'A',
        'box': 'B',
        'goal': 'G',
        'lava': 'V',
    }

    # Map agent's direction to short string
    agent_dir_to_str = {0: '>', 1: 'V', 2: '<', 3: '^'}

    text = ''

    for j in range(self.grid.height):
      for i in range(self.grid.width):
        # Draw agents
        agent_here = False
        for a in range(self.n_agents):
          if self.agent_pos[a] is not None and (i == self.agent_pos[a][0] and
                                                j == self.agent_pos[a][1]):
            text += str(a) + agent_dir_to_str[self.agent_dir[a]]
            agent_here = True
        if agent_here:
          continue

        c = self.grid.get(i, j)

        if c is None:
          text += '  '
          continue

        if c.type == 'door':
          if c.is_open:
            text += '__'
          elif c.is_locked:
            text += 'L' + c.color[0].upper()
          else:
            text += 'D' + c.color[0].upper()
          continue

        text += obj_to_str[c.type] + c.color[0].upper()

      if j < self.grid.height - 1:
        text += '\n'

    return text

  def place_obj(self,
                obj,
                top=None,
                size=None,
                reject_fn=None,
                max_tries=math.inf):
    """Place an object at an empty position in the grid.

    Args:
      obj: Instance of Minigrid WorldObj class (such as Door, Key, etc.).
      top: (x,y) position of the top-left corner of rectangle where to place.
      size: Size of the rectangle where to place.
      reject_fn: Function to filter out potential positions.
      max_tries: Throw an error if a position can't be found after this many
        tries.

    Returns:
      Position where object was placed.
    """
    if top is None:
      top = (0, 0)
    else:
      top = (max(top[0], 0), max(top[1], 0))

    if size is None:
      size = (self.grid.width, self.grid.height)

    num_tries = 0

    while True:
      # This is to handle with rare cases where rejection sampling
      # gets stuck in an infinite loop
      if num_tries > max_tries:
        raise gym.error.RetriesExceededError(
            'Rejection sampling failed in place_obj')

      num_tries += 1

      pos = np.array((self._rand_int(top[0],
                                     min(top[0] + size[0], self.grid.width)),
                      self._rand_int(top[1],
                                     min(top[1] + size[1], self.grid.height))))

      # Don't place the object on top of another object
      if self.grid.get(*pos) is not None:
        continue

      # Don't place the object where the agent is
      pos_no_good = False
      for a in range(self.n_agents):
        if np.array_equal(pos, self.agent_pos[a]):
          pos_no_good = True
      if pos_no_good:
        continue

      # Check if there is a filtering criterion
      if reject_fn and reject_fn(self, pos):
        continue

      break

    self.grid.set(pos[0], pos[1], obj)

    if obj is not None:
      obj.init_pos = pos
      obj.cur_pos = pos

    return pos

  def place_agent(self, top=None, size=None, rand_dir=True, max_tries=math.inf):
    """Set the starting point of all agents in the world.

    Name chosen for backwards compatibility.

    Args:
      top: (x,y) position of the top-left corner of rectangle where agents can
        be placed.
      size: Size of the rectangle where to place.
      rand_dir: Choose a random direction for agents.
      max_tries: Throw an error if a position can't be found after this many
        tries.
    """
    for a in range(self.n_agents):
      self.place_one_agent(
          a, top=top, size=size, rand_dir=rand_dir, max_tries=math.inf)

  def place_one_agent(self,
                      agent_id,
                      top=None,
                      size=None,
                      rand_dir=True,
                      max_tries=math.inf,
                      agent_obj=None):
    """Set the agent's starting point at an empty position in the grid."""

    self.agent_pos[agent_id] = None
    pos = self.place_obj(None, top, size, max_tries=max_tries)

    self.place_agent_at_pos(agent_id, pos, agent_obj=agent_obj,
                            rand_dir=rand_dir)

    return pos

  def place_agent_at_pos(self, agent_id, pos, agent_obj=None, rand_dir=True):
    self.agent_pos[agent_id] = pos
    if rand_dir:
      # self.agent_dir[agent_id] = self._rand_int(0, 4)
      self.agent_dir[agent_id] = 0

    # Place the agent object into the grid
    if not agent_obj:
      agent_obj = Agent(agent_id, self.agent_dir[agent_id])
      agent_obj.init_pos = pos
    else:
      agent_obj.dir = self.agent_dir[agent_id]
    agent_obj.cur_pos = pos
    self.grid.set(pos[0], pos[1], agent_obj)

  @property
  def dir_vec(self):
    """Get the direction vector for the agent (points toward forward movement).

    Returns:
      An array of directions that each agent is facing.
    """

    for a in range(self.n_agents):
      assert self.agent_dir[a] >= 0 and self.agent_dir[a] < 4
    return [
        minigrid.DIR_TO_VEC[self.agent_dir[a]] for a in range(self.n_agents)
    ]

  @property
  def right_vec(self):
    """Get the vector pointing to the right of the agents."""
    return [np.array((-dy, dx)) for (dx, dy) in self.dir_vec]

  @property
  def front_pos(self):
    """Get the position of the cell that is right in front of the agent."""
    front_pos = [None] * self.n_agents
    for a in range(self.n_agents):
      assert self.agent_pos[a] is not None and self.dir_vec[a] is not None
      front_pos[a] = self.agent_pos[a] + self.dir_vec[a]
    return front_pos

  def get_view_coords(self, i, j, agent_id):
    """Convert grid coordinates into agent's partially observed view.

    Translate and rotate absolute grid coordinates (i, j) into the agent's
    partially observable view (sub-grid).

    Note that the resulting coordinates may be negative or outside of the
    agent's view size.

    Args:
      i: Integer x coordinate.
      j: Integer y coordinate.
      agent_id: ID of the agent.

    Returns:
      Agent-centric coordinates.
    """

    ax, ay = self.agent_pos[agent_id]
    dx, dy = self.dir_vec[agent_id]
    rx, ry = self.right_vec[agent_id]

    # Compute the absolute coordinates of the top-left view corner
    sz = self.agent_view_size
    hs = self.agent_view_size // 2
    tx = ax + (dx * (sz - 1)) - (rx * hs)
    ty = ay + (dy * (sz - 1)) - (ry * hs)

    lx = i - tx
    ly = j - ty

    # Project the coordinates of the object relative to the top-left
    # corner onto the agent's own coordinate system
    vx = (rx * lx + ry * ly)
    vy = -(dx * lx + dy * ly)

    return vx, vy

  def get_view_exts(self, agent_id):
    """Get the extents of the square set of tiles visible to the agent.

    Note: the bottom extent indices are not included in the set

    Args:
      agent_id: Integer ID of the agent.

    Returns:
      Top left and bottom right (x,y) coordinates of set of visible tiles.
    """
    # Facing right
    if self.agent_dir[agent_id] == 0:
      top_x = self.agent_pos[agent_id][0]
      top_y = self.agent_pos[agent_id][1] - self.agent_view_size // 2
    # Facing down
    elif self.agent_dir[agent_id] == 1:
      top_x = self.agent_pos[agent_id][0] - self.agent_view_size // 2
      top_y = self.agent_pos[agent_id][1]
    # Facing left
    elif self.agent_dir[agent_id] == 2:
      top_x = self.agent_pos[agent_id][0] - self.agent_view_size + 1
      top_y = self.agent_pos[agent_id][1] - self.agent_view_size // 2
    # Facing up
    elif self.agent_dir[agent_id] == 3:
      top_x = self.agent_pos[agent_id][0] - self.agent_view_size // 2
      top_y = self.agent_pos[agent_id][1] - self.agent_view_size + 1
    else:
      assert False, 'invalid agent direction'

    bot_x = top_x + self.agent_view_size
    bot_y = top_y + self.agent_view_size

    return (top_x, top_y, bot_x, bot_y)

  def relative_coords(self, x, y, agent_id):
    """Check if a grid position belongs to the agent's field of view.

    Args:
      x: Integer x coordinate.
      y: Integer y coordinate.
      agent_id: ID of the agent.

    Returns:
      The corresponding agent-centric coordinates of the grid position.
    """
    vx, vy = self.get_view_coords(x, y, agent_id)

    if (vx < 0 or vy < 0 or vx >= self.agent_view_size or
        vy >= self.agent_view_size):
      return None

    return vx, vy

  def in_view(self, x, y, agent_id):
    """Check if a grid position is visible to the agent."""
    return self.relative_coords(x, y, agent_id) is not None

  def agent_sees(self, x, y, agent_id):
    """Check if a non-empty grid position is visible to the agent."""
    coordinates = self.relative_coords(x, y, agent_id)
    if coordinates is None:
      return False
    vx, vy = coordinates

    obs = self.gen_obs()
    obs_grid, _ = Grid.decode(obs['image'][agent_id])
    obs_cell = obs_grid.get(vx, vy)
    world_cell = self.grid.get(x, y)

    return obs_cell is not None and obs_cell.type == world_cell.type

  def agent_is_done(self, agent_id):
    # Remove correspnding agent object from the grid
    pos = self.agent_pos[agent_id]
    agent_obj = self.grid.get(pos[0], pos[1])
    self.grid.set(pos[0], pos[1], None)

    self.done[agent_id] = True

    # If an agent finishes the level while carrying an object, it is randomly
    # respawned in a new position. Warning: this may break dependencies for the
    # level (e.g. if a key is spawned on the wrong side of a door).
    # TODO(natashajaques): environments can define respawn behavior
    if self.carrying[agent_id]:
      self.place_obj(obj=self.carrying[agent_id])
      self.carrying[agent_id] = None

    # Respawn agent in new location
    self.place_one_agent(agent_id, agent_obj=agent_obj)

  def move_agent(self, agent_id, new_pos):
    # Retrieve agent object
    old_pos = self.agent_pos[agent_id]
    agent_obj = self.grid.get(old_pos[0], old_pos[1])
    assert agent_obj.agent_id == agent_id
    assert (agent_obj.cur_pos == old_pos).all()

    # Update the agent position in grid and environment
    self.grid.set(old_pos[0], old_pos[1], None)
    self.agent_pos[agent_id] = new_pos
    agent_obj.cur_pos = new_pos
    self.grid.set(new_pos[0], new_pos[1], agent_obj)
    assert (self.grid.get(
        new_pos[0], new_pos[1]).cur_pos == self.agent_pos[agent_id]).all()

  def rotate_agent(self, agent_id):
    # Retrieve agent object
    pos = self.agent_pos[agent_id]
    agent_obj = self.grid.get(pos[0], pos[1])
    assert agent_obj.agent_id == agent_id

    # Update the dir
    agent_obj.dir = self.agent_dir[agent_id]
    self.grid.set(pos[0], pos[1], agent_obj)
    assert self.grid.get(pos[0], pos[1]).dir == self.agent_dir[agent_id]

  def step_one_agent(self, action, agent_id):
    reward = 0

    # Get the position in front of the agent
    fwd_pos = self.front_pos[agent_id]

    # Get the contents of the cell in front of the agent
    fwd_cell = self.grid.get(*fwd_pos)

    # Rotate left
    if action == self.actions.left:
      self.agent_dir[agent_id] -= 1
      if self.agent_dir[agent_id] < 0:
        self.agent_dir[agent_id] += 4
      self.rotate_agent(agent_id)

    # Rotate right
    elif action == self.actions.right:
      self.agent_dir[agent_id] = (self.agent_dir[agent_id] + 1) % 4
      self.rotate_agent(agent_id)

    # Move forward
    elif action == self.actions.forward:
      # Make sure agents can't walk into each other
      agent_blocking = False
      for a in range(self.n_agents):
        if a != agent_id and np.array_equal(self.agent_pos[a], fwd_pos):
          agent_blocking = True

      # Deal with object interactions
      if not agent_blocking:
        if fwd_cell is not None and fwd_cell.type == 'goal':
          self.agent_is_done(agent_id)
          reward = self._reward()
        elif fwd_cell is not None and fwd_cell.type == 'lava':
          self.agent_is_done(agent_id)
        elif fwd_cell is None or fwd_cell.can_overlap():
          self.move_agent(agent_id, fwd_pos)

    # Pick up an object
    elif action == self.actions.pickup:
      if fwd_cell and fwd_cell.can_pickup():
        if self.carrying[agent_id] is None:
          self.carrying[agent_id] = fwd_cell
          self.carrying[agent_id].cur_pos = np.array([-1, -1])
          self.grid.set(fwd_pos[0], fwd_pos[1], None)
          a_pos = self.agent_pos[agent_id]
          agent_obj = self.grid.get(a_pos[0], a_pos[1])
          agent_obj.contains = fwd_cell

    # Drop an object
    elif action == self.actions.drop:
      if not fwd_cell and self.carrying[agent_id]:
        self.grid.set(fwd_pos[0], fwd_pos[1], self.carrying[agent_id])
        self.carrying[agent_id].cur_pos = fwd_pos
        self.carrying[agent_id] = None
        a_pos = self.agent_pos[agent_id]
        agent_obj = self.grid.get(a_pos[0], a_pos[1])
        agent_obj.contains = None

    # Toggle/activate an object
    elif action == self.actions.toggle:
      if fwd_cell:
        if fwd_cell.type == 'door':
          fwd_cell.toggle(self, fwd_pos, self.carrying[agent_id])
        else:
          fwd_cell.toggle(self, fwd_pos)

    # Done action -- by default acts as no-op.
    elif action == self.actions.done:
      pass

    else:
      assert False, 'unknown action'

    return reward

  def step(self, actions):
    # Maintain backwards compatibility with MiniGrid when there is one agent
    if not isinstance(actions, list) and self.n_agents == 1:
      actions = [actions]

    self.step_count += 1

    rewards = [0] * self.n_agents

    # Randomize order in which agents act for fairness
    agent_ordering = np.arange(self.n_agents)
    np.random.shuffle(agent_ordering)

    # Step each agent
    for a in agent_ordering:
      rewards[a] = self.step_one_agent(actions[a], a)

    obs = self.gen_obs()

    # Backwards compatibility
    if self.minigrid_mode:
      rewards = rewards[0]

    collective_done = False
    # In competitive version, if one agent finishes the episode is over.
    if self.competitive:
      collective_done = np.sum(self.done) >= 1

    # Running out of time applies to all agents
    if self.step_count >= self.max_steps:
      collective_done = True

    return obs, rewards, collective_done, {}

  def gen_obs_grid(self, agent_id):
    """Generate the sub-grid observed by the agent.

    This method also outputs a visibility mask telling us which grid cells
    the agent can actually see.

    Args:
      agent_id: Integer ID of the agent for which to generate the grid.

    Returns:
      Sub-grid and visibility mask.
    """

    top_x, top_y, _, _ = self.get_view_exts(agent_id)

    grid = self.grid.slice(top_x, top_y, self.agent_view_size,
                           self.agent_view_size)

    for _ in range(self.agent_dir[agent_id] + 1):
      grid = grid.rotate_left()

    # Process occluders and visibility
    # Note that this incurs some performance cost
    if not self.see_through_walls:
      vis_mask = grid.process_vis(
          agent_pos=(self.agent_view_size // 2, self.agent_view_size - 1))
    else:
      vis_mask = np.ones(shape=(grid.width, grid.height), dtype=np.bool)

    # Make it so the agent sees what it's carrying
    # We do this by placing the carried object at the agent's position
    # in the agent's partially observable view
    agent_pos = grid.width // 2, grid.height - 1
    if self.carrying[agent_id]:
      grid.set(agent_pos[0], agent_pos[1], self.carrying[agent_id])
    else:
      grid.set(agent_pos[0], agent_pos[1], None)

    return grid, vis_mask

  def gen_obs(self):
    """Generate the stacked observation for all agents."""
    images = []
    dirs = []
    for a in range(self.n_agents):
      image, direction = self.gen_agent_obs(a)
      images.append(image)
      dirs.append(direction)

    # Backwards compatibility: if there is a single agent do not return an array
    if self.minigrid_mode:
      images = images[0]

    # Observations are dictionaries containing:
    # - an image (partially observable view of the environment)
    # - the agent's direction/orientation (acting as a compass)
    # Note direction has shape (1,) for tfagents compatibility
    obs = {
        'image': images,
        'direction': dirs
    }

    return obs

  def gen_agent_obs(self, agent_id):
    """Generate the agent's view (partially observed, low-resolution encoding).

    Args:
      agent_id: ID of the agent for which to generate the observation.

    Returns:
      3-dimensional partially observed agent-centric view, and int direction
    """
    grid, vis_mask = self.gen_obs_grid(agent_id)

    # Encode the partially observable view into a numpy array
    image = grid.encode(vis_mask)

    return image, self.agent_dir[agent_id]

  def get_obs_render(self, obs, tile_size=minigrid.TILE_PIXELS // 2):
    """Render an agent observation for visualization."""

    grid, vis_mask = Grid.decode(obs)

    # Render the whole grid
    img = grid.render(
        tile_size,
        # agent_pos=self.agent_pos,
        # agent_dir=self.agent_dir,
        highlight_mask=vis_mask)

    return img

  def compute_agent_visibility_mask(self, agent_id):
    # Mask of which cells to highlight
    highlight_mask = np.zeros(shape=(self.width, self.height), dtype=np.bool)

    # Compute which cells are visible to the agent
    _, vis_mask = self.gen_obs_grid(agent_id)

    # Compute the world coordinates of the bottom-left corner
    # of the agent's view area
    f_vec = self.dir_vec[agent_id]
    r_vec = self.right_vec[agent_id]
    top_left = self.agent_pos[agent_id] + f_vec * (self.agent_view_size-1) - \
        r_vec * (self.agent_view_size // 2)

    # For each cell in the visibility mask
    for vis_j in range(0, self.agent_view_size):
      for vis_i in range(0, self.agent_view_size):
        # If this cell is not visible, don't highlight it
        if not vis_mask[vis_i, vis_j]:
          continue

        # Compute the world coordinates of this cell
        abs_i, abs_j = top_left - (f_vec * vis_j) + (r_vec * vis_i)

        if abs_i < 0 or abs_i >= self.width:
          continue
        if abs_j < 0 or abs_j >= self.height:
          continue

        # Mark this cell to be highlighted
        highlight_mask[abs_i, abs_j] = True

    return highlight_mask

  def render(self,
             mode='human',
             close=False,
             highlight=True,
             tile_size=minigrid.TILE_PIXELS):
    """Render the whole-grid human view."""

    if close:
      if self.window:
        self.window.close()
      return None

    if mode == 'human' and not self.window:
      self.window = window.Window('gym_minigrid')
      self.window.show(block=False)

    if highlight:
      highlight_mask = []
      for a in range(self.n_agents):
        if self.agent_pos[a] is not None:
          highlight_mask.append(self.compute_agent_visibility_mask(a))
    else:
      highlight_mask = None

    # Render the whole grid
    img = self.grid.render(tile_size, highlight_mask=highlight_mask)

    if mode == 'human':
      self.window.show_img(img)
      if hasattr(self, 'mission'):
        self.window.set_caption(self.mission)

    if mode == 'human':
      self.window.show()

    return img
