# Import any necessary libraries here (like numpy)

import numpy as np


# Copy and paste functions here (Frame Transform, Bresenham, Logodds Conversion)


def bresenham_line(x0, y0, x1, y1):


    return x_pixels, y_pixels

def compute_lidar_hitpoints(robot_pose, lidar_dists, lidar_angles,
                            lidar_offset=(0.0, 0.0, 0.0)):



    return np.vstack((x, y))

def prob_to_logodds(p):
    return 


# Functions below apply your functions above to execute the mapping process
# There is no need to modify these functions


def bresenham_line_array(x0, y0, x1s, y1s, res=1):
    # Adjusting for resolution
    x0 /= res
    y0 /= res
    x1s /= res
    y1s /= res

    # Preallocation
    endpoints_num = len(x1s)
    max_points = np.int32(endpoints_num * (8 / res - 1))

    x_frees = np.empty(max_points, dtype=np.int32)
    y_frees = np.empty(max_points, dtype=np.int32)

    x_hits = np.empty(endpoints_num, dtype=np.int32)
    y_hits = np.empty(endpoints_num, dtype=np.int32)

    # Iterating through points
    n = 0
    for i in range(endpoints_num):
        x_pixels, y_pixels = bresenham_line(x0, y0, x1s[i], y1s[i])

        frees_len = len(x_pixels) - 1
        x_frees[n:n+frees_len] = x_pixels[:-1] # all but last pixel
        y_frees[n:n+frees_len] = y_pixels[:-1]
        x_hits[i] = x_pixels[-1] # last pixel
        y_hits[i] = y_pixels[-1]
        n += frees_len
    return (x_frees[:n], y_frees[:n]), (x_hits[:i+1], y_hits[:i+1])

class OGM:
    def __init__(self, x_min, y_min, x_max, y_max, resolution, logodds_prob, capped_cell_logodds):
        self.x_min = x_min / resolution # adjust min and max values to map size
        self.y_min = y_min / resolution
        self.x_max = x_max / resolution
        self.y_max = y_max / resolution
        
        self.x_size = self.x_max - self.x_min
        self.y_size = self.y_max - self.y_min

        self.resolution = resolution
        self.logodds_prob = logodds_prob
        self.capped_cell_logodds = abs(capped_cell_logodds)
    
    def initialize_OGM(self):
        # x & y dimension of map (m), resolution (m/cell)
        width = int(self.x_size)
        height = int(self.y_size)
        grid = np.zeros((height, width)) # Initialize to unknown logodds(50%) = 0 
        self.grid = grid
        self.grid_binary = grid
    
    def update_OGM(self, x_frees, y_frees, x_hits, y_hits):
        # Update free space cells
        if x_frees.size > 0: # make sure free values aren't empty
            grid_x_frees = np.round(x_frees - self.x_min).astype(int)
            grid_y_frees = np.round(y_frees - self.y_min).astype(int)
            mask = self.grid[grid_y_frees, grid_x_frees] > -self.capped_cell_logodds # capping values
            self.grid[grid_y_frees[mask], grid_x_frees[mask]] -= self.logodds_prob # logodds for free space

        # Update hit cells
        if x_hits.size > 0: # make sure hit values aren't empty
            grid_x_hits = np.round(x_hits - self.x_min).astype(int)
            grid_y_hits = np.round(y_hits - self.y_min).astype(int)
            mask = self.grid[grid_y_hits, grid_x_hits] < self.capped_cell_logodds # capping values
            self.grid[grid_y_hits[mask], grid_x_hits[mask]] += self.logodds_prob # logodds for occupied space
