basic BFS added. finds path and lists all of the coordinates on the path:
Refactored the utility function.
Added unit test framework
Mostly complete A-Star