finished Astar now need to test