Fixed reachability measurement

Various bug fixes and verified correct on small topology