#include "stdio.h"
#include "stdlib.h"
#define _USE_MATH_DEFINES
#include "math.h"
//#include "vld.h"

#define POP 1000
#define D 2

typedef struct {
	float *x;
	float *sigma;
	float fitness;
} Individuo;

float RNG(float min, float max) {
	float x = ((float)rand() / RAND_MAX) * fabs(max-min)+ min;
	return x;
}

Individuo *createPop(int n) {

	Individuo *p = (Individuo*)malloc(n * sizeof(Individuo));
	for (int i = 0; i < n; i++)
	{
		p[i].x = (float*)calloc(D, sizeof(float));
		for (int j = 0; j < D; j++)
		{
			p[i].x[j] = RNG(-5.12f, 5.12f);
			p[i].sigma[j] = 1.0f;
		}
		p[i].fitness = 0.0f;
	}
	return p;
}

void freePop(Individuo *p, int n) {
	for (int i = 0; i < n; i++)
	{
		free(p[i].x);
	}
	free(p);
	p = NULL;
}

void printPop(Individuo *p, int n) {
	for (int i = 0; i < n; i++)
	{
		printf("[%d]", i);
		for (int j = 0; j < D; j++)
		{
			printf("% -.2f ", p[i].x[j]);
		} 
		printf("   [%f]\n", p[i].fitness);
	}
}

float fitness(float *x) {
	
	int i;
	double a = 20;
	double b = 0.2;
	double c = 2*M_PI;
	double s1 = 0;
	double s2 = 0;

	for(i = 0; i < D; i++){
	   s1 += (x[i]*x[i]);
	   s2 += cos(c*x[i]);
	}

	float r =  (float) (-a* exp(-b * sqrt( (1.0f/D) * s1)) - exp( (1.0f/D) * s2) + a + exp(1.0f));
	return r;

}

void computeFitness(Individuo *p, int n) {
	for (int i = 0; i < n; i++)
	{
		p[i].fitness = fitness(p[i].x);
	}
}

double
randn (double mu, double sigma)
{
  double U1, U2, W, mult;
  static double X1, X2;
  static int call = 0;

  if (call == 1)
	{
	  call = !call;
	  return (mu + sigma * (double) X2);
	}

  do
	{
	  U1 = -1 + ((double) rand () / RAND_MAX) * 2;
	  U2 = -1 + ((double) rand () / RAND_MAX) * 2;
	  W = U1*U1 + U2*U2;
	}
  while (W >= 1 || W == 0);

  mult = sqrt ((-2 * log (W)) / W);
  X1 = U1 * mult;
  X2 = U2 * mult;

  call = !call;

  return (mu + sigma * (double) X1);
}

Individuo *mutacao(Individuo *p, int n) {
	Individuo *o = createPop(n);
	float tau = 1 / sqrt((float)D);
	for (int i = 0; i < n; i++)
	{
		for (int j = 0; j < D; j++)
		{
			p[i].sigma *= (float)exp(tau * randn((double)0,(double)1));
			o[i].x[j] = p[i].x[j] + (float)randn((double)0, (double)p[i].sigma);
		}
	}
	computeFitness(o, n);
	return o;
}

void printBest(Individuo *p, int n, int g) {
	float best = 999;
	int k = 0;
	for (int i = 0; i < n; i++)
	{
		if (p[i].fitness < best)
		{
			best = p[i].fitness;
			k = i;
		}
	}

	printf("G: %d   ", g);
	for (int i = 0; i < D; i++)
	{
		printf("% -.2e ", p[k].x[i]);
	}
	printf("   Fitness: %e\n", p[k].fitness);

}

void copyFromTo(Individuo *from, Individuo *to) {
	for (int i = 0; i < D; i++)
	{
		to->x[i] = from->x[i];
	}
	to->fitness = from->fitness;
	to->sigma   = from->sigma;
}

int cmpfunc(const void *p1, const void *p2) {

	Individuo *i1 = (Individuo*)p1;
	Individuo *i2 = (Individuo*)p2;

	float f1 = i1->fitness;
	float f2 = i2->fitness;

	if (f1 < f2)
		return -1;
	else if (f1 > f2)
		return 1;
	else
		return 0;
}

void select(Individuo *p, Individuo *o, int n) {

	Individuo *m = createPop(POP + n);
	for (int i = 0; i < POP; i++)
	{
		copyFromTo(&p[i], &m[i]);
	}

	for (int i = POP; i < POP+n; i++)
	{
		copyFromTo(&o[i-POP], &m[i]);
	}

	qsort(m, POP + n, sizeof(Individuo), cmpfunc);

	for (int i = 0; i < POP; i++)
	{
		copyFromTo(&m[i], &p[i]);
	}
	//printPop(m, POP + n);

	freePop(m, POP + n);
}

int main() {

	srand(0);
	Individuo *p = createPop(POP);
	Individuo *o;
	int g = 0;
	while (g < 1000) {
		computeFitness(p, POP);
		o = mutacao(p, POP);
		//freePop(p, POP);
		//p = o;
		select(p, o, POP);
		freePop(o, POP);
		//printBest(p, POP, g);
		g++;
	}

	freePop(p, POP);
}