Implementation of A* pathfinding in Swift