text: """Utility functions for environment.py"""
  
  import os
  import cv2
  import math
  import random
  import numpy as np
  import carla
  import pygame
  import threading
  import datetime
  
  from typing import Union
  from tensorforce.agents import Agent
  
  
  # constants:
  epsilon = np.finfo(np.float32).eps
  
  # Use this dict to convert lanes objects to integers:
  WAYPOINT_DICT = dict(lane_change={carla.LaneChange.NONE: 0,
                                    carla.LaneChange.Both: 1,
                                    carla.LaneChange.Left: 2,
                                    carla.LaneChange.Right: 3},
                       lane_type={carla.LaneType.NONE: 0,
                                  carla.LaneType.Bidirectional: 1,
                                  carla.LaneType.Biking: 2,
                                  carla.LaneType.Border: 3,
                                  carla.LaneType.Driving: 4,
                                  carla.LaneType.Entry: 5,
                                  carla.LaneType.Exit: 6,
                                  carla.LaneType.Median: 7,
                                  carla.LaneType.OffRamp: 8,
                                  carla.LaneType.OnRamp: 9,
                                  carla.LaneType.Parking: 10,
                                  carla.LaneType.Rail: 11,
                                  carla.LaneType.Restricted: 12,
                                  carla.LaneType.RoadWorks: 13,
                                  carla.LaneType.Shoulder: 14,
                                  carla.LaneType.Sidewalk: 15,
                                  carla.LaneType.Special1: 16,
                                  carla.LaneType.Special2: 17,
                                  carla.LaneType.Special3: 18,
                                  carla.LaneType.Stop: 19,
                                  carla.LaneType.Tram: 20,
                                  carla.LaneType.Any: 21},
                       lane_marking_type={carla.LaneMarkingType.NONE: 0,
                                          carla.LaneMarkingType.BottsDots: 1,
                                          carla.LaneMarkingType.Broken: 2,
                                          carla.LaneMarkingType.BrokenBroken: 3,
                                          carla.LaneMarkingType.BrokenSolid: 4,
                                          carla.LaneMarkingType.Curb: 5,
                                          carla.LaneMarkingType.Grass: 6,
                                          carla.LaneMarkingType.Solid: 7,
                                          carla.LaneMarkingType.SolidBroken: 8,
                                          carla.LaneMarkingType.SolidSolid: 9,
                                          carla.LaneMarkingType.Other: 10},
                       traffic_light={carla.TrafficLightState.Green: 0,
                                      carla.TrafficLightState.Red: 1,
                                      carla.TrafficLightState.Yellow: 2,
                                      carla.TrafficLightState.Off: 3,
                                      carla.TrafficLightState.Unknown: 4}
                       )
  
  
  # -------------------------------------------------------------------------------------------------
  # -- PyGame
  # -------------------------------------------------------------------------------------------------
  
  def init_pygame():
      if not pygame.get_init():
          pygame.init()
  
      if not pygame.font.get_init():
          pygame.font.init()
  
  
  def get_display(window_size, mode=pygame.HWSURFACE | pygame.DOUBLEBUF):
      """Returns a display used to render images and text.
          :param window_size: a tuple (width: int, height: int)
          :param mode: pygame rendering mode. Default: pygame.HWSURFACE | pygame.DOUBLEBUF
          :return: a pygame.display instance.
      """
      return pygame.display.set_mode(window_size, mode)
  
  
  def get_font(size=14):
      return pygame.font.Font(pygame.font.get_default_font(), size)
  
  
  def display_image(display, image, window_size=(800, 600), blend=False):
      """Displays the given image on a pygame window
      :param blend: whether to blend or not the given image.
      :param window_size: the size of the pygame's window. Default is (800, 600)
      :param display: pygame.display
      :param image: the image (numpy.array) to display/render on.
      """
      # Resize image if necessary
      if (image.shape[1], image.shape[0]) != window_size:
          image = resize(image, size=window_size)
  
      image_surface = pygame.surfarray.make_surface(image.swapaxes(0, 1))
  
      if blend:
          image_surface.set_alpha(100)
  
      display.blit(image_surface, (0, 0))
  
  
  def display_text(display, font, text: [str], color=(255, 255, 255), origin=(0, 0), offset=(0, 2)):
      position = origin
  
      for line in text:
          if isinstance(line, dict):
              display.blit(font.render(line.get('text'), True, line.get('color', color)), position)
          else:
              display.blit(font.render(line, True, color), position)
  
          position = (position[0] + offset[0], position[1] + offset[1])
  
  
  def pygame_save(display, path: str, name: str = None):
      if name is None:
          name = 'image-' + str(datetime.datetime.now()) + '.jpg'
  
      thread = threading.Thread(target=lambda: pygame.image.save(display, os.path.join(path, name)))
      thread.start()
  
  
  # -------------------------------------------------------------------------------------------------
  # -- CARLA
  # -------------------------------------------------------------------------------------------------
  
  def get_client(address, port, timeout=2.0) -> carla.Client:
      """Connects to the simulator.
          @:returns a carla.Client instance if the CARLA simulator accepts the connection.
      """
      client: carla.Client = carla.Client(address, port)
      client.set_timeout(timeout)
      return client
  
  
  def random_blueprint(world: carla.World, actor_filter='vehicle.*', role_name='agent') -> carla.ActorBlueprint:
      """Retrieves a random blueprint.
          :param world: a carla.World instance.
          :param actor_filter: a string used to filter (select) blueprints. Default: 'vehicle.*'
          :param role_name: blueprint's role_name, Default: 'agent'.
          :return: a carla.ActorBlueprint instance.
      """
      blueprints = world.get_blueprint_library().filter(actor_filter)
      blueprint: carla.ActorBlueprint = random.choice(blueprints)
      blueprint.set_attribute('role_name', role_name)
  
      if blueprint.has_attribute('color'):
          color = random.choice(blueprint.get_attribute('color').recommended_values)
          blueprint.set_attribute('color', color)
  
      if blueprint.has_attribute('driver_id'):
          driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values)
          blueprint.set_attribute('driver_id', driver_id)
  
      if blueprint.has_attribute('is_invincible'):
          blueprint.set_attribute('is_invincible', 'true')
  
      # set the max speed
      if blueprint.has_attribute('speed'):
          float(blueprint.get_attribute('speed').recommended_values[1])
          float(blueprint.get_attribute('speed').recommended_values[2])
      else:
          print("No recommended values for 'speed' attribute")
  
      return blueprint
  
  
  def random_spawn_point(world_map: carla.Map, different_from: carla.Location = None) -> carla.Transform:
      """Returns a random spawning location.
          :param world_map: a carla.Map instance obtained by calling world.get_map()
          :param different_from: ensures that the location of the random spawn point is different from the one specified here.
          :return: a carla.Transform instance.
      """
      available_spawn_points = world_map.get_spawn_points()
  
      if different_from is not None:
          while True:
              spawn_point = random.choice(available_spawn_points)
  
              if spawn_point.location != different_from:
                  return spawn_point
      else:
          return random.choice(available_spawn_points)
  
  
  def spawn_actor(world: carla.World, blueprint: carla.ActorBlueprint, spawn_point: carla.Transform,
                  attach_to: carla.Actor = None, attachment_type=carla.AttachmentType.Rigid) -> carla.Actor:
      """Tries to spawn an actor in a CARLA simulator.
          :param world: a carla.World instance.
          :param blueprint: specifies which actor has to be spawned.
          :param spawn_point: where to spawn the actor. A transform specifies the location and rotation.
          :param attach_to: whether the spawned actor has to be attached (linked) to another one.
          :param attachment_type: the kind of the attachment. Can be 'Rigid' or 'SpringArm'.
          :return: a carla.Actor instance.
      """
      actor = world.try_spawn_actor(blueprint, spawn_point, attach_to, attachment_type)
  
      if actor is None:
          raise ValueError(f'Cannot spawn actor. Try changing the spawn_point ({spawn_point}) to something else.')
  
      return actor
  
  
  def get_blueprint(world: carla.World, actor_id: str) -> carla.ActorBlueprint:
      return world.get_blueprint_library().find(actor_id)
  
  
  def global_to_local(point: carla.Location, reference: Union[carla.Transform, carla.Location, carla.Rotation]):
      """Translates a 3D point from global to local coordinates using the current transformation as reference"""
      if isinstance(reference, carla.Transform):
          reference.transform(point)
      elif isinstance(reference, carla.Location):
          carla.Transform(reference, carla.Rotation()).transform(point)
      elif isinstance(reference, carla.Rotation):
          carla.Transform(carla.Location(), reference).transform(point)
      else:
          raise ValueError('Argument "reference" is none of carla.Transform or carla.Location or carla.Rotation!')
  
  
  # -------------------------------------------------------------------------------------------------
  # -- Other
  # -------------------------------------------------------------------------------------------------
  
  def resize(image, size: (int, int), interpolation=cv2.INTER_CUBIC):
      """Resize the given image.
          :param image: a numpy array with shape (height, width, channels).
          :param size: (width, height) to resize the image to.
          :param interpolation: Default: cv2.INTER_CUBIC.
          :return: the reshaped image.
      """
      return cv2.resize(image, dsize=size, interpolation=interpolation)
  
  
  def scale(num, from_interval=(-1.0, +1.0), to_interval=(0.0, 7.0)) -> float:
      """Scales (interpolates) the given number to a given interval.
          :param num: a number
          :param from_interval: the interval the number is assumed to lie in.
          :param to_interval: the target interval.
          :return: the scaled/interpolated number.
      """
      x = np.interp(num, from_interval, to_interval)
      return float(round(x))
  
  
  def cv2_grayscale(image, is_bgr=True, depth=1):
      """Convert a RGB or BGR image to grayscale using OpenCV (cv2).
          :param image: input image, a numpy.ndarray.
          :param is_bgr: tells whether the image is in BGR format. If False, RGB format is assumed.
          :param depth: replicates the gray depth channel multiple times. E.g. useful to display grayscale images as rgb.
      """
      assert depth >= 1
  
      if is_bgr:
          grayscale = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
      else:
          grayscale = cv2.cvtColor(image, cv2.COLOR_RGB2GRAY)
  
      if depth > 1:
          return np.stack((grayscale,) * depth, axis=-1)
  
      return grayscale
  
  
  def save_agent(agent: Agent, agent_name: str, directory: str, separate_dir=True) -> str:
      if separate_dir:
          save_path = os.path.join(directory, agent_name)
          os.makedirs(save_path, exist_ok=True)
      else:
          save_path = directory
  
      checkpoint_path = agent.save(directory=save_path, filename=agent_name)
      return checkpoint_path
  
  
  def get_record_path(base_dir: str, prefix='ep', pattern='-'):
      dirs = sorted(os.listdir(base_dir))
      count = 0
  
      if len(dirs) > 0:
          count = 1 + int(dirs[-1].split(pattern)[1])
  
      record_path = os.path.join(base_dir, f'{prefix}{pattern}{count}')
      os.mkdir(record_path)
  
      return record_path
  
  
  def replace_nans(data: dict, nan=0.0, pos_inf=0.0, neg_inf=0.0):
      """In-place replacement of non-numerical values, i.e. NaNs and +/- infinity"""
      for key, value in data.items():
          if np.isnan(value).any() or np.isinf(value).any():
              data[key] = np.nan_to_num(value, nan=nan, posinf=pos_inf, neginf=neg_inf)
  
      return data
  
  
  # -------------------------------------------------------------------------------------------------
  # -- Debug
  # -------------------------------------------------------------------------------------------------
  
  class Colors(object):
      """Wraps some carla.Color instances."""
      red = carla.Color(255, 0, 0)
      green = carla.Color(0, 255, 0)
      blue = carla.Color(47, 210, 231)
      cyan = carla.Color(0, 255, 255)
      yellow = carla.Color(255, 255, 0)
      orange = carla.Color(255, 162, 0)
      white = carla.Color(255, 255, 255)
      black = carla.Color(0, 0, 0)
  
  
  def draw_transform(debug, trans, col=Colors.red, lt=-1):
      yaw_in_rad = math.radians(trans.rotation.yaw)
      pitch_in_rad = math.radians(trans.rotation.pitch)
  
      p1 = carla.Location(x=trans.location.x + math.cos(pitch_in_rad) * math.cos(yaw_in_rad),
                          y=trans.location.y + math.cos(pitch_in_rad) * math.sin(yaw_in_rad),
                          z=trans.location.z + math.sin(pitch_in_rad))
  
      debug.draw_arrow(trans.location, p1, thickness=0.05, arrow_size=0.1, color=col, life_time=lt)
  
  
  def draw_radar_measurement(debug_helper: carla.DebugHelper, data: carla.RadarMeasurement, velocity_range=7.5,
                             size=0.075, life_time=0.06):
      """Code adapted from carla/PythonAPI/examples/manual_control.py:
          - White: means static points.
          - Red: indicates points moving towards the object.
          - Blue: denoted points moving away.
      """
      radar_rotation = data.transform.rotation
      for detection in data:
          azimuth = math.degrees(detection.azimuth) + radar_rotation.yaw
          altitude = math.degrees(detection.altitude) + radar_rotation.pitch
  
          # move to local coordinates:
          forward_vec = carla.Vector3D(x=detection.depth - 0.25)
          global_to_local(forward_vec,
                          reference=carla.Rotation(pitch=altitude, yaw=azimuth, roll=radar_rotation.roll))
  
          # draw:
          debug_helper.draw_point(data.transform.location + forward_vec, size=size, life_time=life_time,
                                  persistent_lines=False, color=carla.Color(255, 255, 255))
  
  
  # -------------------------------------------------------------------------------------------------
  # -- Math
  # -------------------------------------------------------------------------------------------------
  
  def l2_norm(location1, location2):
      """Computes the Euclidean distance between two carla.Location objects."""
      dx = location1.x - location2.x
      dy = location1.y - location2.y
      dz = location1.z - location2.z
      return math.sqrt(dx**2 + dy**2 + dz**2) + epsilon
  
  
  def vector_norm(vec: carla.Vector3D) -> float:
      """Returns the norm/magnitude (a scalar) of the given 3D vector."""
      return math.sqrt(vec.x**2 + vec.y**2 + vec.z**2)
  
  
  def speed(actor: carla.Actor) -> float:
      """Returns the speed of the given actor in km/h."""
      return 3.6 * vector_norm(actor.get_velocity())
  
  
  def dot_product(a: carla.Vector3D, b: carla.Vector3D) -> float:
      return a.x * b.x + a.y * b.y + a.z * b.z
  
  
  def cosine_similarity(a: carla.Vector3D, b: carla.Vector3D) -> float:
      """-1: opposite vectors (pointing in the opposite direction),
          0: orthogonal,
          1: exactly the same (pointing in the same direction)
      """
      return dot_product(a, b) / (vector_norm(a) * vector_norm(b))
  