/* particleswarm.h    by  M. Hazen
   Implements particle swarm optimization. */

#include <stdlib.h>  // rand,srand
#include <time.h>
#include "particleswarm.h"

/* Optimize takes a pointer to an objective function, 
   plus information about that functions operational range.  
   Optimize executes an optimization run and modifies
   the argument gb to hold the optimal operating point. */
float* optimize(float (*obj)(float*), float* mins, float* maxs) {
  // initialize population
  particle* population;
  // global best position and global best value
  float* gb =(float*) malloc(DIM*sizeof(float));
  float gbval;

  population = initialize_opt(obj, mins, maxs, gb, &gbval);

  // for MAXITS update population and global best
  for (int i = 0; i < MAXITS; i++) {
    update_gb(population, gb, &gbval);
    for (int p = 0; p < POPSIZE; p++) {
      update_vel(&population[p], gb);
      update_pos(obj, &population[p]);
    }
  }

  free(population);
  return gb;
}

/* initialize_opt creates a new randomly seeded population */
particle*  initialize_opt(float (*obj)(float*),
                           float* mins, float* maxs,
                           float* gb, float *gbval) {
  /* initialize random seed: */
  srand(time(NULL));
  particle* population =(particle*) malloc(POPSIZE*sizeof(particle));

  // intialize each particle
  for (int p = 0; p < POPSIZE; p++) {
    // initialize location
    for (int d = 0; d < DIM; d++) {
      float tmpnum =(float)(rand() % 10);  // num between 0&9
      tmpnum =(tmpnum * (maxs[d] - mins[d])/10) + mins[d];  // scale
      population[p].pos[d] = tmpnum;
      population[p].pb[d] = tmpnum;
      tmpnum = tmpnum * (((float)(rand() % 10))-5)/10;
      population[p].vel[d] = tmpnum;
    }
    // initialize pb
    population[p].pbval = obj(population[p].pb);
  }
  for (int d = 0; d < DIM; d++) {
    gb[d] = population[0].pos[d];
  }
  *gbval = population[0].pbval;

  return population;
}

/* update_vel updates the velocity of a single particle. */
void update_vel(particle* part, float* gb) {
    for (int d = 0; d < DIM; d++) {
      part->vel[d] = part->vel[d] +
        2.0*((float)(rand() %100)/100)*(part->pb[d] - part->pos[d]) +
        2.0*((float)(rand() %100)/100)*(gb[d] - part->pos[d]);
    }
}

void update_pos(float (*obj)(float*), particle* part) {
  for (int d = 0; d < DIM; d++) {
    part->pos[d] = part->pos[d] + part->vel[d];
  }
  if (obj(part->pos) < part->pbval) {
    part->pbval = obj(part->pos);
    for (int d = 0; d < DIM; d++) {
      part->pb[d] = part->pos[d];
    }
  }
}

void update_gb(particle* population, float* gb, float* gbval) {
  for (int p = 0; p < POPSIZE; p++) {
    if (population[p].pbval < *gbval) {
      *gbval = population[p].pbval;
      for (int d = 0; d < DIM; d++) {
        gb[d] = population[p].pb[d];
      }
    }
  }
}