aga,agar: Dijkstra's algorithm

Implement Dijkstra's algorithm for one-source shortest-path.

This uses the lpq module as the implementation of the priority queue.  That
means this implementation is some way behind the theoretical efficiency of
Dijkstra's algorithm.  It should be reasonably straightforward to swap out
the priority queue for a better one in the future, though.
Signed-off-by: David Gibson <david@gibson.dropbear.id.au>

ccan/aga/_info

@@ -33,6 +33,8 @@ int main(int argc, char *argv[])
 		printf("ccan/check_type\n");
 		printf("ccan/lstack\n");
 		printf("ccan/lqueue\n");
+		printf("ccan/order\n");
+		printf("ccan/lpq\n");
 		return 0;
 	}
 

ccan/aga/aga.h

@@ -118,6 +118,7 @@
 #include <ccan/check_type/check_type.h>
 #include <ccan/lstack/lstack.h>
 #include <ccan/lqueue/lqueue.h>
+#include <ccan/lpq/lpq.h>
 
 struct aga_graph;
 struct aga_node;
@@ -157,6 +158,13 @@ struct aga_node {
 			struct lqueue_link next;
 			const void *edge;
 		} bfs;
+		struct {
+			aga_icost_t distance;
+			struct aga_node *prev;
+			const void *prevedge;
+			bool complete;
+			struct lpq_link pqlink;
+		} dijkstra;
 	} u;
 };
 
@@ -167,6 +175,9 @@ struct aga_graph {
 	aga_first_edge_fn first_edge;
 	aga_next_edge_fn next_edge;
 	aga_edge_info_fn edge_info;
+	union {
+		LPQ(struct aga_node, u.dijkstra.pqlink) dijkstra;
+	} state;
 };
 
 /*
@@ -213,6 +224,8 @@ void aga_init_graph_(struct aga_graph *g,
 enum aga_error {
 	/* No error */
 	AGA_ERR_NONE = 0,
+	/* Negative edge cost (in a context where it's not valid) */
+	AGA_ERR_NEGATIVE_COST = 1,
 };
 
 /**
@@ -341,4 +354,77 @@ struct aga_node *aga_bfs_explore(struct aga_graph *g, struct aga_node *n);
 #define aga_bfs(_n, _g, _start)					\
 	for ((_n) = (_start) ; ((_n) = aga_bfs_explore((_g), (_n))) != NULL; )
 
+/*
+ * Dijkstra's algorithm
+ */
+
+/**
+ * aga_dijkstra_start - Start Dijkstra's algorithm
+ * @g: graph
+ * @source: source node
+ *
+ * Start's Dijkstra's algorithm on @g to find shortest paths from node
+ * @source, to other nodes in @g.
+ */
+int aga_dijkstra_start(struct aga_graph *g, struct aga_node *source);
+
+
+/**
+ * aga_dijkstra_step - Find the node with the next shortest path
+ * @g: graph
+ *
+ * The first call to this function returns the source node specified
+ * in aga_dijkstra_start().  Subsequent calls return the next closest
+ * node to source by shortest path cost.  Returns NULL if no further
+ * nodes are reachable from source.
+ */
+struct aga_node *aga_dijkstra_step(struct aga_graph *g);
+
+/**
+ * aga_dijkstra_path - Find the shortest path to a node
+ * @g: graph
+ * @dest: destination node
+ * @prev: Second last node in the path *output)
+ * @prevedge: Last edge in the path
+ *
+ * Finds the shortest path from the source node (specified in
+ * aga_dijkstra_start() to @dest using Dijkstra's algorithm.
+ *
+ * If no path exists, return false.
+ *
+ * If a path does exist, returns true.  Additionally if @total_cost is
+ * non-NULL, store the total cost of the path in *@total_cost, if
+ * @prev is non-NULL, store the node in the path immediately before
+ * @dest in *@prev and if @prevedge is non-NULL stores the edge which
+ * leads from *@prev to @dest in *@prevedge.
+ *
+ * If @dest is the same as source, 0 will be stored in @cost, and NULL
+ * will be stored in *@prev and *@prevedge.
+ *
+ * The full path from source to @dest can be determined by repeatedly
+ * calling aga_dijkstra_path() on *@prev.
+ *
+ * NOTE: Dijkstra's algorithm will not work correctly on a graph which
+ * has negative costs on some edges.  If aga detects this case, it
+ * will set aga_error() to AGA_ERR_NEGATIVE_COST.  However,
+ * aga_dijkstra_path() may produce incorrect results without detecting
+ * this situation.  aga_dijkstra_all_paths() *is* guaranteed to
+ * discover any negative cost edge reachable from the starting node.
+ */
+bool aga_dijkstra_path(struct aga_graph *g, struct aga_node *dest,
+		       aga_icost_t *total_cost,
+		       struct aga_node **prev, const void **prevedge);
+
+/**
+ * aga_dijkstra_all_paths - Find shortest paths to all reachable nodes
+ * @g: graph
+ *
+ * Finds shortest paths from the source node (specified in
+ * aga_dijkstra_start()) to all other reachable nodes in @g.  No
+ * results are returned directly, but between calling
+ * aga_dijkstra_all_paths() and aga_finish, aga_dijkstra_path() is
+ * guaranteed to complete in O(1) time for all destinations.
+ */
+void aga_dijkstra_all_paths(struct aga_graph *g);
+
 #endif /* CCAN_AGA_H */

