Finished up to question 4: A* search. including generic implementation for Graph class