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

u16 distances[20*14];
u16 tilemap[40*28];

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