Improving path finding implementation