#include	<stdio.h>
#include	<math.h>
#include	"gene.h"

#define	AllocUnit	128
#define	MaxGrid		128

cell	**WorldMap, *ActiveCells = NULL, *FreeCell = NULL;
individual	*Individuals = NULL, *FreeInd = NULL;
int	WorldGrid, WorldMapSize;
unsigned long	NumberOfCells;

extern	double	CellSize;
extern	unsigned long	PopSize;

mem_init()
{
	int	mem_size;
	WorldGrid = floor(1.0 / (CellSize * 6.1));
	if (WorldGrid > MaxGrid) WorldGrid = MaxGrid;
	else if (WorldGrid < 1) WorldGrid = 1;
	WorldMapSize = WorldGrid * WorldGrid;
	mem_size = sizeof(cell *) * WorldMapSize;
	if (!(WorldMap = (cell **)malloc(mem_size))) {
		perror("malloc");
		exit(1);
	}
	bzero(WorldMap, mem_size);
}

mem_reset()
{
	int	i;
	cell	*p;
	individual	*q;
	for (i = 0; i < WorldMapSize; i ++) if (WorldMap[i]) {
		for (p = WorldMap[i]; p->wmap; p = p->wmap);
		p->wmap = FreeCell;
		FreeCell = WorldMap[i];
		WorldMap[i] = NULL;
	}
	if (FreeInd) {
		for (q = FreeInd; q->next; q = q->next);
		q->next = Individuals;
	} else	FreeInd = Individuals;
	Individuals = NULL;
	ActiveCells = NULL;
	NumberOfCells = PopSize = 0;
}

int	WIndexX, WIndexY;
double	UpperX, LowerX, UpperY, LowerY;

world_index(x,y)
double	x,y;
{
	int	ix, iy;
	ix = floor(x * WorldGrid); if (ix >= WorldGrid) ix = WorldGrid - 1;
	iy = floor(y * WorldGrid); if (iy >= WorldGrid) iy = WorldGrid - 1;
	LowerX = ((double)ix) / WorldGrid;
	UpperX = ((double)(ix + 1)) / WorldGrid;
	LowerY = ((double)iy) / WorldGrid;
	UpperY = ((double)(iy + 1)) / WorldGrid;
	WIndexX = ix; WIndexY = iy;
	return (iy * WorldGrid + ix);
}

cell	*alloc_cell(x,y)
double	x,y;
{
	int	i;
	cell	*c = FreeCell, **w = WorldMap + world_index(x,y);
	if (!FreeCell) {
		if (!(c = FreeCell = (cell *)malloc(sizeof(cell)*AllocUnit))) {
			perror("cell memory");
			exit(1);
		}
		for (i = 0; i < AllocUnit-1; i ++)
			FreeCell[i].wmap = FreeCell + i + 1;
		FreeCell[AllocUnit-1].wmap = NULL;
	}
	FreeCell = FreeCell->wmap;
	if (ActiveCells) ActiveCells->prev = c;
	c->next = ActiveCells;
	c->prev = NULL;
	c->wmap = *w;
	*w = ActiveCells = c;
	NumberOfCells ++;
	return c;
}

fix_cell(c)
cell	*c;
{
	if (c->next) c->next->prev = c->prev;
	if (c->prev) c->prev->next = c->next;
	else if (c == ActiveCells) ActiveCells = c->next;
	c->prev = c->next = NULL;
}
activate_cell(c)
cell	*c;
{
	if (c->next == NULL && c->prev == NULL) {
		c->next = ActiveCells;
		if (ActiveCells) ActiveCells->prev = c;
		ActiveCells = c;
	}
}
kill_cell(c)
cell	*c;
{
	cell	**w = WorldMap + world_index(c->x,c->y);
	fix_cell(c);
	while (*w && *w != c) w = &((*w)->wmap);
	if (*w) {
		erase_cell(c);
		*w = c->wmap;
		c->wmap = FreeCell;
		FreeCell = c;
		NumberOfCells --;
	} else	fprintf(stderr,"Error in kill_cell.\n");
}

for_all_active_cells(proc)
void	(*proc)();
{
	cell	*c, *b;
	for (c = ActiveCells; c; c = b) {
		b = c->next;
		(*proc)(c);
	}
}

for_all_cells(proc)
void	(*proc)();
{
	int	i;
	cell	*c, *d;
	for (i = 0; i < WorldMapSize; i ++)
		for (c = WorldMap[i]; c; c = d) {
			d = c->wmap;
			(*proc)(c);
		}
}

double	difference(a,b)
double	a, b;
{
	double	c = a - b;
	if (c >= 0.5) c -= 1.0;
	else if (c < -0.5) c += 1.0;
	return c;
}

