# Copyright 2019 The dm_control 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.
# ============================================================================

"""A soccer ball that keeps track of ball-player contacts."""

import os

from dm_control import mjcf
from dm_control.entities import props
import numpy as np

from dm_control.utils import io as resources

_ASSETS_PATH = os.path.join(os.path.dirname(__file__), 'assets', 'soccer_ball')

# FIFA regulation parameters for a size 5 ball.
_REGULATION_RADIUS = 0.117  # Meters.
_REGULATION_MASS = 0.45  # Kilograms.

_DEFAULT_FRICTION = (0.7, 0.05, 0.04)  # (slide, spin, roll).
_DEFAULT_DAMP_RATIO = 0.4


def _get_texture(name):
  contents = resources.GetResource(
      os.path.join(_ASSETS_PATH, '{}.png'.format(name)))
  return mjcf.Asset(contents, '.png')


def regulation_soccer_ball():
  return SoccerBall(
      radius=_REGULATION_RADIUS,
      mass=_REGULATION_MASS,
      friction=_DEFAULT_FRICTION,
      damp_ratio=_DEFAULT_DAMP_RATIO)


class SoccerBall(props.Primitive):
  """A soccer ball that keeps track of entities that come into contact."""

  def _build(self,
             radius=0.35,
             mass=0.045,
             friction=(0.7, 0.075, 0.075),
             damp_ratio=1.0,
             name='soccer_ball'):
    """Builds this soccer ball.

    Args:
      radius: The radius (in meters) of this target sphere.
      mass: Mass (in kilograms) of the ball.
      friction: Friction parameters of the ball geom with the three dimensions
        corresponding to (slide, spin, roll) frictions.
      damp_ratio: A real positive number. Lower implies less dampening upon
        contacts.
      name: The name of this entity.
    """
    super()._build(geom_type='sphere', size=(radius,), name=name)
    texture = self._mjcf_root.asset.add(
        'texture',
        name='soccer_ball',
        type='cube',
        fileup=_get_texture('up'),
        filedown=_get_texture('down'),
        filefront=_get_texture('front'),
        fileback=_get_texture('back'),
        fileleft=_get_texture('left'),
        fileright=_get_texture('right'))
    material = self._mjcf_root.asset.add(
        'material', name='soccer_ball', texture=texture)

    if damp_ratio < 0.0:
      raise ValueError(
          f'Invalid `damp_ratio` parameter ({damp_ratio} is not positive).')

    self._geom.set_attributes(
        pos=[0, 0, radius],
        size=[radius],
        condim=6,
        priority=1,
        mass=mass,
        friction=friction,
        solref=[0.02, damp_ratio],
        material=material)

    # Add some tracking cameras for visualization and logging.
    self._mjcf_root.worldbody.add(
        'camera',
        name='ball_cam_near',
        pos=[0, -2, 2],
        zaxis=[0, -1, 1],
        fovy=70,
        mode='trackcom')
    self._mjcf_root.worldbody.add(
        'camera',
        name='ball_cam',
        pos=[0, -7, 7],
        zaxis=[0, -1, 1],
        fovy=70,
        mode='trackcom')
    self._mjcf_root.worldbody.add(
        'camera',
        name='ball_cam_far',
        pos=[0, -10, 10],
        zaxis=[0, -1, 1],
        fovy=70,
        mode='trackcom')

    # Keep track of entities to team mapping.
    self._players = []

    # Initialize tracker attributes.
    self.initialize_entity_trackers()

  def register_player(self, player):
    self._players.append(player)

  def initialize_entity_trackers(self):
    self._last_hit = None
    self._hit = False
    self._repossessed = False
    self._intercepted = False

    # Tracks distance traveled by the ball in between consecutive hits.
    self._pos_at_last_step = None
    self._dist_since_last_hit = None
    self._dist_between_last_hits = None

  def initialize_episode(self, physics, unused_random_state):
    self._geom_id = physics.model.name2id(self._geom.full_identifier, 'geom')
    self._geom_id_to_player = {}
    for player in self._players:
      geoms = player.walker.mjcf_model.find_all('geom')
      for geom in geoms:
        geom_id = physics.model.name2id(geom.full_identifier, 'geom')
        self._geom_id_to_player[geom_id] = player

    self.initialize_entity_trackers()

  def after_substep(self, physics, unused_random_state):
    """Resolve contacts and update ball-player contact trackers."""
    if self._hit:
      # Ball has already registered a valid contact within step (during one of
      # previous after_substep calls).
      return

    # Iterate through all contacts to find the first contact between the ball
    # and one of the registered entities.
    for contact in physics.data.contact:
      # Keep contacts that involve the ball and one of the registered entities.
      has_self = False
      for geom_id in (contact.geom1, contact.geom2):
        if geom_id == self._geom_id:
          has_self = True
        else:
          player = self._geom_id_to_player.get(geom_id)

      if has_self and player:
        # Detected a contact between the ball and an registered player.
        if self._last_hit is not None:
          self._intercepted = player.team != self._last_hit.team
        else:
          self._intercepted = True

        # Register repossessed before updating last_hit player.
        self._repossessed = player is not self._last_hit
        self._last_hit = player
        # Register hit event.
        self._hit = True
        break

  def before_step(self, physics, random_state):
    super().before_step(physics, random_state)
    # Reset per simulation step indicator.
    self._hit = False
    self._repossessed = False
    self._intercepted = False

  def after_step(self, physics, random_state):
    super().after_step(physics, random_state)
    pos = physics.bind(self._geom).xpos
    if self._hit:
      # SoccerBall is hit on this step. Update dist_between_last_hits
      # to dist_since_last_hit before resetting dist_since_last_hit.
      self._dist_between_last_hits = self._dist_since_last_hit
      self._dist_since_last_hit = 0.
      self._pos_at_last_step = pos.copy()

    if self._dist_since_last_hit is not None:
      # Accumulate distance traveled since last hit event.
      self._dist_since_last_hit += np.linalg.norm(pos - self._pos_at_last_step)

    self._pos_at_last_step = pos.copy()

  @property
  def last_hit(self):
    """The player that last came in contact with the ball or `None`."""
    return self._last_hit

  @property
  def hit(self):
    """Indicates if the ball is hit during the last simulation step.

    For a timeline shown below:
      ..., agent.step, simulation, agent.step, ...

    Returns:
      True: if the ball is hit by a registered player during simulation step.
      False: if not.
    return self._hit

  @property
  def repossessed(self):
    """Indicates if the ball has been repossessed by a different player.

    For a timeline shown below:
      ..., agent.step, simulation, agent.step, ...

    Returns:
      True if the ball is hit by a registered player during simulation step
        and that player is different from `last_hit`.
      False: if the ball is not hit, or the ball is hit by `last_hit` player.
    """
    return self._repossessed

  @property
  def intercepted(self):
    """Indicates if the ball has been intercepted by a different team.

    For a timeline shown below:
      ..., agent.step, simulation, agent.step, ...

    Returns:
      True: if the ball is hit for the first time, or repossessed by an player
        from a different team.
      False: if the ball is not hit, not repossessed, or repossessed by a
        teammate to `last_hit`.
    """
    return self._intercepted

  @property
  def dist_between_last_hits(self):
    """Distance between last consecutive hits.

    Returns:
      Distance between last two consecutive hit events or `None` if there has
        not been two consecutive hits on the ball.
    """
    return self._dist_between_last_hits
    """