#!/usr/bin/env python
#
# Copyright (c) 2020 Intel Corporation
#

import glob
import os
import sys

try:
    sys.path.append(glob.glob(os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) + '/carla/dist/carla-*%d.%d-%s.egg' % (
        sys.version_info.major,
        sys.version_info.minor,
        'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
except IndexError:
    pass

from enum import Enum
import math
import numpy as np
import pygame
import weakref
import carla
from carla import ad


class RssStateVisualizer(object):

    def __init__(self, display_dimensions, font, world):
        self._surface = None
        self._display_dimensions = display_dimensions
        self._font = font
        self._world = world

    def tick(self, individual_rss_states):
        state_surface = pygame.Surface((220, self._display_dimensions[1]))
        state_surface.set_colorkey(pygame.Color('black'))
        v_offset = 0

        if individual_rss_states:
            surface = self._font.render('RSS States:', True, (255, 255, 255))
            state_surface.blit(surface, (8, v_offset))
            v_offset += 26
        for state in individual_rss_states:
            object_name = "Obj"
            if state.rss_state.objectId == 18446744073709551614:
                object_name = "Border Left"
            elif state.rss_state.objectId == 18446744073709551615:
                object_name = "Border Right"
            else:
                other_actor = state.get_actor(self._world)
                if other_actor:
                    li = list(other_actor.type_id.split("."))
                    if li:
                        li.pop(0)
                    li = [element.capitalize() for element in li]

                    object_name = " ".join(li).strip()[:15]

            mode = "?"
            if state.actor_calculation_mode == ad.rss.map.RssMode.Structured:
                mode = "S"
            elif state.actor_calculation_mode == ad.rss.map.RssMode.Unstructured:
                mode = "U"
            elif state.actor_calculation_mode == ad.rss.map.RssMode.NotRelevant:
                mode = "-"
            item = '%4s % 2dm %8s' % (mode, state.distance, object_name)

            surface = self._font.render(item, True, (255, 255, 255))
            state_surface.blit(surface, (5, v_offset))
            color = (128, 128, 128)
            if state.actor_calculation_mode != ad.rss.map.RssMode.NotRelevant:
                if state.is_dangerous:
                    color = (255, 0, 0)
                else:
                    color = (0, 255, 0)
            pygame.draw.circle(state_surface, color, (12, v_offset + 7), 5)
            xpos = 184
            if state.actor_calculation_mode == ad.rss.map.RssMode.Structured:
                if not state.rss_state.longitudinalState.isSafe and ((state.rss_state.longitudinalState.rssStateInformation.evaluator == "LongitudinalDistanceSameDirectionOtherInFront") or (state.rss_state.longitudinalState.rssStateInformation.evaluator == "LongitudinalDistanceSameDirectionEgoFront")):
                    pygame.draw.polygon(
                        state_surface, (
                            255, 255, 255), ((xpos + 1, v_offset + 1 + 4), (xpos + 6, v_offset + 1 + 0), (xpos + 11, v_offset + 1 + 4),
                                             (xpos + 7, v_offset + 1 + 4), (xpos + 7, v_offset + 1 + 12), (xpos + 5, v_offset + 1 + 12), (xpos + 5, v_offset + 1 + 4)))
                    xpos += 14

                if not state.rss_state.longitudinalState.isSafe and ((state.rss_state.longitudinalState.rssStateInformation.evaluator == "LongitudinalDistanceOppositeDirectionEgoCorrectLane") or (state.rss_state.longitudinalState.rssStateInformation.evaluator == "LongitudinalDistanceOppositeDirection")):
                    pygame.draw.polygon(
                        state_surface, (
                            255, 255, 255), ((xpos + 2, v_offset + 1 + 8), (xpos + 6, v_offset + 1 + 12), (xpos + 10, v_offset + 1 + 8),
                                             (xpos + 7, v_offset + 1 + 8), (xpos + 7, v_offset + 1 + 0), (xpos + 5, v_offset + 1 + 0), (xpos + 5, v_offset + 1 + 8)))
                    xpos += 14

                if not state.rss_state.lateralStateRight.isSafe and not (state.rss_state.lateralStateRight.rssStateInformation.evaluator == "None"):
                    pygame.draw.polygon(
                        state_surface, (
                            255, 255, 255), ((xpos + 0, v_offset + 1 + 4), (xpos + 8, v_offset + 1 + 4), (xpos + 8, v_offset + 1 + 1),
                                             (xpos + 12, v_offset + 1 + 6), (xpos + 8, v_offset + 1 + 10), (xpos + 8, v_offset + 1 + 8), (xpos + 0, v_offset + 1 + 8)))
                    xpos += 14
                if not state.rss_state.lateralStateLeft.isSafe and not (state.rss_state.lateralStateLeft.rssStateInformation.evaluator == "None"):
                    pygame.draw.polygon(
                        state_surface, (
                            255, 255, 255), ((xpos + 0, v_offset + 1 + 6), (xpos + 4, v_offset + 1 + 1), (xpos + 4, v_offset + 1 + 4),
                                             (xpos + 12, v_offset + 1 + 4), (xpos + 12, v_offset + 1 + 8), (xpos + 4, v_offset + 1 + 8), (xpos + 4, v_offset + 1 + 10)))
                    xpos += 14
            elif state.actor_calculation_mode == ad.rss.map.RssMode.Unstructured:
                text = ""
                if state.rss_state.unstructuredSceneState.response == ad.rss.state.UnstructuredSceneResponse.DriveAway:
                    text = "  D"
                elif state.rss_state.unstructuredSceneState.response == ad.rss.state.UnstructuredSceneResponse.ContinueForward:
                    text = "  C"
                elif state.rss_state.unstructuredSceneState.response == ad.rss.state.UnstructuredSceneResponse.Brake:
                    text = "  B"
                surface = self._font.render(text, True, (255, 255, 255))
                state_surface.blit(surface, (xpos, v_offset))

            v_offset += 14
            self._surface = state_surface

    def render(self, display, v_offset):
        if self._surface:
            display.blit(self._surface, (0, v_offset))


def get_matrix(transform):
    """
    Creates matrix from carla transform.
    """

    rotation = transform.rotation
    location = transform.location
    c_y = np.cos(np.radians(rotation.yaw))
    s_y = np.sin(np.radians(rotation.yaw))
    c_r = np.cos(np.radians(rotation.roll))
    s_r = np.sin(np.radians(rotation.roll))
    c_p = np.cos(np.radians(rotation.pitch))
    s_p = np.sin(np.radians(rotation.pitch))
    matrix = np.matrix(np.identity(4))
    matrix[0, 3] = location.x
    matrix[1, 3] = location.y
    matrix[2, 3] = location.z
    matrix[0, 0] = c_p * c_y
    matrix[0, 1] = c_y * s_p * s_r - s_y * c_r
    matrix[0, 2] = -c_y * s_p * c_r - s_y * s_r
    matrix[1, 0] = s_y * c_p
    matrix[1, 1] = s_y * s_p * s_r + c_y * c_r
    matrix[1, 2] = -s_y * s_p * c_r + c_y * s_r
    matrix[2, 0] = s_p
    matrix[2, 1] = -c_p * s_r
    matrix[2, 2] = c_p * c_r
    return matrix

# ==============================================================================
# -- RssUnstructuredSceneVisualizer ------------------------------------------------
# ==============================================================================


class RssUnstructuredSceneVisualizerMode(Enum):
    disabled = 1
    window = 2
    fullscreen = 3


class RssUnstructuredSceneVisualizer(object):

    def __init__(self, parent_actor, world, display_dimensions):
        self._last_rendered_frame = -1
        self._surface = None
        self._current_rss_surface = None
        self.current_camera_surface = (0, None)
        self._world = world
        self._parent_actor = parent_actor
        self._display_dimensions = display_dimensions
        self._camera = None
        self._mode = RssUnstructuredSceneVisualizerMode.disabled

        self.restart(RssUnstructuredSceneVisualizerMode.window)

    def destroy(self):
        if self._camera:
            self._camera.stop()
            self._camera.destroy()
            self._camera = None

    def restart(self, mode):
        # setup up top down camera
        self.destroy()
        self._mode = mode

        spawn_sensor = False
        if mode == RssUnstructuredSceneVisualizerMode.window:
            self._dim = (self._display_dimensions[0] / 3, self._display_dimensions[1] / 2)
            spawn_sensor = True
        elif mode == RssUnstructuredSceneVisualizerMode.fullscreen:
            self._dim = (self._display_dimensions[0], self._display_dimensions[1])
            spawn_sensor = True
        else:
            self._surface = None

        if spawn_sensor:
            self._calibration = np.identity(3)
            self._calibration[0, 2] = self._dim[0] / 2.0
            self._calibration[1, 2] = self._dim[1] / 2.0
            self._calibration[0, 0] = self._calibration[1, 1] = self._dim[0] / \
            (2.0 * np.tan(90.0 * np.pi / 360.0))  # fov default: 90.0

            bp_library = self._world.get_blueprint_library()
            bp = bp_library.find('sensor.camera.rgb')
            bp.set_attribute('image_size_x', str(self._dim[0]))
            bp.set_attribute('image_size_y', str(self._dim[1]))

            self._camera = self._world.spawn_actor(
                bp,
                carla.Transform(carla.Location(x=7.5, z=10), carla.Rotation(pitch=-90)),
                attach_to=self._parent_actor)
            # We need to pass the lambda a weak reference to self to avoid
            # circular reference.
            weak_self = weakref.ref(self)
            self._camera.listen(lambda image: self._parse_image(weak_self, image))

    def update_surface(self, cam_frame, rss_frame):
        if self._mode == RssUnstructuredSceneVisualizerMode.disabled:
            return
        render = False

        if cam_frame and self._current_rss_surface and self._current_rss_surface[0] == cam_frame:
            render = True

        if rss_frame and self.current_camera_surface and self.current_camera_surface[0] == rss_frame:
            render = True

        if render:
            surface = self.current_camera_surface[1]
            surface.blit(self._current_rss_surface[1], (0, 0))
            rect = pygame.Rect((0, 0), (2, surface.get_height()))
            pygame.draw.rect(surface, (0, 0, 0), rect, 0)
            rect = pygame.Rect((0, 0), (surface.get_width(), 2))
            pygame.draw.rect(surface, (0, 0, 0), rect, 0)
            rect = pygame.Rect((0, surface.get_height() - 2), (surface.get_width(), surface.get_height()))
            pygame.draw.rect(surface, (0, 0, 0), rect, 0)
            rect = pygame.Rect((surface.get_width() - 2, 0), (surface.get_width(), surface.get_width()))
            pygame.draw.rect(surface, (0, 0, 0), rect, 0)
            self._surface = surface

    def toggle_camera(self):
        print("Toggle RssUnstructuredSceneVisualizer")
        if self._mode == RssUnstructuredSceneVisualizerMode.window:
            self.restart(RssUnstructuredSceneVisualizerMode.fullscreen)
        elif self._mode == RssUnstructuredSceneVisualizerMode.fullscreen:
            self.restart(RssUnstructuredSceneVisualizerMode.disabled)
        elif self._mode == RssUnstructuredSceneVisualizerMode.disabled:
            self.restart(RssUnstructuredSceneVisualizerMode.window)

    @staticmethod
    def _parse_image(weak_self, image):
        self = weak_self()
        if not self:
            return
        image.convert(carla.ColorConverter.Raw)
        array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8"))
        array = np.reshape(array, (image.height, image.width, 4))
        array = array[:, :, :3]
        array = array[:, :, ::-1]
        surface = pygame.surfarray.make_surface(array.swapaxes(0, 1))
        self.current_camera_surface = (image.frame, surface)
        self.update_surface(image.frame, None)

    @staticmethod
    def rotate_around_point(xy, radians, origin):
        """Rotate a point around a given point.
        """
        x, y = xy
        offset_x, offset_y = origin
        adjusted_x = (x - offset_x)
        adjusted_y = (y - offset_y)
        cos_rad = math.cos(radians)
        sin_rad = math.sin(radians)
        qx = offset_x + cos_rad * adjusted_x - sin_rad * adjusted_y
        qy = offset_y + sin_rad * adjusted_x + cos_rad * adjusted_y

        return qx, qy

    def tick(self, frame, rss_response, allowed_heading_ranges):
        if not self._camera:
            return
        surface = pygame.Surface(self._dim)
        surface.set_colorkey(pygame.Color('black'))
        surface.set_alpha(180)
        try:
            lines = RssUnstructuredSceneVisualizer.get_trajectory_sets(
                rss_response.rss_state_snapshot, self._camera.get_transform(), self._calibration)

            polygons = []
            for heading_range in allowed_heading_ranges:
                polygons.append((RssUnstructuredSceneVisualizer.transform_points(
                    RssUnstructuredSceneVisualizer._get_points_from_pairs(
                        RssUnstructuredSceneVisualizer.draw_heading_range(
                            heading_range, rss_response.ego_dynamics_on_route)),
                    self._camera.get_transform(), self._calibration), (0, 0, 255)))

            RssUnstructuredSceneVisualizer.draw_lines(surface, lines)
            RssUnstructuredSceneVisualizer.draw_polygons(surface, polygons)

        except RuntimeError as e:
            print("ERROR {}".format(e))
        self._current_rss_surface = (frame, surface)
        self.update_surface(None, frame)

    def render(self, display):
        if self._surface:
            display.blit(self._surface, (display.get_width() - self._dim[0], 0))

    @staticmethod
    def draw_heading_range(heading_range, ego_dynamics_on_route):
        line = [(float(ego_dynamics_on_route.ego_center.x), float(ego_dynamics_on_route.ego_center.y))]
        length = 3.0
        current_angle = float(heading_range.begin)
        max_angle = float(heading_range.end)
        if heading_range.end < heading_range.begin:
            max_angle += 2.0 * np.pi

        while current_angle < max_angle:
            line.append((float(ego_dynamics_on_route.ego_center.x) + length * np.cos(current_angle),
                         float(ego_dynamics_on_route.ego_center.y) + length * np.sin(current_angle)))
            current_angle += 0.2

        if current_angle != max_angle:
            line.append((float(ego_dynamics_on_route.ego_center.x) + length * np.cos(max_angle),
                         float(ego_dynamics_on_route.ego_center.y) + length * np.sin(max_angle)))

        line.append((float(ego_dynamics_on_route.ego_center.x), float(ego_dynamics_on_route.ego_center.y)))
        return line

    @staticmethod
    def get_trajectory_sets(rss_state_snapshot, camera_transform, calibration):
        """
        Creates 3D bounding boxes based on carla vehicle list and camera.
        """
        trajectory_sets = []

        # ego
        trajectory_sets.append((RssUnstructuredSceneVisualizer.transform_points(RssUnstructuredSceneVisualizer._get_trajectory_set_points(
            rss_state_snapshot.unstructuredSceneEgoInformation.brakeTrajectorySet), camera_transform, calibration), (255, 0, 0)))
        trajectory_sets.append((RssUnstructuredSceneVisualizer.transform_points(RssUnstructuredSceneVisualizer._get_trajectory_set_points(
            rss_state_snapshot.unstructuredSceneEgoInformation.continueForwardTrajectorySet), camera_transform, calibration), (0, 255, 0)))

        # others
        for state in rss_state_snapshot.individualResponses:
            if state.unstructuredSceneState.rssStateInformation.brakeTrajectorySet:
                trajectory_sets.append((RssUnstructuredSceneVisualizer.transform_points(RssUnstructuredSceneVisualizer._get_trajectory_set_points(
                    state.unstructuredSceneState.rssStateInformation.brakeTrajectorySet), camera_transform, calibration), (255, 0, 0)))
            if state.unstructuredSceneState.rssStateInformation.continueForwardTrajectorySet:
                trajectory_sets.append((RssUnstructuredSceneVisualizer.transform_points(RssUnstructuredSceneVisualizer._get_trajectory_set_points(
                    state.unstructuredSceneState.rssStateInformation.continueForwardTrajectorySet), camera_transform, calibration), (0, 255, 0)))

        return trajectory_sets

    @staticmethod
    def draw_lines(surface, lines):
        """
        Draws lines on pygame display.
        """
        for line, color in lines:
            if len(line) > 1:
                pygame.draw.lines(surface, color, True, line, 2)

    @staticmethod
    def draw_polygons(surface, polygons):
        """
        Draws polygons on pygame display.
        """
        for polygon, color in polygons:
            if len(polygon) > 1:
                pygame.draw.polygon(surface, color, polygon)

    @staticmethod
    def transform_points(world_cords, camera_transform, calibration):
        """
        Returns trajectory set projected to camera view
        """
        world_cords = np.transpose(world_cords)
        cords_x_y_z = RssUnstructuredSceneVisualizer._world_to_sensor(world_cords, camera_transform)[:3, :]
        cords_y_minus_z_x = np.concatenate([cords_x_y_z[1, :], -cords_x_y_z[2, :], cords_x_y_z[0, :]])
        ts = np.transpose(np.dot(calibration, cords_y_minus_z_x))
        camera_ts = np.concatenate([ts[:, 0] / ts[:, 2], ts[:, 1] / ts[:, 2], ts[:, 2]], axis=1)
        line_to_draw = []
        for point in camera_ts:
            line_to_draw.append((int(point[0, 0]), int(point[0, 1])))
        return line_to_draw

    @staticmethod
    def _get_trajectory_set_points(trajectory_set):
        """
        """
        cords = np.zeros((len(trajectory_set), 4))
        i = 0
        for pt in trajectory_set:
            cords[i, :] = np.array([pt.x, -pt.y, 0, 1])
            i += 1
        return cords

    @staticmethod
    def _get_points_from_pairs(trajectory_set):
        """
        """
        cords = np.zeros((len(trajectory_set), 4))
        i = 0
        for pt in trajectory_set:
            cords[i, :] = np.array([pt[0], -pt[1], 0, 1])
            i += 1
        return cords

    @staticmethod
    def _world_to_sensor(cords, camera_transform):
        """
        Transforms world coordinates to sensor.
        """
        sensor_world_matrix = get_matrix(camera_transform)
        world_sensor_matrix = np.linalg.inv(sensor_world_matrix)
        sensor_cords = np.dot(world_sensor_matrix, cords)
        return sensor_cords

# ==============================================================================
# -- RssBoundingBoxVisualizer ------------------------------------------------------
# ==============================================================================


class RssBoundingBoxVisualizer(object):

    def __init__(self, display_dimensions, world, camera):
        self._last_camera_frame = 0
        self._surface_for_frame = []
        self._world = world
        self._dim = display_dimensions
        self._calibration = np.identity(3)
        self._calibration[0, 2] = self._dim[0] / 2.0
        self._calibration[1, 2] = self._dim[1] / 2.0
        self._calibration[0, 0] = self._calibration[1, 1] = self._dim[0] / \
            (2.0 * np.tan(90.0 * np.pi / 360.0))  # fov default: 90.0
        self._camera = camera

    def tick(self, frame, individual_rss_states):
        if len(self._surface_for_frame) > 0:
            try:
                while self._surface_for_frame[0][0] < self._last_camera_frame:
                    self._surface_for_frame.pop(0)
            except IndexError:
                return

        # only render on new frame
        if len(self._surface_for_frame) > 0:
            if self._surface_for_frame[0][0] == frame:
                return

        surface = pygame.Surface(self._dim)
        surface.set_colorkey(pygame.Color('black'))
        surface.set_alpha(80)
        try:
            bounding_boxes = RssBoundingBoxVisualizer.get_bounding_boxes(
                individual_rss_states, self._camera.get_transform(), self._calibration, self._world)
            RssBoundingBoxVisualizer.draw_bounding_boxes(surface, bounding_boxes)
            self._surface_for_frame.append((frame, surface, len(bounding_boxes)))
        except RuntimeError:
            pass

    def render(self, display, current_camera_frame):
        rendered = False
        boxes_to_render = 0
        for frame, surface, box_count in self._surface_for_frame:
            if frame == current_camera_frame:
                display.blit(surface, (0, 0))
                boxes_to_render = box_count
                rendered = True
                break
        if not rendered and boxes_to_render > 0:
            print("Warning: {} bounding boxes were not drawn.".format(boxes_to_render))
        self._last_camera_frame = current_camera_frame

    @staticmethod
    def get_bounding_boxes(individual_rss_states, camera_transform, calibration, world):
        """
        Creates 3D bounding boxes based on carla vehicle list and camera.
        """
        bounding_boxes = []
        for state in individual_rss_states:
            if state.actor_calculation_mode != ad.rss.map.RssMode.NotRelevant and state.is_dangerous:
                other_actor = state.get_actor(world)
                if other_actor:
                    bounding_boxes.append(RssBoundingBoxVisualizer.get_bounding_box(
                        other_actor, camera_transform, calibration))
        # filter objects behind camera
        bounding_boxes = [bb for bb in bounding_boxes if all(bb[:, 2] > 0)]
        return bounding_boxes

    @staticmethod
    def draw_bounding_boxes(surface, bounding_boxes, color=pygame.Color('red')):
        """
        Draws bounding boxes on pygame display.
        """
        for bbox in bounding_boxes:
            points = [(int(bbox[i, 0]), int(bbox[i, 1])) for i in range(8)]
            # draw lines
            # base
            polygon = [points[0], points[1], points[2], points[3]]
            pygame.draw.polygon(surface, color, polygon)
            # top
            polygon = [points[4], points[5], points[6], points[7]]
            pygame.draw.polygon(surface, color, polygon)
            # base-top
            polygon = [points[0], points[1], points[5], points[4]]
            pygame.draw.polygon(surface, color, polygon)
            polygon = [points[1], points[2], points[6], points[5]]
            pygame.draw.polygon(surface, color, polygon)
            polygon = [points[2], points[6], points[7], points[3]]
            pygame.draw.polygon(surface, color, polygon)
            polygon = [points[0], points[4], points[7], points[3]]
            pygame.draw.polygon(surface, color, polygon)

    @staticmethod
    def get_bounding_box(vehicle, camera_transform, calibration):
        """
        Returns 3D bounding box for a vehicle based on camera view.
        """

        bb_cords = RssBoundingBoxVisualizer._create_bb_points(vehicle)
        cords_x_y_z = RssBoundingBoxVisualizer._vehicle_to_sensor(bb_cords, vehicle, camera_transform)[:3, :]
        cords_y_minus_z_x = np.concatenate([cords_x_y_z[1, :], -cords_x_y_z[2, :], cords_x_y_z[0, :]])
        bbox = np.transpose(np.dot(calibration, cords_y_minus_z_x))
        camera_bbox = np.concatenate([bbox[:, 0] / bbox[:, 2], bbox[:, 1] / bbox[:, 2], bbox[:, 2]], axis=1)
        return camera_bbox

    @staticmethod
    def _create_bb_points(vehicle):
        """
        Returns 3D bounding box for a vehicle.
        """

        cords = np.zeros((8, 4))
        extent = vehicle.bounding_box.extent
        cords[0, :] = np.array([extent.x, extent.y, -extent.z, 1])
        cords[1, :] = np.array([-extent.x, extent.y, -extent.z, 1])
        cords[2, :] = np.array([-extent.x, -extent.y, -extent.z, 1])
        cords[3, :] = np.array([extent.x, -extent.y, -extent.z, 1])
        cords[4, :] = np.array([extent.x, extent.y, extent.z, 1])
        cords[5, :] = np.array([-extent.x, extent.y, extent.z, 1])
        cords[6, :] = np.array([-extent.x, -extent.y, extent.z, 1])
        cords[7, :] = np.array([extent.x, -extent.y, extent.z, 1])
        return cords

    @staticmethod
    def _vehicle_to_sensor(cords, vehicle, camera_transform):
        """
        Transforms coordinates of a vehicle bounding box to sensor.
        """

        world_cord = RssBoundingBoxVisualizer._vehicle_to_world(cords, vehicle)
        sensor_cord = RssBoundingBoxVisualizer._world_to_sensor(world_cord, camera_transform)
        return sensor_cord

    @staticmethod
    def _vehicle_to_world(cords, vehicle):
        """
        Transforms coordinates of a vehicle bounding box to world.
        """

        bb_transform = carla.Transform(vehicle.bounding_box.location)
        bb_vehicle_matrix = get_matrix(bb_transform)
        vehicle_world_matrix = get_matrix(vehicle.get_transform())
        bb_world_matrix = np.dot(vehicle_world_matrix, bb_vehicle_matrix)
        world_cords = np.dot(bb_world_matrix, np.transpose(cords))
        return world_cords

    @staticmethod
    def _world_to_sensor(cords, camera_transform):
        """
        Transforms world coordinates to sensor.
        """

        sensor_world_matrix = get_matrix(camera_transform)
        world_sensor_matrix = np.linalg.inv(sensor_world_matrix)
        sensor_cords = np.dot(world_sensor_matrix, cords)
        return sensor_cords

# ==============================================================================
# -- RssDebugVisualizer ------------------------------------------------------------
# ==============================================================================


class RssDebugVisualizationMode(Enum):
    Off = 1
    RouteOnly = 2
    VehicleStateOnly = 3
    VehicleStateAndRoute = 4
    All = 5


class RssDebugVisualizer(object):

    def __init__(self, player, world):
        self._world = world
        self._player = player
        self._visualization_mode = RssDebugVisualizationMode.Off

    def toggleMode(self):
        if self._visualization_mode == RssDebugVisualizationMode.All:
            self._visualization_mode = RssDebugVisualizationMode.Off
        elif self._visualization_mode == RssDebugVisualizationMode.Off:
            self._visualization_mode = RssDebugVisualizationMode.RouteOnly
        elif self._visualization_mode == RssDebugVisualizationMode.RouteOnly:
            self._visualization_mode = RssDebugVisualizationMode.VehicleStateOnly
        elif self._visualization_mode == RssDebugVisualizationMode.VehicleStateOnly:
            self._visualization_mode = RssDebugVisualizationMode.VehicleStateAndRoute
        elif self._visualization_mode == RssDebugVisualizationMode.VehicleStateAndRoute:
            self._visualization_mode = RssDebugVisualizationMode.All
        print("New Debug Visualizer Mode {}".format(self._visualization_mode))

    def tick(self, route, dangerous, individual_rss_states, ego_dynamics_on_route):
        if self._visualization_mode == RssDebugVisualizationMode.RouteOnly or \
                self._visualization_mode == RssDebugVisualizationMode.VehicleStateAndRoute or \
                self._visualization_mode == RssDebugVisualizationMode.All:
            self.visualize_route(dangerous, route)

        if self._visualization_mode == RssDebugVisualizationMode.VehicleStateOnly or \
                self._visualization_mode == RssDebugVisualizationMode.VehicleStateAndRoute or \
                self._visualization_mode == RssDebugVisualizationMode.All:
            self.visualize_rss_results(individual_rss_states)

        if self._visualization_mode == RssDebugVisualizationMode.All:
            self.visualize_ego_dynamics(ego_dynamics_on_route)

    def visualize_route(self, dangerous, route):
        if not route:
            return
        right_lane_edges = dict()
        left_lane_edges = dict()

        for road_segment in route.roadSegments:
            right_most_lane = road_segment.drivableLaneSegments[0]
            if right_most_lane.laneInterval.laneId not in right_lane_edges:
                edge = ad.map.route.getRightProjectedENUEdge(right_most_lane.laneInterval)
                right_lane_edges[right_most_lane.laneInterval.laneId] = edge
                intersection_lane = ad.map.intersection.Intersection.isLanePartOfAnIntersection(right_most_lane.laneInterval.laneId)

                color = carla.Color(r=(128 if dangerous else 255))
                if intersection_lane:
                    color.b = 128 if dangerous else 255
                color = carla.Color(r=255, g=0, b=255)
                self.visualize_enu_edge(edge, color, self._player.get_location().z)

            left_most_lane = road_segment.drivableLaneSegments[-1]
            if left_most_lane.laneInterval.laneId not in left_lane_edges:
                edge = ad.map.route.getLeftProjectedENUEdge(left_most_lane.laneInterval)
                left_lane_edges[left_most_lane.laneInterval.laneId] = edge
                intersection_lane = ad.map.intersection.Intersection.isLanePartOfAnIntersection(left_most_lane.laneInterval.laneId)
                color = carla.Color(g=(128 if dangerous else 255))
                if intersection_lane:
                    color.b = 128 if dangerous else 255

                self.visualize_enu_edge(edge, color, self._player.get_location().z)

    def visualize_enu_edge(self, edge, color, z_offset):
        for point in edge:
            carla_point = carla.Location(x=float(point.x), y=-1. * float(point.y), z=float(point.z) + z_offset)
            self._world.debug.draw_point(carla_point, 0.1, color, 0.1, False)

    def visualize_rss_results(self, state_snapshot):
        for state in state_snapshot:
            other_actor = state.get_actor(self._world)
            if not other_actor:
                # print("Actor not found. Skip visualizing state {}".format(state))
                continue
            ego_point = self._player.get_location()
            ego_point.z += 0.05
            yaw = self._player.get_transform().rotation.yaw
            cosine = math.cos(math.radians(yaw))
            sine = math.sin(math.radians(yaw))
            line_offset = carla.Location(-sine * 0.1, cosine * 0.1, 0.0)

            point = other_actor.get_location()
            point.z += 0.05
            indicator_color = carla.Color(0, 255, 0)
            dangerous = ad.rss.state.isDangerous(state.rss_state)
            if dangerous:
                indicator_color = carla.Color(255, 0, 0)
            elif state.rss_state.situationType == ad.rss.situation.SituationType.NotRelevant:
                indicator_color = carla.Color(150, 150, 150)

            if self._visualization_mode == RssDebugVisualizationMode.All:
                # the connection lines are only visualized if All is requested
                lon_color = indicator_color
                lat_l_color = indicator_color
                lat_r_color = indicator_color
                if not state.rss_state.longitudinalState.isSafe:
                    lon_color.r = 255
                    lon_color.g = 0 if dangerous else 255
                if not state.rss_state.lateralStateLeft.isSafe:
                    lat_l_color.r = 255
                    lat_l_color.g = 0 if dangerous else 255
                if not state.rss_state.lateralStateRight.isSafe:
                    lat_r_color.r = 255
                    lat_r_color.g = 0 if dangerous else 255
                self._world.debug.draw_line(ego_point, point, 0.1, lon_color, 0.02, False)
                self._world.debug.draw_line(ego_point - line_offset, point -
                                            line_offset, 0.1, lat_l_color, 0.02, False)
                self._world.debug.draw_line(ego_point + line_offset, point +
                                            line_offset, 0.1, lat_r_color, 0.02, False)
            point.z += 3.
            self._world.debug.draw_point(point, 0.2, indicator_color, 0.02, False)

    def visualize_ego_dynamics(self, ego_dynamics_on_route):
        color = carla.Color(0, 0, 255)

        sin_heading = math.sin(float(ego_dynamics_on_route.route_heading))
        cos_heading = math.cos(float(ego_dynamics_on_route.route_heading))

        heading_location_start = self._player.get_location()
        heading_location_start.x -= cos_heading * 10.
        heading_location_start.y += sin_heading * 10.
        heading_location_start.z += 0.5
        heading_location_end = self._player.get_location()
        heading_location_end.x += cos_heading * 10.
        heading_location_end.y -= sin_heading * 10.
        heading_location_end.z += 0.5

        self._world.debug.draw_arrow(heading_location_start, heading_location_end, 0.1, 0.1, color, 0.02, False)

        sin_center = math.sin(float(ego_dynamics_on_route.route_heading) + math.pi / 2.)
        cos_center = math.cos(float(ego_dynamics_on_route.route_heading) + math.pi / 2.)
        center_location_start = self._player.get_location()
        center_location_start.x -= cos_center * 2.
        center_location_start.y += sin_center * 2.
        center_location_start.z += 0.5
        center_location_end = self._player.get_location()
        center_location_end.x += cos_center * 2.
        center_location_end.y -= sin_center * 2.
        center_location_end.z += 0.5

        self._world.debug.draw_line(center_location_start, center_location_end, 0.1, color, 0.02, False)