A* Pathfinding in java