#include	<stdio.h>
#include	"ant.h"
#define	InitialFoods	((NMESH*NMESH)/20)
#define	EvapoRate	0.98
#define	DefusRate	0.05
#define	BIAS		0.01
#define	SBIAS		0.04
#define	HBIAS	((dd > dist(&ta))? .5 : 1.8)

Cell	world[NMESH][NMESH];
Ant	ant[NAnts];
int	EnablePheromone = 1;

memory_init() {
	int	i, j;
	for (i = 0; i < NMESH; i ++) for (j = 0; j < NMESH; j ++) {
		world[i][j].phrmn = 0.;
		world[i][j].Food = false;
	}
	for (i = 0; i < NAnts; i ++) {
		ant[i].x = NestX; ant[i].y = NestY;
		ant[i].d = lrand48() & 3;
		ant[i].haveFood = false;
	}
	for (i = 0; i < 5; i ++) for (j = 0; j < 5; j ++)
		world[i+10][j+10].Food = true;
	for (i = 0; i < 10; i ++) for (j = 0; j < 10; j ++)
		world[i+80][j+80].Food = true;
	for (i = 0; i < 10; i ++) for (j = 0; j < 10; j ++)
		world[i+10][j+90].Food = true;
	show_world();
}
static void phoromone() {
	int	i, j;
	double	rx, pwork[NMESH][NMESH];
	for (i = 0; i < NMESH; i ++) for (j = 0; j < NMESH; j ++)
		world[i][j].phrmn *= EvapoRate;
	memset(pwork,0,sizeof(pwork));
	for (i = 0; i < NMESH-1; i ++) for (j = 0; j < NMESH; j ++) {
		rx = (world[i][j].phrmn - world[i+1][j].phrmn) * DefusRate;
		pwork[i][j] -= rx; pwork[i+1][j] += rx;
	}
	for (i = 0; i < NMESH; i ++) for (j = 0; j < NMESH-1; j ++) {
		rx = (world[i][j].phrmn - world[i][j+1].phrmn) * DefusRate;
		pwork[i][j] -= rx; pwork[i][j+1] += rx;
	}
	for (i = 0; i < NMESH; i ++) for (j = 0; j < NMESH; j ++)
		world[i][j].phrmn += pwork[i][j];
}
static void go_ahead(a) Ant *a; {
	int	nx = a->x, ny = a->y;
	switch (a->d) {
		case 0: ny --; break;
		case 1: nx ++; break;
		case 2: ny ++; break;
		case 3: nx --; break;
	}
	if (nx >= 0 && nx < NMESH && ny >= 0 && ny < NMESH)
		{ a->x = nx; a->y = ny; }
}
static void back_home(a) Ant *a; {
	double	rx, drand48();
	int	d;
	world[a->y][a->x].phrmn = 1.;
	rx = abs(a->x - NestX);
	if (drand48() * (rx + abs(a->y - NestY)) < rx)
		d = (a->x < NestX)? 1 : 3;
	else	d = (a->y < NestY)? 2 : 0;
	if (((a->d - d + 4) & 3) == 2)
		a->d = (d + ((lrand48() & 1)? 1 : 3)) & 3;
	else a->d = d;
	go_ahead(a);
}
static int dist(a) Ant *a; {
	return abs(a->x - NestX) + abs(a->y - NestY);
}
static void seek_food(a) Ant *a; {
	int	dr = (a->d + 1) & 3, dl = (a->d + 3) & 3, dd = dist(a);
	double	p, rr, rl, rs, r, drand48();
	Ant	ta;
	ta.x = a->x; ta.y = a->y;
	ta.d = dr; go_ahead(&ta);
	rr = world[ta.y][ta.x].phrmn * HBIAS + BIAS;
	ta.x = a->x; ta.y = a->y;
	ta.d = dl; go_ahead(&ta);
	rl = world[ta.y][ta.x].phrmn * HBIAS + BIAS;
	ta.x = a->x; ta.y = a->y;
	ta.d = a->d; go_ahead(&ta);
	rs = world[ta.y][ta.x].phrmn * HBIAS + SBIAS;
	r = drand48() * (rr + rl + rs);
	if (r < rr) a->d = dr;
	else if (r < rr + rl) a->d = dl;
	go_ahead(a);
}
static void random_walk(a) Ant *a; {
	double	r, drand48();
	r = drand48() * (BIAS + BIAS + SBIAS);
	if (r < BIAS) a->d = (a->d + 1) & 3;
	else if (r < BIAS + BIAS) a->d = (a->d + 3) & 3;
	go_ahead(a);
}
one_step() {
	int	i;
	Ant	*a;
	phoromone();
	for (i = 0, a = ant; i < NAnts; i ++, a ++) {
		if (a->haveFood) {
			if (a->x == NestX && a->y == NestY)
				a->haveFood = false;
			else	back_home(a);
		} else if (world[a->y][a->x].Food) {
			a->haveFood = true;
			world[a->y][a->x].Food = false;
		} else if (EnablePheromone) seek_food(a);
		else random_walk(a);
	}
	show_world();
	return 0;
}