import numpy as np
import matplotlib.pyplot as plt
from skimage.morphology import remove_small_holes
from PIL import Image
from skimage.draw import line_aa
import matplotlib.patches as plt_patches

# Colors
OBSTACLE = '#2E4053'


############
# Obstacle #
############

class Obstacle:
    def __init__(self, cx, cy, radius):
        """
        Constructor for a circular obstacle to be placed on a map.
        :param cx: x coordinate of center of obstacle in world coordinates
        :param cy: y coordinate of center of obstacle in world coordinates
        :param radius: radius of circular obstacle in m
        """
        self.cx = cx    # 障碍物中心的 x 坐标（世界坐标系）
        self.cy = cy    # 障碍物中心的 y 坐标（世界坐标系）
        self.radius = radius    # 障碍物的半径（以米为单位）

    def show(self):
        """
        Display obstacle on current axis.
        """
        # 使用 matplotlib 在绘图中显示圆形障碍物
        # Draw circle
        circle = plt_patches.Circle(xy=(self.cx, self.cy), radius=
                                        self.radius, color=OBSTACLE, zorder=20)
        ax = plt.gca()  # 获取当前轴
        ax.add_patch(circle) # 将障碍物添加到绘图中


#######
# Map #
#######

class Map:
    def __init__(self, file_path, origin, resolution, threshold_occupied=100):
        """
        Constructor for map object. Map contains occupancy grid map data of
        environment as well as meta information.
        :param file_path: path to image of map
        :param threshold_occupied: threshold value for binarization of map
        image
        :param origin: x and y coordinates of map origin in world coordinates
        [m]
        :param resolution: resolution in m/px
        """

        # Set binarization threshold
        self.threshold_occupied = threshold_occupied    # 设置二值化阈值

        # Numpy array containing map data
        self.data = np.array(Image.open(file_path))[:, :, 0]    # 加载地图图像

        # Process raw map image
        self.process_map()  # 处理地图（包括二值化和小孔去除）

        # Store meta information
        self.height = self.data.shape[0]  # 地图高度（像素）
        self.width = self.data.shape[1]  # 地图宽度（像素）
        self.resolution = resolution  # 地图分辨率（每像素代表的实际米数）
        self.origin = origin  # 地图原点坐标（左下角）
        # (bottom-left corner) in m

        # Containers for user-specified additional obstacles and boundaries
        # 初始化障碍物和边界的容器
        self.obstacles = list()
        self.boundaries = list()

    def w2m(self, x, y):
        """
        World2Map. Transform coordinates from global coordinate system to
        map coordinates.
        :param x: x coordinate in global coordinate system
        :param y: y coordinate in global coordinate system
        :return: discrete x and y coordinates in px
        """
        # 从世界坐标系转换为地图坐标系
        dx = int(np.floor((x - self.origin[0]) / self.resolution))
        dy = int(np.floor((y - self.origin[1]) / self.resolution))

        return dx, dy

    def m2w(self, dx, dy):
        """
        Map2World. Transform coordinates from map coordinate system to
        global coordinates.
        :param dx: x coordinate in map coordinate system
        :param dy: y coordinate in map coordinate system
        :return: x and y coordinates of cell center in global coordinate system
        """
        # 从地图坐标系转换为世界坐标系
        x = (dx + 0.5) * self.resolution + self.origin[0]
        y = (dy + 0.5) * self.resolution + self.origin[1]

        return x, y

    def process_map(self):
        """
        Process raw map image. Binarization and removal of small holes in map.
        """

        # Binarization using specified threshold
        # 1 corresponds to free, 0 to occupied
        self.data = np.where(self.data >= self.threshold_occupied, 1, 0)    # 二值化

        # Remove small holes in map corresponding to spurious measurements
        self.data = remove_small_holes(self.data, area_threshold=5,
                                       connectivity=8).astype(np.int8)  # 去除小孔

    def add_obstacles(self, obstacles):
        """
        Add obstacles to the map.
        :param obstacles: list of obstacle objects
        """

        # Extend list of obstacles
        self.obstacles.extend(obstacles)

        # Iterate over list of new obstacles
        for obstacle in obstacles:

            # Compute radius of circular object in pixels
            radius_px = int(np.ceil(obstacle.radius / self.resolution))     # 半径像素
            # Get center coordinates of obstacle in map coordinates
            cx_px, cy_px = self.w2m(obstacle.cx, obstacle.cy)       # 转为地图坐标

            # Add circular object to map
            # 在地图上生成障碍物区域
            y, x = np.ogrid[-radius_px: radius_px, -radius_px: radius_px]
            index = x ** 2 + y ** 2 <= radius_px ** 2
            self.data[cy_px-radius_px:cy_px+radius_px, cx_px-radius_px:
                                                cx_px+radius_px][index] = 0

    def add_boundary(self, boundaries):
        """
        Add boundaries to the map.
        :param boundaries: list of tuples containing coordinates of boundaries'
        start and end points
        """
        # 向地图中添加线性边界
        # Extend list of boundaries
        self.boundaries.extend(boundaries)

        # Iterate over list of boundaries
        # 遍历边界列表
        for boundary in boundaries:
            sx = self.w2m(boundary[0][0], boundary[0][1])   # 起点
            gx = self.w2m(boundary[1][0], boundary[1][1])   # 终点
            path_x, path_y, _ = line_aa(sx[0], sx[1], gx[0], gx[1]) # 绘制线条
            for x, y in zip(path_x, path_y):
                self.data[y, x] = 0 # 标记边界


if __name__ == '__main__':
    map = Map(file_path='maps/real_map.png', origin=(-30.0, -24.0),
                  resolution=0.06)
    # map = Map(file_path='maps/sim_map.png', origin=[-1, -2],
    #                   resolution=0.005)
    plt.imshow(np.flipud(map.data), cmap='gray')    # 显示地图
    plt.show()
