A* algorithm now working