int	NeighborsMap[9], NeighborsMapN;

get_neighbors_map(x,y,r)
double	x,y,r;
{
	int	flag = 0;
	NeighborsMap[0] = world_index(x,y);
	NeighborsMapN = 1;
	if (x + r >= UpperX) {
		NeighborsMap[NeighborsMapN++] =
		WIndexY * WorldGrid + (WIndexX + 1) % WorldGrid;
		flag = 1;
	} else if (x - r < LowerX) {
		NeighborsMap[NeighborsMapN++] =
		WIndexY * WorldGrid + (WIndexX + WorldGrid - 1) % WorldGrid;
		flag = 2;
	}
	if (y + r >= UpperY) {
		NeighborsMap[NeighborsMapN++] =
		((WIndexY + 1) % WorldGrid) * WorldGrid + WIndexX;
		flag += 4;
	} else if (y - r < LowerY) {
		NeighborsMap[NeighborsMapN++] =
		((WIndexY + WorldGrid - 1) % WorldGrid) * WorldGrid + WIndexX;
		flag += 8;
	}
	switch (flag) {
		case 5: /* upper and upper */
		NeighborsMap[NeighborsMapN++] =
		((WIndexY + 1) % WorldGrid) * WorldGrid
		+ (WIndexX + 1) % WorldGrid;
		break;
		case 6: /* lower and upper */
		NeighborsMap[NeighborsMapN++] =
		((WIndexY + 1) % WorldGrid) * WorldGrid
		+ (WIndexX + WorldGrid - 1) % WorldGrid;
		break;
		case 9: /* upper and lower */
		NeighborsMap[NeighborsMapN++] =
		((WIndexY + WorldGrid - 1) % WorldGrid) * WorldGrid
		+ (WIndexX + 1) % WorldGrid;
		break;
		case 10: /* lower and lower */
		NeighborsMap[NeighborsMapN++] =
		((WIndexY + WorldGrid - 1) % WorldGrid) * WorldGrid
		+ (WIndexX + WorldGrid - 1) % WorldGrid;
	}
	return 0;
}

cell	*conflict_cell(x,y)
double	x,y;
{
	int	i, j;
	cell	*c;
	get_neighbors_map(x,y,CellSize*2.0);
	for (i = 0; i < NeighborsMapN; i ++)
		for (c = WorldMap[NeighborsMap[i]]; c; c = c->wmap)
			if (hypot(difference(x,c->x),difference(y,c->y))
			 < CellSize * 1.99) return c;
	return NULL;
}

get_sense(x,y,d)
double	x,y;
int	d;
{
	int	i;
	cell	*c;
	double	cx, cy, dist, th, r = CellSize * 3;
	int	sense = 0;
	get_neighbors_map(x,y,r);
	for (i = 0; i < NeighborsMapN; i ++) {
		for (c = WorldMap[NeighborsMap[i]]; c; c = c->wmap) {
			cx = difference(c->x,x);
			cy = difference(c->y,y);
			dist = hypot(cx, cy);
			if (dist > CellSize && dist < r) {
				th = atan2(cy, cx)/M_PI - c->d/128.0;
				while (th < -1.0) th += 2.0;
				if (-0.25 < th && th < 0.25) sense |= 2;
				else if (th < -0.75 || 0.75 < th) sense |= 8;
				else if (th < 0) sense |= 4;
				else sense |= 1;
			}
		}
	}
	return sense;
}

cell	*nearest_neighbor(x,y)
double	x,y;
{
	int	i;
	cell	*c, *nn = NULL;
	double	d, nd = 1e10;
	get_neighbors_map(x,y,CellSize*2.0);
	for (i = 0; i < NeighborsMapN; i ++) {
		for (c = WorldMap[NeighborsMap[i]]; c; c = c->wmap) {
			d = hypot(difference(x,c->x),difference(y,c->y));
			if (d < nd) { nd = d; nn = c; }
		}
	}
	return nn;
}

individual	*alloc_ind(root_id)
int	root_id;
{
	int	i;
	individual	*c;
	if (!(c = FreeInd)) {
		if (!(c = FreeInd =
			(individual *)malloc(sizeof(individual)*AllocUnit))) {
			perror("individual memory");
			exit(1);
		}
		for (i = 0; i < AllocUnit-1; i ++)
			FreeInd[i].next = FreeInd + i + 1;
		FreeInd[AllocUnit-1].next = NULL;
	}
	FreeInd = FreeInd->next;
	if (Individuals) Individuals->prev = c;
	c->next = Individuals;
	c->prev = NULL;
	Individuals = c;
	c->size = c->growing = 0;
	c->root_id = root_id;
	c->id = PopSize ++;
	return c;
}