Merge dijkstra heap algorithms

Fixes #43