import numpy as np
import queue
from suppClass import *

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

def Search(world,start,goal):
    mat = np.matrix(world)
    row,col = mat.shape
    graph = Grid(row,col)
    
    graph.walls =list(zip(np.where(mat==1)[0].tolist(),np.where(mat==1)[1].tolist()))
    del(mat)

    frontier = 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()
        
        if current == goal:
            break
        
        for next in graph.neighbors(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

    current = goal
    path = []
    while current != start:
        path.append(current)
        current = came_from[current]
    path.append(start)
    path.reverse()
    return path


if __name__ == '__main__' :
    world_state                   = [[0,0,1,0,0,0],[0,0,1,0,0,0],[0,0,0,0,1,0],[0,0,0,0,1,0],[0,0,1,1,1,0],[0,0,1,0,1,0]]
    robot_pose, goal_pose         = [(2,0),(5,5)]
    optimal_path                  = Search(world_state,robot_pose,goal_pose)
    print optimal_path
