Fix bug and add FindNearestStolps helper function