ccan/aga/dijkstra.c

+/* Licensed under LGPLv2+ - see LICENSE file for details */
+#include "config.h"
+
+#include <stdbool.h>
+#include <stdlib.h>
+#include <assert.h>
+
+#include <ccan/build_assert/build_assert.h>
+#include <ccan/check_type/check_type.h>
+#include <ccan/order/order.h>
+#include <ccan/lpq/lpq.h>
+
+#include <ccan/aga/aga.h>
+#include "private.h"
+
+/*
+ * Dijkstra's algorithm
+ */
+
+/* Node states */
+#define STATE_INFINITE	0
+#define STATE_FINITE	1
+#define STATE_COMPLETE	2
+
+
+static void candidate_path(struct aga_graph *g, struct aga_node *node,
+			   aga_icost_t distance,
+			   struct aga_node *prev, const void *prevedge)
+{
+	if (aga_update_node(g, node)) {
+		/* New node, treat has having infinite distance */
+		node->u.dijkstra.distance = distance;
+		node->u.dijkstra.prev = prev;
+		node->u.dijkstra.prevedge = prevedge;
+		node->u.dijkstra.complete = false;
+
+		lpq_enqueue(&g->state.dijkstra, node);
+	} else if (distance < node->u.dijkstra.distance) {
+		assert(!node->u.dijkstra.complete);
+
+		node->u.dijkstra.distance = distance;
+		node->u.dijkstra.prev = prev;
+		node->u.dijkstra.prevedge = prevedge;
+
+		lpq_reorder(&g->state.dijkstra, node);
+	}
+}
+
+int aga_dijkstra_start(struct aga_graph *g, struct aga_node *source)
+{
+	total_order_by_field(order, long_reverse,
+			     struct aga_node, u.dijkstra.distance);
+	int rc;
+
+	/* Make sure we're actually using the right ordering for
+	 * aga_icost_t */
+	BUILD_ASSERT(check_types_match(long, aga_icost_t) == 0);
+
+	rc = aga_start(g);
+	if (rc < 0)
+		return rc;
+
+	lpq_init(&g->state.dijkstra, order.cb, order.ctx);
+
+	candidate_path(g, source, 0, NULL, NULL);
+
+	return 0;
+}
+
+struct aga_node *aga_dijkstra_step(struct aga_graph *g)
+{
+	struct aga_node *n = lpq_dequeue(&g->state.dijkstra);
+	const void *e;
+	struct aga_edge_info ei;
+	int err;
+
+	if (!aga_check_state(g))
+		return NULL;
+
+	if (!n)
+		return NULL;
+
+	aga_for_each_edge_info(e, ei, err, g, n) {
+		if (ei.icost < 0) {
+			aga_fail(g, AGA_ERR_NEGATIVE_COST);
+			return NULL;
+		}
+		candidate_path(g, ei.to,
+			       n->u.dijkstra.distance + ei.icost, n, e);
+	}
+	if (err) {
+		aga_fail(g, err);
+		return NULL;
+	}
+
+	n->u.dijkstra.complete = true;
+
+	return n;
+}
+
+bool aga_dijkstra_path(struct aga_graph *g, struct aga_node *node,
+		       aga_icost_t *total_cost,
+		       struct aga_node **prev, const void **prevedge)
+{
+	if (!aga_check_state(g))
+		return false;
+
+	while (aga_node_needs_update(g, node) || !node->u.dijkstra.complete) {
+		if (!aga_dijkstra_step(g))
+			return false;
+	};
+
+	if (total_cost)
+		*total_cost = node->u.dijkstra.distance;
+	if (prev)
+		*prev = node->u.dijkstra.prev;
+	if (prevedge)
+		*prevedge = node->u.dijkstra.prevedge;
+
+	return true;
+}
+
+void aga_dijkstra_all_paths(struct aga_graph *g)
+{
+	if (!aga_check_state(g))
+		return;
+
+	while (aga_dijkstra_step(g))
+		;
+}
+

