Example astar pathfinding code