Working PSO. Need to implement a better fitness function