ccan/aga/test/api-dijkstra.c

+#include "config.h"
+
+#include <stddef.h>
+#include <assert.h>
+#include <stdlib.h>
+
+#include <ccan/tap/tap.h>
+#include <ccan/array_size/array_size.h>
+
+#include <ccan/aga/aga.h>
+
+#include "simple-graph.h"
+
+static void test_trivial(void)
+{
+	struct trivial_graph tg;
+	aga_icost_t cost;
+	struct aga_node *node;
+	const void *edge;
+
+	trivial_graph_init(&tg);
+
+	ok1(aga_dijkstra_start(&tg.sg.g, &tg.sg.nodes[1]) == 0);
+	ok1(aga_dijkstra_step(&tg.sg.g) == &tg.sg.nodes[1]);
+	ok1(aga_dijkstra_step(&tg.sg.g) == NULL);
+	ok1(aga_dijkstra_path(&tg.sg.g, &tg.sg.nodes[1], &cost, &node, &edge));
+	ok1(cost == 0);
+	ok1(node == NULL);
+	ok1(edge == NULL);
+	aga_finish(&tg.sg.g);
+}
+
+static void test_parallel(void)
+{
+	struct parallel_graph pg;
+	aga_icost_t cost;
+	struct aga_node *node;
+
+	parallel_graph_init(&pg, 3);
+
+	ok1(aga_dijkstra_start(&pg.sg.g, &pg.sg.nodes[1]) == 0);
+	ok1(aga_dijkstra_step(&pg.sg.g) == &pg.sg.nodes[1]);
+	ok1(aga_dijkstra_step(&pg.sg.g) == &pg.sg.nodes[2]);
+	ok1(aga_dijkstra_step(&pg.sg.g) == NULL);
+	ok1(aga_dijkstra_path(&pg.sg.g, &pg.sg.nodes[1], &cost, NULL, NULL));
+	ok1(cost == 0);
+	ok1(aga_dijkstra_path(&pg.sg.g, &pg.sg.nodes[2], &cost, &node, NULL));
+	ok1(cost == 1);
+	ok1(node == &pg.sg.nodes[1]);
+	aga_finish(&pg.sg.g);
+
+	ok1(aga_dijkstra_start(&pg.sg.g, &pg.sg.nodes[2]) == 0);
+	ok1(aga_dijkstra_step(&pg.sg.g) == &pg.sg.nodes[2]);
+	ok1(aga_dijkstra_step(&pg.sg.g) == NULL);
+	ok1(aga_dijkstra_path(&pg.sg.g, &pg.sg.nodes[2], &cost, NULL, NULL));
+	ok1(cost == 0);
+	ok1(!aga_dijkstra_path(&pg.sg.g, &pg.sg.nodes[1], NULL, NULL, NULL));
+	aga_finish(&pg.sg.g);
+}
+
+#define FULL_LEN	4
+
+static void test_full(void)
+{
+	struct full_graph fg;
+	int i, j;
+
+	full_graph_init(&fg, FULL_LEN);
+
+	for (i = 1; i <= FULL_LEN; i++) {
+		ok1(aga_dijkstra_start(&fg.sg.g, &fg.sg.nodes[i]) == 0);
+
+		for (j = 1; j <= FULL_LEN; j++) {
+			aga_icost_t cost;
+			struct aga_node *node;
+			const void *edge;
+
+			ok1(aga_dijkstra_path(&fg.sg.g, &fg.sg.nodes[j],
+					      &cost, &node, &edge));
+			if (i == j) {
+				ok1(cost == 0);
+				ok1(node == NULL);
+				ok1(edge == NULL);
+			} else {
+				ok1(cost == 1);
+				ok1(node == &fg.sg.nodes[i]);
+				ok1(edge == &fg.sg.nodes[j]);
+			}
+		}
+
+		aga_finish(&fg.sg.g);
+	}
+}
+
+#define CHAIN_LEN	8
+
+static void test_chain(void)
+{
+	struct chain_graph cg;
+	int i, j;
+
+	chain_graph_init(&cg, CHAIN_LEN);
+
+	for (i = 1; i <= CHAIN_LEN; i++) {
+		ok1(aga_dijkstra_start(&cg.fg.sg.g, &cg.fg.sg.nodes[i]) == 0);
+
+		for (j = 1; j <= CHAIN_LEN; j++) {
+			aga_icost_t cost;
+
+			ok1(aga_dijkstra_path(&cg.fg.sg.g, &cg.fg.sg.nodes[j],
+					      &cost, NULL, NULL));
+			ok1(cost == labs(i - j));
+		}
+
+		aga_finish(&cg.fg.sg.g);
+	}
+}
+
+static void test_error(void)
+{
+	struct error_graph eg;
+	aga_icost_t cost;
+
+	error_graph_init(&eg);
+
+	ok1(aga_dijkstra_start(&eg.sg.g, &eg.sg.nodes[1]) == 0);
+	ok1(aga_dijkstra_path(&eg.sg.g, &eg.sg.nodes[1], &cost, NULL, NULL));
+	ok1(cost == 0);
+	ok1(aga_dijkstra_path(&eg.sg.g, &eg.sg.nodes[2], &cost, NULL, NULL));
+	ok1(cost == 1);
+	ok1(!aga_dijkstra_path(&eg.sg.g, &eg.sg.nodes[3], &cost, NULL, NULL));
+	ok1(!aga_dijkstra_path(&eg.sg.g, &eg.sg.nodes[4], &cost, NULL, NULL));
+	aga_finish(&eg.sg.g);
+
+	ok1(aga_dijkstra_start(&eg.sg.g, &eg.sg.nodes[3]) == 0);
+	ok1(aga_dijkstra_path(&eg.sg.g, &eg.sg.nodes[3], &cost, NULL, NULL));
+	ok1(cost == 0);
+	ok1(!aga_dijkstra_path(&eg.sg.g, &eg.sg.nodes[4], &cost, NULL, NULL));
+	ok1(aga_error(&eg.sg.g) == -1);
+	aga_finish(&eg.sg.g);
+}
+
+static void test_traversal1(void)
+{
+	struct traversal1_graph t1g;
+	aga_icost_t cost;
+
+	/* This is mostly about testing we correctly handle
+	 * non-reachable nodes */
+	traversal1_graph_init(&t1g);
+
+	ok1(aga_dijkstra_start(&t1g.sg.g, &t1g.sg.nodes[1]) == 0);
+	ok1(aga_dijkstra_path(&t1g.sg.g, &t1g.sg.nodes[1],
+			      &cost, NULL, NULL));
+	ok1(cost == 0);
+	ok1(aga_dijkstra_path(&t1g.sg.g, &t1g.sg.nodes[2],
+			      &cost, NULL, NULL));
+	ok1(cost == 1);
+	ok1(aga_dijkstra_path(&t1g.sg.g, &t1g.sg.nodes[3],
+			      &cost, NULL, NULL));
+	ok1(cost == 1);
+	ok1(aga_dijkstra_path(&t1g.sg.g, &t1g.sg.nodes[4],
+			      &cost, NULL, NULL));
+	ok1(cost == 2);
+	ok1(aga_dijkstra_path(&t1g.sg.g, &t1g.sg.nodes[5],
+			      &cost, NULL, NULL));
+	ok1(cost == 2);
+	ok1(aga_dijkstra_path(&t1g.sg.g, &t1g.sg.nodes[6],
+			      &cost, NULL, NULL));
+	ok1(cost == 2);
+	ok1(!aga_dijkstra_path(&t1g.sg.g, &t1g.sg.nodes[7],
+			       NULL, NULL, NULL));
+	ok1(!aga_dijkstra_path(&t1g.sg.g, &t1g.sg.nodes[8],
+			       NULL, NULL, NULL));
+	ok1(!aga_dijkstra_path(&t1g.sg.g, &t1g.sg.nodes[9],
+			       NULL, NULL, NULL));
+	aga_finish(&t1g.sg.g);
+
+	ok1(aga_dijkstra_start(&t1g.sg.g, &t1g.sg.nodes[9]) == 0);
+	ok1(aga_dijkstra_path(&t1g.sg.g, &t1g.sg.nodes[9],
+			      &cost, NULL, NULL));
+	ok1(cost == 0);
+	ok1(aga_dijkstra_path(&t1g.sg.g, &t1g.sg.nodes[8],
+			      &cost, NULL, NULL));
+	ok1(cost == 1);
+	ok1(aga_dijkstra_path(&t1g.sg.g, &t1g.sg.nodes[7],
+			      &cost, NULL, NULL));
+	ok1(cost == 1);
+	ok1(aga_dijkstra_path(&t1g.sg.g, &t1g.sg.nodes[6],
+			      &cost, NULL, NULL));
+	ok1(cost == 2);
+	ok1(aga_dijkstra_path(&t1g.sg.g, &t1g.sg.nodes[5],
+			      &cost, NULL, NULL));
+	ok1(cost == 2);
+	ok1(aga_dijkstra_path(&t1g.sg.g, &t1g.sg.nodes[4],
+			      &cost, NULL, NULL));
+	ok1(cost == 2);
+	ok1(!aga_dijkstra_path(&t1g.sg.g, &t1g.sg.nodes[3],
+			       NULL, NULL, NULL));
+	ok1(!aga_dijkstra_path(&t1g.sg.g, &t1g.sg.nodes[2],
+			       NULL, NULL, NULL));
+	ok1(!aga_dijkstra_path(&t1g.sg.g, &t1g.sg.nodes[1],
+			       NULL, NULL, NULL));
+	aga_finish(&t1g.sg.g);
+}
+
+int main(void)
+{
+	plan_tests(7 + 15
+		   + FULL_LEN * (1 + FULL_LEN*4)
+		   + CHAIN_LEN * (1 + CHAIN_LEN*2)
+		   + 12 + 32);
+
+	test_trivial();
+	test_parallel();
+	test_full();
+	test_chain();
+	test_error();
+	test_traversal1();
+	
+	return exit_status();
+}

