A* fully implemented but with a bug