diff --git a/demo.scm b/demo.scm
index 1cddeeb..3b92ddb 100644
--- a/demo.scm
+++ b/demo.scm
@@ -1,13 +1,31 @@
 (use-modules (graphgif)
-	     (srfi srfi-9 gnu)
+	     (srfi srfi-1)
+	     (srfi srfi-43)
 	     (ice-9 copy-tree)
 	     (d-))
 
 (define red (create-color 1 0 0))
 
-(define graph1 (generate-web 10 10))
-(define graph2
-  (~> (copy-tree graph1)
+(define graph
+  (~> (generate-web 10 10)
       (remove-rect 10 1 3 6 3)
       (remove-rect 10 6 3 6 7)))
-(write-graphs-to-file (vector graph1 graph2) "graph.webp")
+
+(define (color-graph graph visited heap)
+  (map (lambda (pair)
+	 (cons (car pair)
+	       (if (memq (car pair) visited)
+		   (set-node-color (cdr pair) red)
+		   (cdr pair))))
+       graph))
+
+(define* (heap-unfold heap #:optional (infinites? #f))
+  (if (and (heap-peek heap) (or infinites? (finite? (car (heap-peek heap)))))
+      (cons (heap-peek heap) (heap-unfold (heap-pop heap) infinites?))
+      '()))
+
+(define graphs '())
+(djikstra graph 90 9 (lambda (visited heap)
+		       (set! graphs (cons (color-graph graph visited heap) graphs))))
+(write-graphs-to-file (reverse-list->vector graphs) "djikstra.webp")
+
diff --git a/graphgif.scm b/graphgif.scm
index fe5fbca..0defe00 100644
--- a/graphgif.scm
+++ b/graphgif.scm
@@ -111,7 +111,7 @@
 	  (node-edges node)
 	  (fold (lambda (entry set)
 		  (if (memq idx (node-edges (cdr entry)))
-		      (cons idx set)
+		      (cons (car entry) set)
 		      set))
 		'()
 		graph)))))
@@ -180,11 +180,11 @@
 ;;;;;;;;;;;;;;;;;;;
 
 ;; Heap algorithms
-(define (heap-insert priority item heap)
+(define-public (heap-insert priority item heap)
   (define (fix-up! i heap)
     (if (zero? i)
 	heap
-	(let* ([parent (quotient i 2)]
+	(let* ([parent (quotient (1- i) 2)]
 	       [i-priority (car (vector-ref heap i))]
 	       [parent-priority (car (vector-ref heap parent))])
 	  (when (<= i-priority parent-priority)
@@ -192,13 +192,13 @@
 	  (fix-up! parent heap))))
   (fix-up! (vector-length heap) (vector-append heap (vector (cons priority item)))))
 
-(define (heap-peek heap)
+(define-public (heap-peek heap)
   (if (vector-empty? heap) #f (vector-ref heap 0)))
 
-(define (heap-pop heap)
+(define-public (heap-pop heap)
   (define (fix-down! heap i)
     (let* ([len (vector-length heap)]
-	   [left (* 2 i)]
+	   [left (1+ (* 2 i))]
 	   [right (1+ left)])
       (if (< left len)
 	  (let ([min (if (< right len)
@@ -208,14 +208,35 @@
 		heap
 		(begin
 		  (vector-swap! heap i min)
-		  (fix-down! heap i))))
+		  (fix-down! heap min))))
 	  heap)))
-  (if (vector-empty? heap)
-      heap
+  (if (or (vector-empty? heap) (= (vector-length heap) 1))
+      #()
       (let ([new-heap (vector-copy heap 0 (1- (vector-length heap)))])
 	(vector-set! new-heap 0 (vector-ref heap (1- (vector-length heap))))
 	(fix-down! new-heap 0))))
 
+(define* (djikstra graph source sink #:optional update)
+  (define unvisited (list->vector (map (lambda (entry)
+					 (cons (inf) (car entry)))
+				       graph)))
+  (let iter ([heap (heap-insert 0 source unvisited)]
+	     [visited '()])
+    (define-values (dist idx) (car+cdr (heap-peek heap)))
+    (cond
+     [(eqv? sink idx) dist]
+     [(memv idx visited) (iter (heap-pop heap) visited)]
+     [(inf? dist) #f]
+     [else
+      (when update (update visited heap))
+      (iter (fold
+	     (lambda (edge heap)
+	       (heap-insert (+ dist 1) edge heap))
+	     (heap-pop heap)
+	     (node-edges (assv-ref graph idx)))
+	    (cons idx visited))])))
+(export djikstra)
+
 ;;;;;;;;;;;;
 ;; Output ;;
 ;;;;;;;;;;;;
@@ -230,7 +251,7 @@
    surfaces)
   (system* "ffmpeg" "-y"
 	   "-i" (string-append pngdir "/img-%d.png")
-	   "-r" "1" "-loop" "0" filename))
+	   "-loop" "0" filename))
 
 (define-public (write-graphs-to-file graphs filename)
   (output-to-file (vector-map (lambda (_ graph) (draw-abstract-graph graph)) graphs)
diff --git a/test.scm b/test.scm
new file mode 100644
index 0000000..846d770
--- /dev/null
+++ b/test.scm
@@ -0,0 +1,20 @@
+(use-modules (rnrs base)
+	     (d-)
+	     (graphgif))
+
+(begin
+  (display "Testing heap operations...")
+  (newline)
+  (let ([heap (~>> #()
+		   (heap-insert (inf) 1)
+		   (heap-insert (inf) 2)
+		   (heap-insert (inf) 3)
+		   (heap-insert 0 1))])
+    (assert (equal? (heap-peek heap) '(0 . 1)))
+    (set! heap (heap-insert 1 2 (heap-pop heap)))
+    (assert (equal? (heap-peek heap) '(1 . 2)))
+    (set! heap (heap-insert 3 3 heap))
+    (assert (equal? (heap-peek heap) '(1 . 2)))
+    (assert (equal? (heap-peek (heap-pop heap)) '(3 . 3))))
+  (assert (equal? (heap-pop #((0 . 0))) #()))
+  (assert (equal? (heap-peek (heap-pop (heap-pop #((1 . 1) (1 . 10) (+inf.0 . 2) (+inf.0 . 3) (1 . 11))))) '(1 . 11))))