Implemented A* pathfinding algorithm with very basic heuristic