Moved path finding stuff out of PathWander state