#include "dijkstrasearch.h"
#include "logging.h"
#include <stdio.h>

void dijkstra_search_reset(struct dijkstra_search *search)
{
	unsigned int i;
	for (i=0; i < search->node_count; i++) {
		search->states[i]->cost = DIJKSTRA_COST_MAX;
		search->states[i]->visited = 0;
	}

	if (search->start) {
		search->states[search->start->uid]->cost = 0;
	}

	if (search->active_nodes) {
		g_list_free(search->active_nodes);
		search->active_nodes = NULL;
	}
}

// WARNING: After creating this object, you must NOT modify the solver!
struct dijkstra_search *dijkstra_search_new(struct dijkstra_solver *solver)
{
	struct dijkstra_search *search;
	unsigned int i;

	search = malloc(sizeof(*search));
	if (search) {
		search->states = malloc(sizeof(*search->states) * g_list_length(solver->nodes));
		search->node_count = g_list_length(solver->nodes);
		search->start = NULL;
		for (i=0; i < search->node_count; i++) {
			search->states[i] = malloc(sizeof(struct dijkstra_state));
			if (!search->states) {
				// TODO: I should dealloc all allocated memory before returning,
				// but whatever, we'll crash and burn one way of the other.
				return NULL;
			}
		}
		search->active_nodes = NULL;
		dijkstra_search_reset(search);
	}

	return search;
}

void dijkstra_search_free(struct dijkstra_search *search)
{
	unsigned int i;

	for (i=0; i < search->node_count; i++) {
		free(search->states[i]);
	}
	free(search->states);
	free(search);
}

void dijkstra_search_set_start(struct dijkstra_search *search,
			       struct dijkstra_node *start)
{
	if (search->start != start) {
		search->start = start;
		dijkstra_search_reset(search);
	}
}

struct dijkstra_node *dijkstra_search_process_queue(struct dijkstra_search *search)
{
	struct dijkstra_path *path;
	struct dijkstra_state *state;
	struct dijkstra_node *connection;
	struct dijkstra_node *cheapest_node = NULL;
	struct dijkstra_node *node;

	dijkstra_cost cost;
	GList *l;

	// 1. Find cheapest node in active nodes
	// TODO: We can make this significantly quicker by using a sorted list
	for (l = search->active_nodes; l != NULL; l = l->next) {
		node = (struct dijkstra_node*) l->data;
		state = search->states[node->uid];
		if (!cheapest_node ||
				state->cost < search->states[cheapest_node->uid]->cost) {
			cheapest_node = node;
		}
		if (state->visited) {
			report(LL_WARNING,
			       "Visited node in active list (%d). This is probably an error.",
			       node->uid); // TODO: This can be removed after I'm sure the algorithm works correctly
		}
	}
	node = cheapest_node;
	state = search->states[node->uid];
	report(LL_DEBUG, "Current cheapest node is %d", node->uid);

	// 2. Mark the current node as visited and remove it from the actives
	state->visited = 1;
	search->active_nodes = g_list_remove(search->active_nodes, node);

	// 3. Update connecting nodes
	for (l = node->paths; l != NULL; l = l->next) {
		path = (struct dijkstra_path*) l->data;
		cost = state->cost + path->weight;

		connection = dijkstra_node_get_connection(node, path);
		// 3.1 Update cost if cheaper
		if (cost < search->states[connection->uid]->cost) {
			search->states[connection->uid]->cost = cost;
			search->states[connection->uid]->cheapest_path = path;
			report(LL_DEBUG, "  Set cost of node %d to %d (backpath from %d to %d)", connection->uid, cost, connection->uid, node->uid);
			if (search->states[connection->uid]->visited) {
				report(LL_WARNING, "  Reset cost of visited node!"); // TODO: not necessary if algorithm OK
			}
		}
		// 3.2. Add connecting nodes to active nodes if not already visited
		if (!search->states[connection->uid]->visited) {
			if (!g_list_find(search->active_nodes, connection)) {
				search->active_nodes = g_list_append(search->active_nodes,
								     (gpointer) connection);
				report(LL_DEBUG, "  Append %d as an active node.", connection->uid);
			}
		}
	}

	return node;
}

int dijkstra_search_find_path(struct dijkstra_search *search,
			      struct dijkstra_node *destination)
{
	int iteration = 0;
	search->active_nodes = g_list_append(search->active_nodes, search->start);

	while (g_list_length(search->active_nodes) > 0) {
		iteration++;
		dijkstra_search_process_queue(search);
		if (search->states[destination->uid]->visited)
			return iteration;
		if (iteration >= DIJKSTRA_SEARCH_MAX_ITERATIONS) {
			report(LL_ERROR, "Exceeded maximum iteration limit for query.");
			return -2;
		}
	}

	return -1;
}

int dijkstra_print_path(struct dijkstra_search *search,
			struct dijkstra_node *destination)
{
	struct dijkstra_node *node = NULL;
	GList *l;
	int i=500; // TODO: This should not be necessary. Plus the number is arbitrary.

	if (!destination || !search->states[destination->uid]->visited)
		return 0;

	node = destination;
	while (node != search->start) {
		if (node != destination)
			printf(",");
		printf("[%d,%d]", (int) node->position.x, (int) node->position.y);

	//	printf("%u [%d]-> ", node->uid, search->states[node->uid]->cost);
		node = dijkstra_node_get_connection(node, search->states[node->uid]->cheapest_path);
		if (!i--) {
			report(LL_CRITICAL, "Iteration limit for backsolver. This should never happen.");
			break;
		}
	}
	printf("\n");
}