Finished PSO, need to finish helper functions inside PSOSolutionSet