/*

libusearch - microlib for searching/graph traversal

This example file is placed in the public domain by the author:
2020 Tibor 'Igor2' Palinkas

Source code: svn://svn.repo.hu/libusearch/trunk
Contact the author: http://igor2.repo.hu/contact.html

*/

#include <stdio.h>
#include <ctype.h>
#include <assert.h>

#define USRCH_INLINE static inline
#include <libusearch/a_star_api.h>
#include <libusearch/a_star_impl.h>

int verbose = 1;

typedef struct {
	char type;
	char in_path;
	usrch_a_star_node_t *mark;
} tile_t;

tile_t *start, *target, *map;
int sx, sy, Tx, Ty;
long mapsize;

USRCH_INLINE int tile2xy(tile_t *tile, int *x, int *y)
{
	int offs;

	offs = tile - map;
	if ((offs < 0) || (offs >= mapsize))
		return -1;
	*y = offs / sx;
	*x = offs % sx;
	return 0;
}

static void load(FILE *f)
{
	long ms;
	tile_t *s;

	if (fscanf(f, "%d %d\n", &sx, &sy) != 2) {
		fprintf(stderr, "invalid first line; must be x y\n");
		exit(1);
	}

	mapsize = ms = sx*sy;
	s = map = malloc(ms * sizeof(tile_t));

	while(ms > 0) {
		int c = fgetc(f);

		if (isspace(c)) continue;

		switch(c) {
			case 'S': start = s; break;
			case 'T': target = s; break;
		}

		s->type = c;
		s->in_path = 0;
		s->mark = NULL;
		s++;
		ms--;
	}

	if (start == NULL) {
		fprintf(stderr, "map: missing S\n");
		exit(1);
	}
	if (target == NULL) {
		fprintf(stderr, "map: missing T\n");
		exit(1);
	}
	assert(tile2xy(target, &Tx, &Ty) == 0);
}

static void draw_mark(tile_t *tile)
{
	
	if ((tile->mark != NULL) && (tile->type != 'S') && (tile->type != 'T'))
		putchar('*');
	else
		putchar(tile->type);
}

static void draw_path(tile_t *tile)
{
	
	if ((tile->in_path) /*&& (tile->type != 'S') && (tile->type != 'T')*/)
		putchar('*');
	else
		putchar(tile->type);
}

static void draw(void (*ptile)(tile_t *tile))
{
	tile_t *t, *end = map+mapsize;
	int x;

	for(x = 0, t = map; t < end; t++,x++) {
		if (x == sx) {
			printf("\n");
			x = 0;
		}
		ptile(t);
	}
	printf("\n");
}

static long heur(usrch_a_star_t *ctx, void *node)
{
	int x, y;

	assert(tile2xy(node, &x, &y) == 0);

	/* manhattan distance */
	x -= Tx;
	y -= Ty;
	if (x < 0) x = -x;
	if (y < 0) y = -y;
	return x+y;
}

static long cost(usrch_a_star_t *ctx, void *from, void *to_)
{
	tile_t *to = to_;
	assert(to->type != '#');
	switch(to->type) {
		case 'T': return 1;
		case ' ': return 1;
		case '.': return 2;
		case '+': return 4;
		default:
			fprintf(stderr, "map: invalid tile type '%c'\n", to->type);
			exit(1);
	}
	return 0;
}

static void *neighbor_pre(usrch_a_star_t *ctx, void *curr)
{
	static int count;
	count = 0;
	return &count;
}

static void *neighbor(usrch_a_star_t *ctx, void *curr, void *nctx)
{
	int *count = nctx, x, y, ox, oy;
	tile_t *ntile, *tile = curr;

	assert(tile2xy(curr, &x, &y) == 0);
	ox = x; oy = y;

	retry:;
	switch(*count) {
		case 0: x--; ntile = tile-1; break;
		case 1: x++; ntile = tile+1; break;
		case 2: y--; ntile = tile-sx; break;
		case 3: y++; ntile = tile+sx; break;
		default:
			return NULL;
	}
	(*count)++;

	if ((x < 0) || (x >= sx) || (y < 0) || (y >= sx)) {
		x = ox; y = oy;
		goto retry;
	}

	if ((ntile->type == '#') || (ntile->type == 'S')) {
		x = ox; y = oy;
		goto retry;
	}

	if (verbose)
		printf("neighbor: %d;%d -> %d;%d (type=%c mark=%s)\n", ox, oy, x, y, ntile->type, ntile->mark == NULL ? "no" : "yes");

	return ntile;
}

static void set_mark(usrch_a_star_t *ctx, void *node, usrch_a_star_node_t *mark)
{
	tile_t *tile = node;
	tile->mark = mark;
}

static usrch_a_star_node_t *get_mark(usrch_a_star_t *ctx, void *node)
{
	tile_t *tile = node;
	return tile->mark;
}


int main(int argc, char *argv[])
{
	usrch_a_star_t a = {0};
	long totcost;

	load(stdin);

	a.heuristic = heur;
	a.cost = cost;
	a.neighbor_pre = neighbor_pre;
	a.neighbor = neighbor;
	a.set_mark = set_mark;
	a.get_mark = get_mark;

	usrch_a_star_search(&a, start, target);

	/* mark tiles along the final path */
	{
		usrch_a_star_node_t *it;
		tile_t *t;
		for(t = usrch_a_star_path_first(&a, &it); t != NULL; t = usrch_a_star_path_next(&a, &it)) {
			t->in_path = 1;
		}

		usrch_a_star_path_first(&a, &it);
		if (it != NULL)
			totcost = it->gscore;
		else
			totcost = -1;
	}

	printf("visited:\n");
	draw(draw_mark);

	printf("\npath (%ld):\n", totcost);
	draw(draw_path);

	usrch_a_star_uninit(&a);
	free(map);
	return 0;
}
