When getting the nn for path tracking, take the closest one on the ref path...

When getting the nn for path tracking, take the closest one on the ref path that ocurrs the soonest and has not been visited already