#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>

extern	float sqrtf(float x);

#define	MAXPLACE	64
#define	MAXPOP		(64*1024)

#define	XOF(PLACE)	((int) ((PLACE) & 0xffff))
#define	YOF(PLACE)	((int) (((PLACE) >> 16) & 0xffff))

#define	forplaces(I)	for (I=0; I<places; ++I)
#define	SWAP(X,Y)	do { X^=Y; Y^=X; X^=Y; } while(0)

typedef	unsigned int	place_t;

place_t	p[MAXPOP][MAXPLACE];
float	m[MAXPOP];

int	places;

#ifdef	VERBOSE
	char *by = "Random";
#endif

void
mutate(int a, int b)
{
	/* a = mutate(b) */
	int i;
	int j = (rand() % (places-1)) + 1;

	/* Copy b rotated */
	forplaces(i) {
		p[a][i] = p[b][(i + j) % places];
	}

	/* Potentially do some swaps */
	for (i=(rand() % places); i>0; --i) {
		/* Swap a couple of elements */
		int k = (rand() % places);
		j = (k + 1 + (rand() % (places-1))) % places;
		SWAP(p[a][j], p[a][k]);
	}

#ifdef	VERBOSE
	by = "Mutate";
#endif
}

void
cross(int a, int b, int c)
{
	/* a = cross(b, c) */
	int i, k;
	int j = (rand() % places);
	int bj = p[b][j];
	int cj = p[c][j];

	if (bj != cj) {
		/* Find substitute */
		for (k=0; p[b][k]!=cj; ++k);

		/* Copy b */
		forplaces(i) {
			p[a][i] = p[b][i];
		}

		/* Make substitution */
		p[a][j] = cj;
		p[a][k] = bj;

#ifdef	VERBOSE
		by = "Cross";
#endif
	} else {
		/* Same? Mutate! */
		mutate(a, b);
	}
}

float
distance(place_t a, place_t b)
{
	/* Return distance between a and b */
	float dx = XOF(a) - XOF(b);
	float dy = YOF(a) - YOF(b);
	return(sqrtf((dx * dx) + (dy * dy)));
}

float
metric(int a)
{
	/* Return total distance of path a */
	int i;
	place_t x = p[a][0];
	float dist = 0;

	for (i=1; i<places; ++i) {
		place_t y = p[a][i];
		dist += distance(x, y);
		x = y;
	}

	return(dist);
}

void
show(int a)
{
	/* Print path a */
	int i;

	printf("{(%d,%d)", XOF(p[a][0]), YOF(p[a][0]));
	for (i=1; i<places; ++i) {
		printf(", (%d,%d)", XOF(p[a][i]), YOF(p[a][i]));
	}
	printf("}");
}

int
main(int argc, char **argv)
{
	int i, abest = -1;
	float m[MAXPOP], mbest;
	int pop, cro, mut, gens, gen = 0;
	place_t ref[MAXPLACE];
	int dumtest = 0;

	if (argc == 6) {
		pop = atoi(argv[1]);
		cro = atoi(argv[2]);
		mut = atoi(argv[3]);
		gens = atoi(argv[4]);
		places = atoi(argv[5]);
		if ((pop < 3) || (pop > MAXPOP)) {
			fprintf(stderr,
				"%s: population must be between 3 and %d\n",
				argv[0],
				MAXPOP);
			exit(2);
		}
		if (places < 0) {
			dumtest = 1;
			places = -places;
		}
		if ((places < 2) || (places > MAXPLACE)) {
			fprintf(stderr,
				"%s: places must be between 2 and %d\n",
				argv[0],
				MAXPLACE);
			exit(2);
		}
	} else {
		fprintf(stderr,
			"Usage: %s [pop] [cro] [mut] [gens] [places]\n"
			"-places forces an obvious test data set\n",
			argv[0]);
		exit(1);
	}

	/* Make a reference problem */
	srand(time(0));
	if (dumtest) {
		forplaces(i) {
			ref[i] = (i * 0x10001);
		}
	} else {
		forplaces(i) {
			ref[i] = rand();
		}
	}

	/* The genetic algorithm loop... */
	do {
		register int a;

		/* Make a new guy... */
		if (gen < pop) {
			/* Still creating initial population...
			   start with shifted reference and
			   do random swaps
			*/
			a = gen;
			forplaces(i) {
				p[a][i] = ref[(i + a) % places];
			}
			forplaces(i) {
				int j = (i + 1 + (rand() % (places-1))) % places;
				SWAP(p[a][i], p[a][j]);
			}
		} else {
			/* Pick a pair of random pop members */
			register int b;

			a = (rand() % pop);
			do {
				b = (rand() % pop);
			} while (a == b);

			/* Make a be the worst one */
			if ((m[a] < m[b]) || (a == abest)) {
				SWAP(a, b);
			}

			/* Crossover or mutation? */
			if ((rand() % (cro + mut)) < cro) {
				/* Crossover needs a third */
				register int c = b;

				do {
					b = (rand() % pop);
				} while ((a == b) || (c == b));

				/* Make a be the worst one */
				if ((m[a] < m[b]) || (a == abest)) {
					SWAP(a, b);
				}

				/* Crossover */
				cross(a, b, c);
			} else {
				/* Mutation */
				mutate(a, b);
			}
		}

		/* Evaluate the metric & update best */
		m[a] = metric(a);
		if ((gen == 0) || (m[a] < mbest)) {
			mbest = m[a];
			abest = a;

			/* Print the stats */
#ifdef	VERBOSE
			printf("Gen %d, by %s, %f:\n",
			       gen,
			       by,
			       mbest);
			show(abest);
			printf("\n");
#endif
		}
	} while (++gen < gens);

	if (dumtest) {
		printf("Theoretical best is %f. ",
		       ((places-1) * sqrtf(2)));
	}
	printf("Best is %f:\n",
	       m[abest]);
	show(abest);
	printf("\n");
	exit(0);
}