Fixed a* pathfinding