Tidy up code and move some functions around

known issue with flocking avoidance.