from messages import *
import numpy as np
import matplotlib.image as mpimg
# import matplotlib.pyplot as plt

class GroundMap():
    """
    Basic class for representing Ground Map for risk-aware planning
    Data entries:
    Header header # timestamp - gridmap timestamp
                  # frame_id - coordinate frame of the ground map 
    float64 resolution # the map resolution [m/cell]
    int width     # width of the map [cells]
    int height    # height of the map [cells]
    Pose pose     # the origin of the map [m, m, rad]. This is the real-world pose of the cell (0,0) in the map.
    Dict{str: np.array} layers # each layer represented as a NumPy array
    """

    def __init__(self, resolution, width, height, origin):
        self.layers = {}
        self.resolution = resolution
        self.width = width
        self.height = height
        self.origin = origin

    def populate_map(self, layer_sources):
        for layer_name, path in layer_sources.items():
            print("Creating ground map layer '" + layer_name + "' from file " + path)
            
            # Read the map layer data
            # TODO - implement me
            data = np.zeros(mpimg.imread(path).shape)

            # Create and store the layer
            self.layers[layer_name] = data

            print("Created map layer [" + str(layer_name) + "] with min value " + str(np.min(data)) + " and max value " + str(np.max(data)))

    def ENUtoGrid(self, pos):
        return [int(i) for i in np.floor(np.array(pos) / self.resolution)]

    def GridToENU(self, pos):
        return (np.array(pos) + 0.5)  * self.resolution