ccan/agar/agar.c

@@ -234,3 +234,60 @@ bool agar_bfs_explore(struct agar_state *sr, const void *nr,
 	*nextr = n_to_nr(sr, next);
 	return true;
 }
+
+/*
+ * Dijkstra's algorithm
+ */
+
+struct agar_state *agar_dijkstra_new(void *ctx, struct agar_graph *gr,
+				     const void *nr)
+{
+	struct agar_state *sr = agar_new(ctx, gr);
+
+	if (aga_dijkstra_start(&sr->g, nr_to_n(sr, nr)) < 0) {
+		tal_free(sr);
+		return NULL;
+	}
+
+	return sr;
+}
+
+bool agar_dijkstra_step(struct agar_state *sr, const void **nextr)
+{
+	struct aga_node *next = aga_dijkstra_step(&sr->g);
+
+	if (!next)
+		return false;
+
+	*nextr = n_to_nr(sr, next);
+	return true;
+}
+
+bool agar_dijkstra_path(struct agar_state *sr, const void *destr,
+			aga_icost_t *total_cost,
+			const void **prevr, const void **prevedge)
+{
+	struct aga_node *dest = nr_to_n(sr, destr);
+	struct aga_node *prev;
+
+	if (!aga_dijkstra_path(&sr->g, dest, total_cost, &prev, prevedge))
+		return false;
+
+	/*
+	 * When destr is the same as the source node, there obviously
+	 * isn't a previous node or edge.  In that case aga, sets them
+	 * to NULL.  But for agar, NULL could be a valid node
+	 * references (particularly if using ptrint).  So we don't
+	 * have much choice here but to leave *prevr as undefined when
+	 * destr is the source node. */
+	if (prevr && prev)
+		*prevr = n_to_nr(sr, prev);
+
+	return true;
+}
+
+void agar_dijkstra_all_paths(struct agar_state *sr)
+{
+	aga_dijkstra_all_paths(&sr->g);
+}
+

