A* Pathfinding algorithm implemented in Java