Sayter99/lab5.py

## lab5.py
'''
 IMPORTANT: Read through the code before beginning implementation!
 Your solution should fill in the various "TODO" items within this starter code.
'''
from __future__ import print_function
import copy
import math
import random
import argparse
from PIL import Image
import numpy as np
from pprint import pprint
from heapq import heappush,heappop
import matplotlib.pyplot as plt


g_CYCLE_TIME = .100

# Parameters you might need to use which will be set automatically
MAP_SIZE_X = None
MAP_SIZE_Y = None
map_array = []

# Default parameters will create a 4x4 grid to test with
g_MAP_SIZE_X = 2. # 2m wide
g_MAP_SIZE_Y = 1.5 # 1.5m tall
g_MAP_RESOLUTION_X = 0.5 # Each col represents 50cm
g_MAP_RESOLUTION_Y = 0.375 # Each row represents 37.5cm
g_NUM_X_CELLS = int(g_MAP_SIZE_X // g_MAP_RESOLUTION_X) # Number of columns in the grid map
g_NUM_Y_CELLS = int(g_MAP_SIZE_Y // g_MAP_RESOLUTION_Y) # Number of rows in the grid map

# Map from Lab 4: values of 0 indicate free space, 1 indicates occupied space
g_WORLD_MAP = [0] * g_NUM_Y_CELLS*g_NUM_X_CELLS # Initialize graph (grid) as array

# Source and Destination (I,J) grid coordinates
g_dest_coordinates = (3,3)
g_src_coordinates = (0,0)

dest_vertex = 3
source_vertex = 0


def create_test_map(map_array):
  # Takes an array representing a map of the world, copies it, and adds simulated obstacles
  num_cells = len(map_array)
  new_map = copy.copy(map_array)
  # Add obstacles to up to sqrt(n) vertices of the map
  for i in range(int(math.sqrt(len(map_array)))):
    random_cell = random.randint(0, num_cells-1)
    new_map[random_cell] = 1

  return new_map


def _load_img_to_intensity_matrix(img_filename):
  '''
  Helper function to read the world image containing obstacles
  YOu should not modify this
  '''
  global MAP_SIZE_X, MAP_SIZE_Y

  if img_filename is None:
      grid = np.zeros([800,1200])
      return grid

  img = Image.open(img_filename)

  MAP_SIZE_X = img.width
  MAP_SIZE_Y = img.height

  grid = np.zeros([img.height, img.width])
  for y in range(img.height):
      for x in range(img.width):
          pixel = img.getpixel((x,y))
          grid[y,x] = 255 - pixel[0] # Dark pixels have high values to indicate being occupied/having something interesting

  return grid


def vertex_index_to_ij(vertex_index):
  '''
  vertex_index: unique ID of graph vertex to be convered into grid coordinates
  Returns COL, ROW coordinates in 2D grid
  '''
  global g_NUM_X_CELLS
  return vertex_index % g_NUM_X_CELLS, vertex_index // g_NUM_X_CELLS

def ij_to_vertex_index(i,j):
  '''
  i: Column of grid map
  j: Row of grid map

  returns integer 'vertex index'
  '''
  global g_NUM_X_CELLS
  return j*g_NUM_X_CELLS + i


def ij_coordinates_to_xy_coordinates(i,j):
  '''
  i: Column of grid map
  j: Row of grid map

  returns (X, Y) coordinates in meters at the center of grid cell (i,j)
  '''
  global g_MAP_RESOLUTION_X, g_MAP_RESOLUTION_Y
  return (i+0.5)*g_MAP_RESOLUTION_X, (j+0.5)*g_MAP_RESOLUTION_Y

def xy_coordinates_to_ij_coordinates(x,y):
  '''
  i: Column of grid map
  j: Row of grid map

  returns (X, Y) coordinates in meters at the center of grid cell (i,j)
  '''
  global g_MAP_RESOLUTION_X, g_MAP_RESOLUTION_Y
  return int(i // g_MAP_RESOLUTION_X), int(j // g_MAP_RESOLUTION_Y)

# **********************************
# *      Core Dijkstra Functions   *
# **********************************

def get_travel_cost(vertex_source, vertex_dest):
  # Returns the cost of moving from vertex_source (int) to vertex_dest (int)
  # INSTRUCTIONS:

  '''
      This function should return 1 if:
        vertex_source and vertex_dest are neighbors in a 4-connected grid (i.e., N,E,S,W of each other but not diagonal) and neither is occupied in g_WORLD_MAP (i.e., g_WORLD_MAP isn't 1 for either)

      This function should return 1000 if:
        vertex_source corresponds to (i,j) coordinates outside the map
        vertex_dest corresponds to (i,j) coordinates outside the map
        vertex_source and vertex_dest are not adjacent to each other (i.e., more than 1 move away from each other)
  '''
  # source will store the row column coord of vertex_source in 2d grid
  source = vertex_index_to_ij(vertex_source)
  dest = vertex_index_to_ij(vertex_dest)
  if g_WORLD_MAP[vertex_source] == 1:
    return 1000
  elif source[0] < 0: #vertex source corresponded to (i)
    return 1000
  elif source[1] < 0: #vertex source corresponded to (j)
    return 1000
  elif source[0] > g_NUM_X_CELLS:
    return 1000
  elif source[1] > g_NUM_Y_CELLS:
    return 1000
  elif dest[0] < 0: #vertex source corresponded to (i)
    return 1000
  elif dest[1] < 0: #vertex source corresponded to (j)
    return 1000
  elif dest[0] > g_NUM_X_CELLS:
    return 1000
  elif dest[1] > g_NUM_Y_CELLS:
    return 1000
  elif g_WORLD_MAP[vertex_dest] == 1:
    return 1000
  elif abs(source[1]-dest[1])>1:
    return 1000
  elif abs(source[0]-dest[0])>1:
    return 1000
  else:
    return 1


  #return 100


def run_dijkstra(source_vertex):
  '''
  source_vertex: vertex index to find all paths back to
  returns: 'prev' array from a completed Dijkstra's algorithm run

  Function to return an array of ints corresponding to the 'prev' variable in Dijkstra's algorithm
  The 'prev' array stores the next vertex on the best path back to source_vertex.
  Thus, the returned array prev can be treated as a lookup table:  prev[vertex_index] = next vertex index on the path back to source_vertex
  '''
  global g_NUM_X_CELLS, g_NUM_Y_CELLS

  # Array mapping vertex_index to distance of shortest path from vertex_index to source_vertex.
  dist = [1000] * g_NUM_X_CELLS * g_NUM_Y_CELLS

  # Queue for identifying which vertices are up to still be explored:
  # Will contain tuples of (vertex_index, cost), sorted such that the min cost is first to be extracted (explore cheapest/most promising vertices first)
  Q_cost = []

  # Array of ints for storing the next step (vertex_index) on the shortest path back to source_vertex for each vertex in the graph
  prev = [-1] * g_NUM_X_CELLS*g_NUM_Y_CELLS

  # Insert your Dijkstra's code here. Don't forget to initialize Q_cost properly!
  #print("Source: ", source_vertex  )
  #Goal: (3,1)
  #print("Goal: ",dest_vertex)


  heappush(Q_cost, (source_vertex, 0))
  n = len(Q_cost) #length of the cost vector
  while True:
    Recent, RecentCost = heappop(Q_cost)
    #RecentCost = heappop(Q_cost)

    Array = [Recent-1, Recent+1, Recent+g_NUM_X_CELLS, Recent-g_NUM_X_CELLS]

    for i in Array:
      value = get_travel_cost(Recent,i)
      result = get_travel_cost(Recent,i) + RecentCost
      if result < dist[i]:
        heappush(Q_cost,(i, result))
        dist[i] = result
        prev[i] = Recent

        if i == dest_vertex:
	  #print('Prev',prev)
          #print('Dist',dist)
          return prev


def reconstruct_path(prev, source_vertex, dest_vertex):
  '''
  Given a populated 'prev' array, a source vertex_index, and destination vertex_index,
  allocate and return an integer array populated with the path from source to destination.
  The first entry of your path should be source_vertex and the last entry should be the dest_vertex.
  If there is no path between source_vertex and dest_vertex, as indicated by hitting a '-1' on the
  path from dest to source, return an empty list.
  '''
  final_path = []


  # TODO: Insert your code here
  curr_vertex = dest_vertex

  while source_vertex!=curr_vertex:
    final_path.insert(0,curr_vertex)
    curr_vertex = prev[curr_vertex]
    if curr_vertex==-1:
      return []
  final_path.insert(0,source_vertex)
  return final_path


def render_map(map_array):


  '''
  TODO-
    Display the map in the following format:
    Use " . " for free grid cells
    Use "[ ]" for occupied grid cells

    Example:
    For g_WORLD_MAP = [0, 0, 1, 0,
                       0, 1, 1, 0,
                       0, 0, 0, 0,
                       0, 0, 0, 0]
    There are obstacles at (I,J) coordinates: [ (2,0), (1,1), (2,1) ]
    The map should render as:
      .  .  .  .
      .  .  .  .
      . [ ][ ] .
      .  . [ ] .


    Make sure to display your map so that I,J coordinate (0,0) is in the bottom left.
    (To do this, you'll probably want to iterate from row 'J-1' to '0')
  '''

  '''
  occupied = '[]'
  free = '.'
  if len(map_array) > 0:
    for i in reversed(range(len(map_array))):
      for j in range(len(map_array)):
        if map_array[i][j] == 0:
          map_array[i][j] = free
        else:
          map_array[i][j] = occupied
  '''
  row = ''
  for i in range(len(map_array)):
    if map_array[i] == 1:
      row += '[ ]'
    elif map_array[i] == 0:
      row += ' . '
    else:
      row += ' X '
    if (i+1) % (g_NUM_X_CELLS) == 0:
      row += '\n'
      print(row)
      row = ''

  pass


def part_1():
  global g_WORLD_MAP, map_array, g_src_coordinates,source_vertex,dest_vertex

  # TODO: Initialize a grid map to use for your test -- you may use create_test_map for this, or manually set one up with obstacles
  map_array = g_WORLD_MAP
  g_map = create_test_map(map_array)
  g_WORLD_MAP = g_map
  # Use render_map to render your initialized obstacle map
  r_map = render_map(g_map)

  # TODO: Find a path from the (I,J) coordinate pair in g_src_coordinates to the one in g_dest_coordinates using run_dijkstra and reconstruct_path
  if g_map[dest_vertex]==0 & g_map[source_vertex]==0:
    r_d = run_dijkstra(source_vertex)
    r_p = reconstruct_path(r_d, source_vertex, dest_vertex)
  #print(r_p)
  #print(r_d)
    source = vertex_index_to_ij(source_vertex)
    dest = vertex_index_to_ij(dest_vertex)
    print('Source: ', source)
    print('Goal: ', dest)
    if len(r_p)==0:
      print('No path between source and goal')
    else:
      print('Path:')
      for n in range(len(r_p)):
        print(r_p[n], end=" -> ")
  elif g_WORLD_MAP[source_vertex]==1:
    print('Source is blocked')
  else:
    print('Goal is blocked')
  '''
  TODO-
    Display the final path in the following format:
    Source: (0,0)
    Goal: (3,1)
    0 -> 1 -> 2 -> 6 -> 7
  '''


def part_2(args):
  global g_dest_coordinates
  global g_src_coordinates
  global g_WORLD_MAP, g_NUM_X_CELLS, g_NUM_Y_CELLS, source_vertex, dest_vertex

  g_src_coordinates = (args.src_coordinates[0], args.src_coordinates[1])
  g_dest_coordinates = (args.dest_coordinates[0], args.dest_coordinates[1])

  # pixel_grid has intensity values for all the pixels
  # You will have to convert it to the earlier 0 and 1 matrix yourself
  pixel_grid = _load_img_to_intensity_matrix(args.obstacles)

  '''
  TODO -
  1) Compute the g_WORLD_MAP -- depending on the resolution, you need to decide if your cell is an obstacle cell or a free cell.
  2) Run Dijkstra's to get the plan
  3) Show your plan/path on the image
  Feel free to add more helper functions
  '''

  #### Your code goes here ####

  #print(pixel_grid[0][0])

  cell_size = 40
  g_NUM_X_CELLS = 1200
  g_NUM_Y_CELLS = 800
  num_cells_x = g_NUM_X_CELLS/cell_size #30
  num_cells_y = g_NUM_Y_CELLS/cell_size #20
  cell_array = [[0]*num_cells_y]*num_cells_x
#Make grid of cells to run Djikstras
  for i in range(num_cells_x):
    for j in range(num_cells_y):
      dark_pixels=0
      for k in range(cell_size):
        for l in range(cell_size):
          if pixel_grid[i*num_cells_y+k][j*num_cells_x+l]>=1:
            dark_pixels = dark_pixels+1
      dark_ratio = float(dark_pixels)/(cell_size*cell_size)
      #print(dark_ratio)
      if dark_ratio > .2:
        cell_array[i][j]=1
        g_WORLD_MAP.append(1)
      else:
        g_WORLD_MAP.append(0)

  g_NUM_X_CELLS = num_cells_x
  g_NUM_Y_CELLS = num_cells_y

  source_vertex = int(ij_to_vertex_index(num_cells_x-1-float(g_src_coordinates[0])*num_cells_x/1.8, float(g_src_coordinates[1])*num_cells_y/1.2))
  dest_vertex = int(ij_to_vertex_index(num_cells_x-1-float(g_dest_coordinates[0])*num_cells_x/1.8, float(g_dest_coordinates[1])*num_cells_y/1.2))

  if g_WORLD_MAP[dest_vertex]==0 & g_WORLD_MAP[source_vertex]==0:
    r_d = run_dijkstra(source_vertex)
    r_p = reconstruct_path(r_d, source_vertex, dest_vertex)
    source = vertex_index_to_ij(source_vertex)
    dest = vertex_index_to_ij(dest_vertex)
    print('Source: ', source)
    print('Goal: ', dest)
    if len(r_p)==0:
      print('No path between source and goal')
    else:
      print('Path:')
      for n in range(len(r_p)):
        g_WORLD_MAP[r_p[n]] = 2
      render_map(g_WORLD_MAP)
  elif g_WORLD_MAP[source_vertex]==1:
    print('Source is blocked')
  else:
    print('Goal is blocked')

#Create image with path
'''
  fig, ax = plt.subplots()
  ax.matshow(cell_array, cmap=plt.cm.Blues)
  h, w = len(cell_array), len(cell_array[0])
  for y in range(h):
    for x in range(w):
      if ij_to_vertex_index(x,y) in r_p:
        cell_array[y] [x] = 255
'''
'''
  df_cm = pd.DataFrame(cell_array, index = [i for i in range(11)],columns = [i for i in range(11)])
  plt.figure(figsize = (15,7))
  plt.matshow(df_cm)
'''

if __name__ == "__main__":
  parser = argparse.ArgumentParser(description="Dijkstra on image file")
  parser.add_argument('-s','--src_coordinates', nargs=2, default=[1.2, 0.2], help='Starting x, y location in world coords')
  parser.add_argument('-g','--dest_coordinates', nargs=2, default=[0.3, 0.7], help='Goal x, y location in world coords')
  parser.add_argument('-o','--obstacles', nargs='?', type=str, default='obstacles_test1.png', help='Black and white image showing the obstacle locations')
  args = parser.parse_args()
  part_2(args)
	'''
	IMPORTANT: Read through the code before beginning implementation!
	Your solution should fill in the various "TODO" items within this starter code.
	'''
	from __future__ import print_function
	import copy
	import math
	import random
	import argparse
	from PIL import Image
	import numpy as np
	from pprint import pprint
	from heapq import heappush,heappop
	import matplotlib.pyplot as plt


	g_CYCLE_TIME = .100

	# Parameters you might need to use which will be set automatically
	MAP_SIZE_X = None
	MAP_SIZE_Y = None
	map_array = []

	# Default parameters will create a 4x4 grid to test with
	g_MAP_SIZE_X = 2. # 2m wide
	g_MAP_SIZE_Y = 1.5 # 1.5m tall
	g_MAP_RESOLUTION_X = 0.5 # Each col represents 50cm
	g_MAP_RESOLUTION_Y = 0.375 # Each row represents 37.5cm
	g_NUM_X_CELLS = int(g_MAP_SIZE_X // g_MAP_RESOLUTION_X) # Number of columns in the grid map
	g_NUM_Y_CELLS = int(g_MAP_SIZE_Y // g_MAP_RESOLUTION_Y) # Number of rows in the grid map

	# Map from Lab 4: values of 0 indicate free space, 1 indicates occupied space
	g_WORLD_MAP = [0] * g_NUM_Y_CELLS*g_NUM_X_CELLS # Initialize graph (grid) as array

	# Source and Destination (I,J) grid coordinates
	g_dest_coordinates = (3,3)
	g_src_coordinates = (0,0)

	dest_vertex = 3
	source_vertex = 0


	def create_test_map(map_array):
	# Takes an array representing a map of the world, copies it, and adds simulated obstacles
	num_cells = len(map_array)
	new_map = copy.copy(map_array)
	# Add obstacles to up to sqrt(n) vertices of the map
	for i in range(int(math.sqrt(len(map_array)))):
	random_cell = random.randint(0, num_cells-1)
	new_map[random_cell] = 1

	return new_map


	def _load_img_to_intensity_matrix(img_filename):
	'''
	Helper function to read the world image containing obstacles
	YOu should not modify this
	'''
	global MAP_SIZE_X, MAP_SIZE_Y

	if img_filename is None:
	grid = np.zeros([800,1200])
	return grid

	img = Image.open(img_filename)

	MAP_SIZE_X = img.width
	MAP_SIZE_Y = img.height

	grid = np.zeros([img.height, img.width])
	for y in range(img.height):
	for x in range(img.width):
	pixel = img.getpixel((x,y))
	grid[y,x] = 255 - pixel[0] # Dark pixels have high values to indicate being occupied/having something interesting

	return grid


	def vertex_index_to_ij(vertex_index):
	'''
	vertex_index: unique ID of graph vertex to be convered into grid coordinates
	Returns COL, ROW coordinates in 2D grid
	'''
	global g_NUM_X_CELLS
	return vertex_index % g_NUM_X_CELLS, vertex_index // g_NUM_X_CELLS

	def ij_to_vertex_index(i,j):
	'''
	i: Column of grid map
	j: Row of grid map

	returns integer 'vertex index'
	'''
	global g_NUM_X_CELLS
	return j*g_NUM_X_CELLS + i


	def ij_coordinates_to_xy_coordinates(i,j):
	'''
	i: Column of grid map
	j: Row of grid map

	returns (X, Y) coordinates in meters at the center of grid cell (i,j)
	'''
	global g_MAP_RESOLUTION_X, g_MAP_RESOLUTION_Y
	return (i+0.5)g_MAP_RESOLUTION_X, (j+0.5)g_MAP_RESOLUTION_Y

	def xy_coordinates_to_ij_coordinates(x,y):
	'''
	i: Column of grid map
	j: Row of grid map

	returns (X, Y) coordinates in meters at the center of grid cell (i,j)
	'''
	global g_MAP_RESOLUTION_X, g_MAP_RESOLUTION_Y
	return int(i // g_MAP_RESOLUTION_X), int(j // g_MAP_RESOLUTION_Y)

	# **********************************
	# * Core Dijkstra Functions *
	# **********************************

	def get_travel_cost(vertex_source, vertex_dest):
	# Returns the cost of moving from vertex_source (int) to vertex_dest (int)
	# INSTRUCTIONS:

	'''
	This function should return 1 if:
	vertex_source and vertex_dest are neighbors in a 4-connected grid (i.e., N,E,S,W of each other but not diagonal) and neither is occupied in g_WORLD_MAP (i.e., g_WORLD_MAP isn't 1 for either)

	This function should return 1000 if:
	vertex_source corresponds to (i,j) coordinates outside the map
	vertex_dest corresponds to (i,j) coordinates outside the map
	vertex_source and vertex_dest are not adjacent to each other (i.e., more than 1 move away from each other)
	'''
	# source will store the row column coord of vertex_source in 2d grid
	source = vertex_index_to_ij(vertex_source)
	dest = vertex_index_to_ij(vertex_dest)
	if g_WORLD_MAP[vertex_source] == 1:
	return 1000
	elif source[0] < 0: #vertex source corresponded to (i)
	return 1000
	elif source[1] < 0: #vertex source corresponded to (j)
	return 1000
	elif source[0] > g_NUM_X_CELLS:
	return 1000
	elif source[1] > g_NUM_Y_CELLS:
	return 1000
	elif dest[0] < 0: #vertex source corresponded to (i)
	return 1000
	elif dest[1] < 0: #vertex source corresponded to (j)
	return 1000
	elif dest[0] > g_NUM_X_CELLS:
	return 1000
	elif dest[1] > g_NUM_Y_CELLS:
	return 1000
	elif g_WORLD_MAP[vertex_dest] == 1:
	return 1000
	elif abs(source[1]-dest[1])>1:
	return 1000
	elif abs(source[0]-dest[0])>1:
	return 1000
	else:
	return 1


	#return 100



	def run_dijkstra(source_vertex):
	'''
	source_vertex: vertex index to find all paths back to
	returns: 'prev' array from a completed Dijkstra's algorithm run

	Function to return an array of ints corresponding to the 'prev' variable in Dijkstra's algorithm
	The 'prev' array stores the next vertex on the best path back to source_vertex.
	Thus, the returned array prev can be treated as a lookup table: prev[vertex_index] = next vertex index on the path back to source_vertex
	'''
	global g_NUM_X_CELLS, g_NUM_Y_CELLS

	# Array mapping vertex_index to distance of shortest path from vertex_index to source_vertex.
	dist = [1000] * g_NUM_X_CELLS * g_NUM_Y_CELLS

	# Queue for identifying which vertices are up to still be explored:
	# Will contain tuples of (vertex_index, cost), sorted such that the min cost is first to be extracted (explore cheapest/most promising vertices first)
	Q_cost = []

	# Array of ints for storing the next step (vertex_index) on the shortest path back to source_vertex for each vertex in the graph
	prev = [-1] * g_NUM_X_CELLS*g_NUM_Y_CELLS

	# Insert your Dijkstra's code here. Don't forget to initialize Q_cost properly!
	#print("Source: ", source_vertex )
	#Goal: (3,1)
	#print("Goal: ",dest_vertex)


	heappush(Q_cost, (source_vertex, 0))
	n = len(Q_cost) #length of the cost vector
	while True:
	Recent, RecentCost = heappop(Q_cost)
	#RecentCost = heappop(Q_cost)

	Array = [Recent-1, Recent+1, Recent+g_NUM_X_CELLS, Recent-g_NUM_X_CELLS]

	for i in Array:
	value = get_travel_cost(Recent,i)
	result = get_travel_cost(Recent,i) + RecentCost
	if result < dist[i]:
	heappush(Q_cost,(i, result))
	dist[i] = result
	prev[i] = Recent

	if i == dest_vertex:
	#print('Prev',prev)
	#print('Dist',dist)
	return prev


	def reconstruct_path(prev, source_vertex, dest_vertex):
	'''
	Given a populated 'prev' array, a source vertex_index, and destination vertex_index,
	allocate and return an integer array populated with the path from source to destination.
	The first entry of your path should be source_vertex and the last entry should be the dest_vertex.
	If there is no path between source_vertex and dest_vertex, as indicated by hitting a '-1' on the
	path from dest to source, return an empty list.
	'''
	final_path = []


	# TODO: Insert your code here
	curr_vertex = dest_vertex

	while source_vertex!=curr_vertex:
	final_path.insert(0,curr_vertex)
	curr_vertex = prev[curr_vertex]
	if curr_vertex==-1:
	return []
	final_path.insert(0,source_vertex)
	return final_path


	def render_map(map_array):


	'''
	TODO-
	Display the map in the following format:
	Use " . " for free grid cells
	Use "[ ]" for occupied grid cells

	Example:
	For g_WORLD_MAP = [0, 0, 1, 0,
	0, 1, 1, 0,
	0, 0, 0, 0,
	0, 0, 0, 0]
	There are obstacles at (I,J) coordinates: [ (2,0), (1,1), (2,1) ]
	The map should render as:
	. . . .
	. . . .
	. [ ][ ] .
	. . [ ] .


	Make sure to display your map so that I,J coordinate (0,0) is in the bottom left.
	(To do this, you'll probably want to iterate from row 'J-1' to '0')
	'''

	'''
	occupied = '[]'
	free = '.'
	if len(map_array) > 0:
	for i in reversed(range(len(map_array))):
	for j in range(len(map_array)):
	if map_array[i][j] == 0:
	map_array[i][j] = free
	else:
	map_array[i][j] = occupied
	'''
	row = ''
	for i in range(len(map_array)):
	if map_array[i] == 1:
	row += '[ ]'
	elif map_array[i] == 0:
	row += ' . '
	else:
	row += ' X '
	if (i+1) % (g_NUM_X_CELLS) == 0:
	row += '\n'
	print(row)
	row = ''

	pass


	def part_1():
	global g_WORLD_MAP, map_array, g_src_coordinates,source_vertex,dest_vertex

	# TODO: Initialize a grid map to use for your test -- you may use create_test_map for this, or manually set one up with obstacles
	map_array = g_WORLD_MAP
	g_map = create_test_map(map_array)
	g_WORLD_MAP = g_map
	# Use render_map to render your initialized obstacle map
	r_map = render_map(g_map)

	# TODO: Find a path from the (I,J) coordinate pair in g_src_coordinates to the one in g_dest_coordinates using run_dijkstra and reconstruct_path
	if g_map[dest_vertex]==0 & g_map[source_vertex]==0:
	r_d = run_dijkstra(source_vertex)
	r_p = reconstruct_path(r_d, source_vertex, dest_vertex)
	#print(r_p)
	#print(r_d)
	source = vertex_index_to_ij(source_vertex)
	dest = vertex_index_to_ij(dest_vertex)
	print('Source: ', source)
	print('Goal: ', dest)
	if len(r_p)==0:
	print('No path between source and goal')
	else:
	print('Path:')
	for n in range(len(r_p)):
	print(r_p[n], end=" -> ")
	elif g_WORLD_MAP[source_vertex]==1:
	print('Source is blocked')
	else:
	print('Goal is blocked')
	'''
	TODO-
	Display the final path in the following format:
	Source: (0,0)
	Goal: (3,1)
	0 -> 1 -> 2 -> 6 -> 7
	'''


	def part_2(args):
	global g_dest_coordinates
	global g_src_coordinates
	global g_WORLD_MAP, g_NUM_X_CELLS, g_NUM_Y_CELLS, source_vertex, dest_vertex

	g_src_coordinates = (args.src_coordinates[0], args.src_coordinates[1])
	g_dest_coordinates = (args.dest_coordinates[0], args.dest_coordinates[1])

	# pixel_grid has intensity values for all the pixels
	# You will have to convert it to the earlier 0 and 1 matrix yourself
	pixel_grid = _load_img_to_intensity_matrix(args.obstacles)

	'''
	TODO -
	1) Compute the g_WORLD_MAP -- depending on the resolution, you need to decide if your cell is an obstacle cell or a free cell.
	2) Run Dijkstra's to get the plan
	3) Show your plan/path on the image
	Feel free to add more helper functions
	'''

	#### Your code goes here ####

	#print(pixel_grid[0][0])

	cell_size = 40
	g_NUM_X_CELLS = 1200
	g_NUM_Y_CELLS = 800
	num_cells_x = g_NUM_X_CELLS/cell_size #30
	num_cells_y = g_NUM_Y_CELLS/cell_size #20
	cell_array = [[0]num_cells_y]num_cells_x
	#Make grid of cells to run Djikstras
	for i in range(num_cells_x):
	for j in range(num_cells_y):
	dark_pixels=0
	for k in range(cell_size):
	for l in range(cell_size):
	if pixel_grid[inum_cells_y+k][jnum_cells_x+l]>=1:
	dark_pixels = dark_pixels+1
	dark_ratio = float(dark_pixels)/(cell_size*cell_size)
	#print(dark_ratio)
	if dark_ratio > .2:
	cell_array[i][j]=1
	g_WORLD_MAP.append(1)
	else:
	g_WORLD_MAP.append(0)

	g_NUM_X_CELLS = num_cells_x
	g_NUM_Y_CELLS = num_cells_y

	source_vertex = int(ij_to_vertex_index(num_cells_x-1-float(g_src_coordinates[0])num_cells_x/1.8, float(g_src_coordinates[1])num_cells_y/1.2))
	dest_vertex = int(ij_to_vertex_index(num_cells_x-1-float(g_dest_coordinates[0])num_cells_x/1.8, float(g_dest_coordinates[1])num_cells_y/1.2))

	if g_WORLD_MAP[dest_vertex]==0 & g_WORLD_MAP[source_vertex]==0:
	r_d = run_dijkstra(source_vertex)
	r_p = reconstruct_path(r_d, source_vertex, dest_vertex)
	source = vertex_index_to_ij(source_vertex)
	dest = vertex_index_to_ij(dest_vertex)
	print('Source: ', source)
	print('Goal: ', dest)
	if len(r_p)==0:
	print('No path between source and goal')
	else:
	print('Path:')
	for n in range(len(r_p)):
	g_WORLD_MAP[r_p[n]] = 2
	render_map(g_WORLD_MAP)
	elif g_WORLD_MAP[source_vertex]==1:
	print('Source is blocked')
	else:
	print('Goal is blocked')

	#Create image with path
	'''
	fig, ax = plt.subplots()
	ax.matshow(cell_array, cmap=plt.cm.Blues)
	h, w = len(cell_array), len(cell_array[0])
	for y in range(h):
	for x in range(w):
	if ij_to_vertex_index(x,y) in r_p:
	cell_array[y] [x] = 255
	'''
	'''
	df_cm = pd.DataFrame(cell_array, index = [i for i in range(11)],columns = [i for i in range(11)])
	plt.figure(figsize = (15,7))
	plt.matshow(df_cm)
	'''

	if __name__ == "__main__":
	parser = argparse.ArgumentParser(description="Dijkstra on image file")
	parser.add_argument('-s','--src_coordinates', nargs=2, default=[1.2, 0.2], help='Starting x, y location in world coords')
	parser.add_argument('-g','--dest_coordinates', nargs=2, default=[0.3, 0.7], help='Goal x, y location in world coords')
	parser.add_argument('-o','--obstacles', nargs='?', type=str, default='obstacles_test1.png', help='Black and white image showing the obstacle locations')
	args = parser.parse_args()
	part_2(args)