/*

libusearch - microlib for searching/graph traversal

Copyright (c) 2020 Tibor 'Igor2' Palinkas
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions
are met:
1. Redistributions of source code must retain the above copyright
   notice, this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright
   notice, this list of conditions and the following disclaimer in the
   documentation and/or other materials provided with the distribution.
3. Neither the name of the Author nor the names of contributors
   may be used to endorse or promote products derived from this software
   without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
SUCH DAMAGE.

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

*/

#ifndef LIBUSEARCH_A_STAR_IMPL_H
#define LIBUSEARCH_A_STAR_IMPL_H

#include <libusearch/a_star_api.h>

/* internal representation of a node */
struct usrch_a_star_node_s {
	void *user_node;
	long gscore, fscore, hval;
	usrch_a_star_node_t *from;
	fbhn_t fbhn;
	usrch_a_star_node_t *all_next;
};

USRCH_INLINE usrch_a_star_node_t *usrch_a_star_open_node(usrch_a_star_t *ctx, void *user_node, long gscore, long fscore, long hval, usrch_a_star_node_t *from)
{
	usrch_a_star_node_t *node = usrch_malloc(ctx, sizeof(usrch_a_star_node_t));

	node->user_node = user_node;
	node->gscore = gscore;
	node->fscore = fscore;
	node->hval = hval;
	node->from = from;
	node->all_next = ctx->all;
	ctx->all = node;
	ctx->set_mark(ctx, user_node, node);
	fbh_insert(&ctx->open, node, fscore);

	return node;
}

USRCH_INLINE usrch_res_t usrch_a_star_start(usrch_a_star_t *ctx, void *start_node)
{
	long hval;
	if (fbh_init(&ctx->open, offsetof(usrch_a_star_node_t, fbhn)) != 0)
		return USRCH_RES_ERROR;

	hval = ctx->heuristic(ctx, start_node);
	usrch_a_star_open_node(ctx, start_node, 0, hval, hval, NULL);

	return USRCH_RES_SUCCESS;
}

USRCH_INLINE usrch_res_t usrch_a_star_start_arr(usrch_a_star_t *ctx, void **start_nodes, int len)
{
	if (fbh_init(&ctx->open, offsetof(usrch_a_star_node_t, fbhn)) != 0)
		return USRCH_RES_ERROR;

	while(len > 0) {
		long hval = ctx->heuristic(ctx, *start_nodes);
		usrch_a_star_open_node(ctx, *start_nodes, 0, hval, hval, NULL);
		len--;
		start_nodes++;
	}
	return USRCH_RES_SUCCESS;
}


USRCH_INLINE usrch_res_t usrch_a_star_iter(usrch_a_star_t *ctx)
{
	usrch_a_star_node_t *neigh, *curr = fbh_pop_min(&ctx->open);
	void *user_neigh, *nctx;
	int processed = 0, finished;

	if (curr == NULL)
		return USRCH_RES_ERROR;

	if (ctx->is_target == NULL)
		finished = (curr->user_node == ctx->target);
	else
		finished = ctx->is_target(ctx, curr->user_node);

	if (finished) {
		ctx->last = curr;
		return USRCH_RES_FOUND;
	}

	if (ctx->neighbor_pre != NULL)
		nctx = ctx->neighbor_pre(ctx, curr->user_node);
	else
		nctx = NULL;

	while((user_neigh = ctx->neighbor(ctx, curr->user_node, nctx)) != NULL) {
		long gscore_neigh, tentative_gscore = curr->gscore + ctx->cost(ctx, curr->user_node, user_neigh);;

		processed++;
		neigh = ctx->get_mark(ctx, user_neigh);
		if (neigh != NULL)
			gscore_neigh = neigh->gscore;
		else
			gscore_neigh = LONG_MAX;

		/* first/better path to neigh */
		if (tentative_gscore < gscore_neigh) {
			if (neigh != NULL) {
				neigh->gscore = tentative_gscore;
				neigh->fscore = tentative_gscore + neigh->hval;
				neigh->from = curr;
			}
			else {
				long hval = ctx->heuristic(ctx, user_neigh);
				neigh = usrch_a_star_open_node(ctx, user_neigh, tentative_gscore, tentative_gscore + hval, hval, curr);
			}
		}
	}

	if (ctx->neighbor_post != NULL)
		ctx->neighbor_post(ctx, curr->user_node, nctx);

	if ((processed == 0) && (ctx->open.num_nodes == 0))
		return USRCH_RES_ERROR; /* didn't find anything here and there's no other open node to check */

	return USRCH_RES_CONTINUE;
}

USRCH_INLINE usrch_res_t usrch_a_star_search(usrch_a_star_t *ctx, void *start_node, void *target_node)
{
	usrch_res_t res;
	usrch_a_star_start(ctx, start_node);
	ctx->target = target_node;

	while((res = usrch_a_star_iter(ctx)) == USRCH_RES_CONTINUE) ;

	return res;
}

USRCH_INLINE void *usrch_a_star_path_next(usrch_a_star_t *ctx, usrch_a_star_node_t **it)
{
	void *res;

	if (*it == NULL)
		return NULL;

	res = (*it)->user_node;
	*it = (*it)->from;
	return res;
}

USRCH_INLINE void *usrch_a_star_path_first(usrch_a_star_t *ctx, usrch_a_star_node_t **it)
{
	*it = ctx->last;
	return usrch_a_star_path_next(ctx, it);
}

USRCH_INLINE void *usrch_a_star_path_first_from(usrch_a_star_t *ctx, void *user_node, usrch_a_star_node_t **it)
{
	*it = ctx->get_mark(ctx, user_node);
	return usrch_a_star_path_next(ctx, it);
}

USRCH_INLINE void usrch_a_star_uninit(usrch_a_star_t *ctx)
{
	usrch_a_star_node_t *node, *next;

	for(node = ctx->all; node != NULL; node = next) {
		next = node->all_next;
		ctx->set_mark(ctx, node->user_node, NULL);
		free(node);
	}
}

#endif
