task2.py

import heapq
from collections import deque
import math
import numpy as np
import pandas as pd
import Queue as Q
import networkx as nx


def heuristic(a, b):
    (x1, y1) = a
    (x2, y2) = b
    return abs(x1 - x2) + abs(y1 - y2)


def a_star_search(graph, start, goal):
    frontier = Q.PriorityQueue()
    frontier.put(start, 0)
    came_from = {}
    cost_so_far = {}
    came_from[start] = None
    cost_so_far[start] = 0
    
    while not frontier.empty():
        current = frontier.get()

        # print current
        
        if current == goal:
            break
        
        for next in graph[current]:
            new_cost = cost_so_far[current] + graph.cost(current, next)
            if next not in cost_so_far or new_cost < cost_so_far[next]:
                cost_so_far[next] = new_cost
                priority = new_cost + heuristic(goal, next)
                frontier.put(next, priority)
                came_from[next] = current
    
    return came_from, cost_so_far



def construct_graph():
	grid = np.arange(100).reshape(10,10)
	# print grid
	return grid



if __name__ == '__main__':
	start, goal = (1, 4), (7, 8)
	
	G = construct_graph()
	print G


	graph = nx.DiGraph(G)

	# nx.draw(G, with_labels=True)

	print graph[1]
	
	came_from, cost_so_far = a_star_search(graph, start, goal)

	print came_from
	print cost_so_far