First prototype of Dijkstras algorithm rewritten from Scala. #not-tested