The last node needs to have distance "0" to next node. Do the inverse of when adding nodes. Also add some basic tests covering this bug. Fixes: #4659 Signed-off-by: Ralph Sennhauser <ralph.sennhauser@gmail.com>