# Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de
# Barcelona (UAB).
#
# This work is licensed under the terms of the MIT license.
# For a copy, see <https://opensource.org/licenses/MIT>.

"""Class used for operating the city map."""

import math
import os

try:
    import numpy as np
except ImportError:
    raise RuntimeError('cannot import numpy, make sure numpy package is installed')

try:
    from PIL import Image
except ImportError:
    raise RuntimeError('cannot import PIL, make sure pillow package is installed')

from .graph import Graph
from .graph import sldist
from .grid import Grid
from .converter import Converter


def color_to_angle(color):
    return (float(color) / 255.0) * 2 * math.pi


class CarlaMap(object):

    def __init__(self, city, pixel_density=0.1643, node_density=50):
        dir_path = os.path.dirname(__file__)
        city_file = os.path.join(dir_path, city + '.txt')

        city_map_file = os.path.join(dir_path, city + '.png')
        city_map_file_lanes = os.path.join(dir_path, city + 'Lanes.png')
        city_map_file_center = os.path.join(dir_path, city + 'Central.png')

        # The built graph. This is the exact same graph that unreal builds. This
        # is a generic structure used for many cases
        self._graph = Graph(city_file, node_density)

        self._pixel_density = pixel_density
        self._grid = Grid(self._graph)
        # The number of game units per pixel. For now this is fixed.

        self._converter = Converter(city_file, pixel_density, node_density)

        # Load the lanes image
        self.map_image_lanes = Image.open(city_map_file_lanes)
        self.map_image_lanes.load()
        self.map_image_lanes = np.asarray(self.map_image_lanes, dtype="int32")
        # Load the image
        self.map_image = Image.open(city_map_file)
        self.map_image.load()
        self.map_image = np.asarray(self.map_image, dtype="int32")

        # Load the lanes image
        self.map_image_center = Image.open(city_map_file_center)
        self.map_image_center.load()
        self.map_image_center = np.asarray(self.map_image_center, dtype="int32")

    def get_graph_resolution(self):

        return self._graph.get_resolution()
    def check_pixel_on_map(self, pixel):

        if pixel[0] < self.map_image_lanes.shape[1] and pixel[0] > 0 and \
            pixel[1] < self.map_image_lanes.shape[0] and pixel[1] > 0:
            return True
        else:
            return False

    def get_map(self, height=None):
        if height is not None:
            img = Image.fromarray(self.map_image.astype(np.uint8))

            aspect_ratio = height / float(self.map_image.shape[0])

            img = img.resize((int(aspect_ratio * self.map_image.shape[1]), height), Image.ANTIALIAS)
            img.load()
            return np.asarray(img, dtype="int32")
        return np.fliplr(self.map_image)

    def get_map_lanes(self, size=None):
        if size is not None:
            img = Image.fromarray(self.map_image_lanes.astype(np.uint8))
            img = img.resize((size[1], size[0]), Image.ANTIALIAS)
            img.load()
            return np.fliplr(np.asarray(img, dtype="int32"))
        return np.fliplr(self.map_image_lanes)

    def get_lane_orientation(self, world):
        """Get the lane orientation of a certain world position."""
        pixel = self.convert_to_pixel(world)

        ori = self.map_image_lanes[int(pixel[1]), int(pixel[0]), 2]
        ori = color_to_angle(ori)

        return (-math.cos(ori), -math.sin(ori))

    def get_lane_orientation_degrees(self, world):
        """Get the lane orientation of a certain world position."""
        pixel = self.convert_to_pixel(world)

        ori = self.map_image_lanes[int(pixel[1]), int(pixel[0]), 2]

        return color_to_angle(ori)


    def convert_to_node(self, input_data):
        """
        Receives a data type (Can Be Pixel or World )
        :param input_data: position in some coordinate
        :return: A node object
        """
        return self._converter.convert_to_node(input_data)

    def convert_to_pixel(self, input_data):
        """
        Receives a data type (Can Be Node or World )
        :param input_data: position in some coordinate
        :return: A node object
        """
        return self._converter.convert_to_pixel(input_data)

    def convert_to_world(self, input_data):
        """
        Receives a data type (Can Be Pixel or Node )
        :param input_data: position in some coordinate
        :return: A node object
        """
        return self._converter.convert_to_world(input_data)

    def get_walls_directed(self, node_source, source_ori, node_target, target_ori, both_walls=True):
        """
        This is the most hacky function. Instead of planning on two ways,
        we basically use a one way road and interrupt the other road by adding
        an artificial wall.
        """

        if both_walls:
            final_walls = self._grid.get_wall_source(node_source, source_ori, node_target)

            final_walls = final_walls.union(self._grid.get_wall_target(
                node_target, target_ori, node_source))

            return final_walls
        else:

            return self._grid.get_wall_source(node_source, source_ori, node_target)

    def is_point_on_lane(self, world):
        pixel = self.convert_to_pixel(world)

        if not self.check_pixel_on_map(pixel):
            return False

        ori = self.map_image_lanes[int(pixel[1]), int(pixel[0]), 1]

        if ori == 0:
            return False
        else:
            return True

    def is_point_on_intersection(self, world):
        pixel = self.convert_to_pixel(world)
        if not self.check_pixel_on_map(pixel):
            return False
        ori = self.map_image_lanes[int(pixel[1]), int(pixel[0]), 0]


        if ori > 0:
            return True
        else:
            return False

    def get_walls(self):

        return self._grid.get_walls()

    def get_distance_closest_node(self, pos):

        distance = []
        for node_iter in self._graph.intersection_nodes():
            distance.append(sldist(node_iter, pos))

        return sorted(distance)[0]

    def get_intersection_nodes(self):
        return self._graph.intersection_nodes()

    def get_curve_nodes(self):
        return self._graph.curve_nodes()

    def get_adjacent_free_nodes(self, pos):
        return self._grid.get_adjacent_free_nodes(pos)

    def search_on_grid(self, node):
        return self._grid.search_on_grid(node[0], node[1])
