/*
 * Simple Implementierung des Dijkstra-Algorithmus in lite-C. Verwendet ein
 * regelmäßiges Gitter als Graphen. Ohne Optimierungen.
 *
 * Diesen Code in eine .c-Datei kopieren und in SED ausführen.
 *
 * Getestet in A8.03.2
 */

#include <acknex.h>
#include <default.c>

// Eigenschaften des Graphen
#define NUMBER_OF_COLS 10
#define NUMBER_OF_ROWS 10
#define NUMBER_OF_NODES 100

// Die Kosten, um von einem Knoten zum Nachbarknoten zu gelangen.
// Der Wert an sich ist hier ohne große Bedeutung.
#define EDGE_COST 10.0

// Kennzeichnet ungültige bzw. nicht vorhandene Knoten.
#define NULLNODE -1

// row-major order Berechnung
#define NODE_INDEX(col, row) ((row) * NUMBER_OF_COLS + (col))

// Nummer des Vorgängerknotens für jeden Knoten
int nodeParent[NUMBER_OF_NODES];

// Pfadkosten vom Startknoten bis zu jedem Knoten
var nodePathCost[NUMBER_OF_NODES];

// speichert, ob sich der jeweilige Knoten in der Open List befindet
BOOL inOpenList[NUMBER_OF_NODES];

// speichert die Passierbarkeit eines Knotens: 0 = passierbar, 1 = unpassierbar
BOOL nodeBlocked[NUMBER_OF_NODES]  = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
				       0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
				       0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
				       0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
				       0, 0, 1, 0, 0, 1, 1, 0, 1, 1,
				       0, 0, 1, 1, 1, 1, 0, 0, 1, 0,
				       0, 0, 0, 0, 0, 0, 0, 1, 1, 0,
				       0, 0, 0, 1, 0, 0, 0, 0, 0, 0,
				       0, 0, 0, 1, 1, 0, 0, 0, 0, 0,
				       0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };

// speichert den Pfad vom Zielknoten bis zum Startknoten
int path[NUMBER_OF_NODES];

// Prototypen
BOOL isOpenListEmpty();
int getBestNodeFromOpenList();
void reconstructPath(int node);
void expandNode(int parent);

// Die Hauptfunktion des Suchalgorithmus. Liefert TRUE zurück, wenn ein Pfad
// zwischen Start- und Zielknoren gefunden wird, ansonsten FALSE.
// Der gefundene Pfad wird in umgekehrter Reihenfolge im globalen Array path[]
// gespeichert.
BOOL dijkstra(int start_node, int dest_node)
{
	int node;
	
	if ((nodeBlocked[start_node] == TRUE) ||
		(nodeBlocked[dest_node] == TRUE))
	{
		return FALSE;
	}
	
	// Knoten initialisieren
	for (node = 0; node < NUMBER_OF_NODES; node++)
	{
		nodeParent[node] = NULLNODE;
		nodePathCost[node] = VAR_MAX;
		inOpenList[node] = TRUE;
	}
	
	nodePathCost[start_node] = 0;
	
	do
	{
		node = getBestNodeFromOpenList();
		inOpenList[node] = FALSE;
		
		if (node == dest_node) // Pfad gefunden!
		{
			reconstructPath(node);
			return TRUE;
		}
		
		expandNode(node);
	}
	while (isOpenListEmpty() == FALSE);
	
	return FALSE;
}

// Liefert TRUE zurück, wenn die Open List keine Knoten mehr enthält,
// ansonsten FALSE.
BOOL isOpenListEmpty()
{
	int node = 0;
	
	for (node = 0; node < NUMBER_OF_NODES; node++)
	{
		if (inOpenList[node] == TRUE)
		{
			return FALSE;
		}
	}
	
	return TRUE;
}

