Implement preliminary GeoJson route output

src/dijkstrasearch.c

@@ -1,6 +1,7 @@
 #include "dijkstrasearch.h"
 #include "logging.h"
 #include <stdio.h>
+#include <math.h>
 
 void dijkstra_search_reset(struct dijkstra_search *search)
 {
@@ -139,6 +140,12 @@ struct dijkstra_node *dijkstra_search_process_queue(struct dijkstra_search *sear
 	return node;
 }
 
+/*!
+ * \brief dijkstra_search_find_path Calculates the path between start and destination
+ * \param search
+ * \param destination the destination. May also be set to NULL to calculate the entire map costs.
+ * \return
+ */
 int dijkstra_search_find_path(struct dijkstra_search *search,
 			      struct dijkstra_node *destination)
 {
@@ -162,6 +169,100 @@ int dijkstra_search_find_path(struct dijkstra_search *search,
 	return -1;
 }
 
+/*!
+ * \brief dijkstra_search_get_path get list of nodes (must call dijkstra_search_find_path before this)
+ * \param search
+ * \param destination
+ * \return a list of nodes or NULL if the route has not been found
+ */
+GList *dijkstra_search_get_route(struct dijkstra_search *search,
+				 struct dijkstra_node *destination)
+{
+	GList *l = NULL;
+	struct dijkstra_node *node = NULL;
+	unsigned int i = 0;
+
+	if (!destination || !search->states[destination->uid]->visited)
+		return NULL;
+
+	node = destination;
+	while (1) {
+		l = g_list_prepend(l, node);
+
+		if (node == search->start)
+			break;
+
+		node = dijkstra_node_get_connection(node, search->states[node->uid]->cheapest_path);
+
+		if (i++ > search->node_count) {
+			report(LL_CRITICAL, "Iteration limit for backsolver. This should never happen.");
+			break;
+		}
+	}
+
+	return l;
+}
+
+void dijkstra_search_route_to_geojson(struct dijkstra_search *search,
+				      GList *route)
+{
+	GList *l = NULL;
+	struct dijkstra_node *node = NULL;
+	struct dijkstra_node *node_last = NULL;
+	float heading = -1.0;
+	float last_heading = 0;
+	char *relative_direction_str = NULL;
+	char *format_str = NULL;
+
+	printf("[");
+
+	for (l = route; l != NULL; l = l->next) {
+		node = (struct dijkstra_node*) l->data;
+		if (node_last) {
+			heading = atanf((node->position.y - node_last->position.y) / (node->position.x - node_last->position.x)) / (M_PI * 2) * 360;
+			heading = atanf((node->position.y - node_last->position.y) / (node->position.x - node_last->position.x)) / (M_PI * 2) * 360;
+			if (last_heading - heading > 10) {
+				relative_direction_str = "right";
+			} else if (last_heading - heading < -10) {
+				relative_direction_str = "left";
+			} else {
+				relative_direction_str = "straight";
+			}
+			if (node_last == search->start)
+				format_str = "Start here.";
+			else if (search->states[node_last->uid]->cheapest_path->name == search->states[node->uid]->cheapest_path->name)
+				format_str = "Stay on %2$s by going %1$s.";
+			else
+				format_str = "Go %s onto %s.";
+
+			if (node_last != search->start)
+				printf(",");
+			printf("\n");
+			printf("{\"type\": \"Feature\",\n"
+			       "    \"geometry\": {\n"
+			       "        \"type\": \"LineString\", \"coordinates\": [[%f,%f],[%f,%f]]\n"
+			       "    },\n",
+			       node_last->position.x, node_last->position.y,
+			       node->position.x, node->position.y);
+			printf("    \"properties\": {\n"
+			       "        \"heading\": \"%f\",\n"
+			       "        \"through\": \"%s\",\n",
+			       heading, search->states[node->uid]->cheapest_path->name);
+
+			printf("        \"description\": \"");
+			printf(format_str, relative_direction_str, search->states[node->uid]->cheapest_path->name);
+			printf("\"\n");
+
+			printf("    }\n");
+			printf("}");
+			last_heading = heading;
+		}
+		node_last = node;
+	}
+
+	printf("]\n");
+}
+
 void dijkstra_print_path(struct dijkstra_search *search,
 			 struct dijkstra_node *destination)
 {

src/dijkstrasearch.h

@@ -25,7 +25,13 @@ void dijkstra_search_set_start(struct dijkstra_search *search,
 			       struct dijkstra_node *start);
 int dijkstra_search_find_path(struct dijkstra_search *search,
 			      struct dijkstra_node *destination);
+GList *dijkstra_search_get_route(struct dijkstra_search *search,
+				struct dijkstra_node *destination);
+
+/* Debugging functions */
 void dijkstra_print_path(struct dijkstra_search *search,
 			struct dijkstra_node *destination);
 void dijkstra_print_nodes(struct dijkstra_search *search);
+void dijkstra_search_route_to_geojson(struct dijkstra_search *search,
+				      GList *route);
 #endif // DIJKSTRASEARCH_H

src/main.c

@@ -51,6 +51,8 @@ int main(int argc, char *argv[])
 	struct dijkstra_search *search;
 	int iterations;
 
+	GList *route = NULL;
+
 	/* Init stuff */
 	set_log_level(LL_DEBUG);
 
@@ -129,7 +131,9 @@ int main(int argc, char *argv[])
 	dijkstra_search_set_start(search, start);
 	if ((iterations = dijkstra_search_find_path(search, dest)) > 0) {
 		report(LL_INFO, "Found path from %d to %d after %d iterations.", start->uid, dest->uid, iterations);
-		dijkstra_print_path(search, dest);
+		route = dijkstra_search_get_route(search, dest);
+		dijkstra_search_route_to_geojson(search, route);
+		// dijkstra_print_path(search, dest);
 		// dijkstra_print_nodes(search);
 	} else {
 		report(LL_WARNING, "Couldn't find path.");