Still working on SMPSO
SMPSO refactored and tested