// Liefert den Knoten mit den kleinsten Wegkosten aus den Open List zurück.
int getBestNodeFromOpenList()
{
	var smallest_path_cost = VAR_MAX;
	int best_node = NULLNODE;
	int node;
	
	for (node = 0; node < NUMBER_OF_NODES; node++)
	{
		if ((inOpenList[node] == TRUE) &&
			(nodePathCost[node] < smallest_path_cost))
		{
			best_node = node;
			smallest_path_cost = nodePathCost[node];
		}
	}
	
	return best_node;
}

// Rekonstruiert den Pfad vom Zielknoten (node) bis zum Startknoten.
// Das Ergebnis wird im globalen Array path[] gespeichert.
void reconstructPath(int node)
{
	int path_length = 0;
		
	for (; node >= 0; path_length++)
	{
		path[path_length] = node;
		node = nodeParent[node];
	}
	
	if (path_length < (NUMBER_OF_NODES - 1))
	{
		// Markiere das Ende des Pfads
		path[path_length] = NULLNODE;
	}
}

// Aktualisiert die Wegkosten der Nachbarknoten von parent.
void expandNode(int parent)
{
	int parent_col = parent % NUMBER_OF_COLS;
	int parent_row = parent / NUMBER_OF_COLS;
	int node_col, node_row;
	
	for (node_row = parent_row - 1; node_row <= parent_row + 1; node_row++)
	{
		for (node_col = parent_col - 1; node_col <= parent_col + 1; node_col++)
		{
			int node = NODE_INDEX(node_col, node_row);
		
			if ((node_col < 0) || (node_col >= NUMBER_OF_COLS) ||
				(node_row < 0) || (node_row >= NUMBER_OF_ROWS) ||
				(node == parent))
			{
				// kein gültiger Nachbarknoten
				continue;
			}
			
			if ((inOpenList[node] == TRUE) && (nodeBlocked[node] == FALSE))
			{
				var path_cost = nodePathCost[parent] + EDGE_COST;
				
				if ((node_col != parent_col) && (node_row != parent_row))
				{
					// diagonale Pfade bekommen höhere Wegkosten
					path_cost += EDGE_COST * 0.5;
				}
				
				if (path_cost < nodePathCost[node])
				{
					nodePathCost[node] = path_cost;
					nodeParent[node] = parent;
				}
			}
		}
	}
}

// Liefert zurück, ob sich der angegebene Knoten im Pfad befindet.
BOOL isNodeInPath(int node)
{
	int n;
	
	for (n = 0; n < NUMBER_OF_NODES; n++)
	{
		if (path[n] == node)
			return TRUE;
		else if (path[n] == NULLNODE) // Ende des Pfads
			break;
	}
	
	return FALSE;
}

PANEL *pfindDescPnl =
{
	digits(10, 10, "rot: blockierte Knoten\ngruen: Pfad", *, 0, 0);
	flags = SHOW;
}

void main()
{
	level_load(NULL);
	
	vec_set(&camera->x, vector(50, -80, 180));
	vec_set(&camera->pan, vector(90, -80, 0));
	
	path[0] = NULLNODE; // noch kein Pfad gefunden
	
	// Finde den Pfad von Knoten 12 zu Knoten 98
	dijkstra(12, 98);
	
	while (1)
	{
		int row, col;
		
		// Zeichne den Graphen
		for (row = 0; row < NUMBER_OF_ROWS; row++)
		{
			for (col = 0; col < NUMBER_OF_COLS; col++)
			{	
				COLOR node_color;
				int node = NODE_INDEX(col, row);
				
				if (isNodeInPath(node) == TRUE)
					vec_set(&node_color, vector(0, 255, 0));
				else if (nodeBlocked[node] == FALSE)
					vec_set(&node_color, vector(128, 128, 128));
				else
					vec_set(&node_color, vector(0, 0, 255));
				
				draw_point3d(vector(col * 10, -row * 10, 0), &node_color, 100, 5);
			}
		}
		wait(1);
	}
}