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

#define D 50
#define F 0.6
#define CR 0.8
#define GMAX 200000
#define NPOP 1000
#define MIN -5.12
#define MAX  5.12


gsl_rng *r;
size_t *v = NULL;

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

float uniform() {
	return gsl_rng_uniform(r);
}

void init() {
	gsl_rng_env_setup();
	const gsl_rng_type *T = gsl_rng_default;
	r = gsl_rng_alloc(T);
}

void finalize() {
	gsl_rng_free(r);
	if (v != NULL) free(v);
}

Individuo *createPop(size_t n) {
	Individuo *p = (Individuo*)malloc(sizeof(Individuo) * n);
	for (int i = 0; i < n; i++)
	{
		p[i].x = (float*) malloc(sizeof(float) * D);
		for (int j = 0; j < D; j++)
		{
			p[i].x[j] = uniform() * fabs(MAX - MIN) + MIN;
		}
		p[i].fitness = 0.0f;
	}
	return p;
}

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

float rastrigin(float *x) {

	float s = 0.0f;
	for (int i = 0; i < D; i++)
	{
		s += x[i]*x[i] -10 * cos(2.0f * M_PI * x[i]);
	}

	s += 10 * D;

	return s;

}

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

}

size_t *sample(size_t n) {
	if (v == NULL) {
		v = (size_t*)malloc(sizeof(size_t) * n);
		for (size_t i = 0; i < n; i++)
		{
			v[i] = i;
		}
	}
	size_t k = 3;
	size_t *s = (size_t*)malloc(sizeof(size_t) * k);
	gsl_ran_sample(r, s, k, v, n, sizeof(size_t));
	return s;
}

void printBest(Individuo *p, int n, int g) {

	if ((g % 50) != 0) return;

	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;
}

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(NPOP + n);
	for (int i = 0; i < NPOP; i++)
	{
		copyFromTo(&p[i], &m[i]);
	}

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

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

	for (int i = 0; i < NPOP; i++)
	{
		copyFromTo(&m[i], &p[i]);
	}

	freePop(m, NPOP + n);
}


int main() {

	init();

	Individuo *p = createPop(NPOP);
	Individuo *o = createPop(NPOP);
	fitnessPop(p, NPOP);
	size_t g = 0;
	size_t *R;
	size_t iRand;
	while (g < GMAX)
	{
		for (int i = 0; i < NPOP; i++)
		{
			R = sample(NPOP);
			iRand = gsl_rng_uniform_int(r, D-1);
			for (int j = 0; j < D; j++)
			{
				if ((uniform() <= CR) || (j == iRand))
					o[i].x[j] = p[R[0]].x[j] + F * (p[R[1]].x[j] - p[R[2]].x[j]);
				else
					o[i].x[j] = p[i].x[j];
			}
			free(R);
		}
		fitnessPop(o, NPOP);
		select(p, o, NPOP);

		printBest(p, NPOP, g++);
	}
	printBest(p, NPOP, g++);

	freePop(p, NPOP);
	freePop(o, NPOP);
	finalize();
	return 0;
}