Implemented correctly the A* pathfinding algorithm