ccan/agar/agar.h

@@ -71,4 +71,16 @@ bool agar_bfs_explore(struct agar_state *sr, const void *nr,
 #define agar_bfs(_nr, _sr, _startr)					\
 	for ((_nr) = (_startr); agar_bfs_explore((_sr), (_nr), &(_nr)); )
 
+/*
+ * Dijkstra's algorithm
+ */
+
+struct agar_state *agar_dijkstra_new(void *ctx, struct agar_graph *gr,
+				     const void *nr);
+bool agar_dijkstra_step(struct agar_state *sr, const void **nextr);
+bool agar_dijkstra_path(struct agar_state *sr, const void *destr,
+			aga_icost_t *total_cost,
+			const void **prevr, const void **prevedge);
+void agar_dijkstra_all_paths(struct agar_state *sr);
+
 #endif /* CCAN_AGAR_H */

ccan/agar/test/api-dijkstra.c

+#include "config.h"
+
+#include <stddef.h>
+#include <assert.h>
+#include <stdlib.h>
+
+#include <ccan/tap/tap.h>
+#include <ccan/tal/tal.h>
+#include <ccan/array_size/array_size.h>
+
+#include <ccan/agar/agar.h>
+
+#include "simple-graphr.h"
+
+static void test_trivial(void)
+{
+	struct trivial_graphr tgr;
+	struct agar_state *sr;
+	aga_icost_t cost;
+	const void *node;
+
+	trivial_graphr_init(&tgr);
+
+	ok1(sr = agar_dijkstra_new(NULL, &tgr.gr, int2ptr(1)));
+	ok1(agar_dijkstra_step(sr, &node));
+	ok1(ptr2int(node) == 1);
+	ok1(!agar_dijkstra_step(sr, &node));
+	ok1(agar_dijkstra_path(sr, int2ptr(1), &cost, NULL, NULL));
+	ok1(cost == 0);
+	tal_free(sr);
+}
+
+static void test_parallel(void)
+{
+	struct parallel_graphr pgr;
+	struct agar_state *sr;
+	aga_icost_t cost;
+	const void *node;
+
+	parallel_graphr_init(&pgr, 3);
+
+	ok1(sr = agar_dijkstra_new(NULL, &pgr.gr, int2ptr(1)));
+	ok1(agar_dijkstra_step(sr, &node));
+	ok1(ptr2int(node) == 1);
+	ok1(agar_dijkstra_step(sr, &node));
+	ok1(ptr2int(node) == 2);
+	ok1(!agar_dijkstra_step(sr, &node));
+	ok1(agar_dijkstra_path(sr, int2ptr(1), &cost, NULL, NULL));
+	ok1(cost == 0);
+	ok1(agar_dijkstra_path(sr, int2ptr(2), &cost, &node, NULL));
+	ok1(cost == 1);
+	ok1(node == int2ptr(1));
+	tal_free(sr);
+
+	ok1(sr = agar_dijkstra_new(NULL, &pgr.gr, int2ptr(2)));
+	ok1(agar_dijkstra_step(sr, &node));
+	ok1(ptr2int(node) == 2);
+	ok1(!agar_dijkstra_step(sr, &node));
+	ok1(agar_dijkstra_path(sr, int2ptr(2), &cost, NULL, NULL));
+	ok1(cost == 0);
+	ok1(!agar_dijkstra_path(sr, int2ptr(1), NULL, NULL, NULL));
+	tal_free(sr);
+}
+
+#define FULL_LEN	4
+
+static void test_full(void)
+{
+	struct full_graphr fgr;
+	int i, j;
+
+	full_graphr_init(&fgr, FULL_LEN);
+
+	for (i = 1; i <= FULL_LEN; i++) {
+		struct agar_state *sr;
+
+		ok1(sr = agar_dijkstra_new(NULL, &fgr.gr, int2ptr(i)));
+
+		for (j = 1; j <= FULL_LEN; j++) {
+			aga_icost_t cost;
+			const void *node, *edge;
+
+			ok1(agar_dijkstra_path(sr, int2ptr(j),
+					      &cost, &node, &edge));
+			if (i == j) {
+				ok1(cost == 0);
+			} else {
+				ok1(cost == 1);
+				ok1(node == int2ptr(i));
+				ok1(edge == int2ptr(j));
+			}
+		}
+
+		tal_free(sr);
+	}
+}
+
+#define CHAIN_LEN	8
+
+static void test_chain(void)
+{
+	struct chain_graphr cgr;
+	int i, j;
+
+	chain_graphr_init(&cgr, CHAIN_LEN);
+
+	for (i = 1; i <= CHAIN_LEN; i++) {
+		struct agar_state *sr;
+
+		ok1(sr = agar_dijkstra_new(NULL, &cgr.fgr.gr, int2ptr(i)));
+
+		for (j = 1; j <= CHAIN_LEN; j++) {
+			aga_icost_t cost;
+
+			ok1(agar_dijkstra_path(sr, int2ptr(j),
+					      &cost, NULL, NULL));
+			ok1(cost == labs(i - j));
+		}
+
+		tal_free(sr);
+	}
+}
+
+static void test_error(void)
+{
+	struct error_graphr egr;
+	struct agar_state *sr;
+	aga_icost_t cost;
+
+	error_graphr_init(&egr);
+
+	ok1(sr = agar_dijkstra_new(NULL, &egr.gr, int2ptr(1)));
+	ok1(agar_dijkstra_path(sr, int2ptr(1), &cost, NULL, NULL));
+	ok1(cost == 0);
+	ok1(agar_dijkstra_path(sr, int2ptr(2), &cost, NULL, NULL));
+	ok1(cost == 1);
+	ok1(!agar_dijkstra_path(sr, int2ptr(3), &cost, NULL, NULL));
+	ok1(!agar_dijkstra_path(sr, int2ptr(4), &cost, NULL, NULL));
+	tal_free(sr);
+
+	ok1(sr = agar_dijkstra_new(NULL, &egr.gr, int2ptr(3)));
+	ok1(agar_dijkstra_path(sr, int2ptr(3), &cost, NULL, NULL));
+	ok1(cost == 0);
+	ok1(!agar_dijkstra_path(sr, int2ptr(4), &cost, NULL, NULL));
+	ok1(agar_error(sr) == -1);
+	tal_free(sr);
+}
+
+static void test_traversal1(void)
+{
+	struct traversal1_graphr t1gr;
+	struct agar_state *sr;
+	aga_icost_t cost;
+
+	/* This is mostly about testing we correctly handle
+	 * non-reachable nodes */
+	traversal1_graphr_init(&t1gr);
+
+	ok1(sr = agar_dijkstra_new(NULL, &t1gr.gr, int2ptr(1)));
+	ok1(agar_dijkstra_path(sr, int2ptr(1),
+			      &cost, NULL, NULL));
+	ok1(cost == 0);
+	ok1(agar_dijkstra_path(sr, int2ptr(2),
+			      &cost, NULL, NULL));
+	ok1(cost == 1);
+	ok1(agar_dijkstra_path(sr, int2ptr(3),
+			      &cost, NULL, NULL));
+	ok1(cost == 1);
+	ok1(agar_dijkstra_path(sr, int2ptr(4),
+			      &cost, NULL, NULL));
+	ok1(cost == 2);
+	ok1(agar_dijkstra_path(sr, int2ptr(5),
+			      &cost, NULL, NULL));
+	ok1(cost == 2);
+	ok1(agar_dijkstra_path(sr, int2ptr(6),
+			      &cost, NULL, NULL));
+	ok1(cost == 2);
+	ok1(!agar_dijkstra_path(sr, int2ptr(7),
+			       NULL, NULL, NULL));
+	ok1(!agar_dijkstra_path(sr, int2ptr(8),
+			       NULL, NULL, NULL));
+	ok1(!agar_dijkstra_path(sr, int2ptr(9),
+			       NULL, NULL, NULL));
+	tal_free(sr);
+
+	ok1(sr = agar_dijkstra_new(NULL, &t1gr.gr, int2ptr(9)));
+	ok1(agar_dijkstra_path(sr, int2ptr(9),
+			      &cost, NULL, NULL));
+	ok1(cost == 0);
+	ok1(agar_dijkstra_path(sr, int2ptr(8),
+			      &cost, NULL, NULL));
+	ok1(cost == 1);
+	ok1(agar_dijkstra_path(sr, int2ptr(7),
+			      &cost, NULL, NULL));
+	ok1(cost == 1);
+	ok1(agar_dijkstra_path(sr, int2ptr(6),
+			      &cost, NULL, NULL));
+	ok1(cost == 2);
+	ok1(agar_dijkstra_path(sr, int2ptr(5),
+			      &cost, NULL, NULL));
+	ok1(cost == 2);
+	ok1(agar_dijkstra_path(sr, int2ptr(4),
+			      &cost, NULL, NULL));
+	ok1(cost == 2);
+	ok1(!agar_dijkstra_path(sr, int2ptr(3),
+			       NULL, NULL, NULL));
+	ok1(!agar_dijkstra_path(sr, int2ptr(2),
+			       NULL, NULL, NULL));
+	ok1(!agar_dijkstra_path(sr, int2ptr(1),
+			       NULL, NULL, NULL));
+	tal_free(sr);
+}
+
+int main(void)
+{
+	plan_tests(6 + 18
+		   + FULL_LEN * (FULL_LEN*4 - 1)
+		   + CHAIN_LEN * (1 + CHAIN_LEN*2)
+		   + 12 + 32);
+
+	test_trivial();
+	test_parallel();
+	test_full();
+	test_chain();
+	test_error();
+	test_traversal1();
+	
+	return exit_status();
+}