A bit more code cleanup and added some stagnation related functions for HPSO.