Based on A* search algorithm (http://en.wikipedia.org/wiki/A*_search_algorithm)