Working on graph implementation and getting astar finder working