#include <genesis.h>
#include "creep.h"

creep creeps[MAX_CREEPS];
u16 cur_creeps;
extern u16 tilemap[40*28];

u16 distances[20*14];

const s16 speeds[NUM_SPECIES] = { 2 };

u16 spawn_creep(u8 species, s16 x, s16 y)
{
	u16 index;
	for (index = 0; index < MAX_SPRITE; index++)
	{
		if (spriteDefCache[index].posx == -0x80) {
			break;
		}
	}
	if (index > 0) {
		VDP_setSprite(index, x, y, SPRITE_SIZE(1, 1), TILE_ATTR_FULL(2, 0, 0, 0, 'C' + TILE_FONTINDEX), spriteDefCache[0].link);
		spriteDefCache[0].link = index;
	} else {
		VDP_setSprite(index, x, y, SPRITE_SIZE(1, 1), TILE_ATTR_FULL(2, 0, 0, 0, 'C' + TILE_FONTINDEX), 0);
	}
	creeps[cur_creeps].index = index;
	creeps[cur_creeps].health = 1000;
	creeps[cur_creeps].species = species;
	creeps[cur_creeps].direction = 0;
	creeps[cur_creeps].targetx = x;
	creeps[cur_creeps].targety = y;
	return cur_creeps++;
}

typedef struct {
	u16 index;
	u16 x;
	u16 y;
} mpoint;

s16 explore(mpoint * points, s16 num_points, u16 src, u16 srcx, u16 srcy)
{
	u16 dist = distances[src]+1;
	if (srcx < 19 && distances[src + 1] > dist && !tilemap[(srcx+1)*2+srcy*2*40])
	{
		distances[src + 1] = dist;
		points[num_points].index = src + 1;
		points[num_points].x = srcx+1;
		points[num_points++].y = srcy;
	}
	if (srcx && distances[src - 1] > dist && !tilemap[(srcx-1)*2 + srcy*2*40])
	{
		distances[src - 1] = dist;
		points[num_points].index = src - 1;
		points[num_points].x = srcx-1;
		points[num_points++].y = srcy;
	}
	if (srcy < 13 && distances[src + 20] > dist && !tilemap[srcx*2+(srcy+1)*2*40])
	{
		distances[src + 20] = dist;
		points[num_points].index = src + 20;
		points[num_points].x = srcx;
		points[num_points++].y = srcy+1;
	}
	if (srcy && distances[src - 20] > dist && !tilemap[srcx*2 + (srcy-1)*2*40])
	{
		distances[src - 20] = dist;
		points[num_points].index = src - 20;
		points[num_points].x = srcx;
		points[num_points++].y = srcy-1;
	}
	return num_points;
}

void gen_distances(u16 x, u16 y)
{
	//TODO: Figure out the actual maximum number of candidate points that can exist
	mpoint pointsa[20*14];
	mpoint pointsb[20*14];
	mpoint *points=pointsa;
	mpoint *new_points;
	s16 num_points = 0, old_points;
	x /= 2;
	y /= 2;
	memset(distances, 0xFF, sizeof(distances));

	distances[x + y*20] = 0;
	num_points = explore(points, num_points, x + y*20, x, y);

	while (num_points)
	{
		new_points = points == pointsa ? pointsb : pointsa;
		old_points = num_points;
		for (num_points = 0, old_points--; old_points >= 0; old_points--)
		{
			num_points = explore(new_points, num_points, points[old_points].index, points[old_points].x, points[old_points].y);
		}
		points = new_points;
	}
}

void print_distances(void)
{
	u16 x,y,tindex,dindex,dist;
	for (y = 0; y < 14; y++)
	{
		dindex = y * 20;
		tindex = y * 2 * 40;
		for (x = 0; x < 20; x++, dindex++, tindex += 2)
		{
			dist = distances[dindex];
			if (dist < 10000)
			{
				tilemap[tindex+41] = TILE_ATTR_FULL(3, 0, 0, 0, '0' - 32 + dist % 10 + TILE_FONTINDEX);
				dist /= 10;
				if (dist)
				{
					tilemap[tindex+40] = TILE_ATTR_FULL(3, 0, 0, 0, '0' - 32 + dist % 10 + TILE_FONTINDEX);
					dist /= 10;
					if (dist)
					{
						tilemap[tindex+1] = TILE_ATTR_FULL(3, 0, 0, 0, '0' - 32 + dist % 10 + TILE_FONTINDEX);
						dist /= 10;
						if (dist)
							tilemap[tindex] = TILE_ATTR_FULL(3, 0, 0, 0, '0' - 32 + dist % 10 + TILE_FONTINDEX);
					}
				}
			}
		}
	}
}

void update_creeps(void)
{
	s16 i, disty, distx, mdist;
	for (i = cur_creeps-1; i >= 0; --i)
	{
		creep *cur = creeps + i;
		SpriteDef * sprite = spriteDefCache + cur->index;
		distx = sprite->posx - cur->targetx;
		if (distx < 0)
			distx = -distx;
		disty = sprite->posy - cur->targety;
		if (disty < 0)
			disty = -disty;
		mdist = distx > disty ? distx : disty;
		if (mdist < speeds[cur->species])
		{
			s16 dindex = sprite->posx / 16 + (sprite->posy / 16) * 20;
			if (distances[dindex])
			{
				u16 bestdist = 0xFFFF;
				s16 bestindex = 0xFFFF,diff,next;
				if (sprite->posx > 8 && distances[dindex-1] < bestdist)
				{
					bestindex = dindex-1;
					bestdist = distances[bestindex];
				}
				if (sprite->posx < (320-16) && distances[dindex+1] < bestdist)
				{
					bestindex = dindex+1;
					bestdist = distances[bestindex];
				}
				if (sprite->posy > 8 && distances[dindex-20] < bestdist)
				{
					bestindex = dindex-20;
					bestdist = distances[bestindex];
				}
				if (sprite->posy < (224-16) && distances[dindex+20] < bestdist)
				{
					bestindex = dindex+20;
					bestdist = distances[bestindex];
				}
				diff = bestindex-dindex;
				next = bestindex + diff;
				while (next >= 0 && next < 20*14 && distances[next] < bestdist)
				{
					bestindex = next;
					bestdist = distances[bestindex];
				}
				cur->targetx = (bestindex % 20) * 16 + 4;
				cur->targety = (bestindex / 20) * 16 + 4;
			} else {
				//creep is at the goal
				VDP_setSpritePosition(cur->index, -0x80, -0x80);
				cur_creeps--;
				if (i != cur_creeps)
				{
					memcpy(cur, creeps+cur_creeps, sizeof(creep));
				}
			}
		} else {
			if (distx > disty) {
				VDP_setSpritePosition(cur->index,
					sprite->posx + (sprite->posx > cur->targetx ? -speeds[cur->species] : speeds[cur->species]),
					sprite->posy);
			} else {
				VDP_setSpritePosition(cur->index,
					sprite->posx,
					sprite->posy + (sprite->posy > cur->targety ? -speeds[cur->species] : speeds[cur->species]));
			}
		}
	}
}