Fixed torus distance and added tests. Witness example still seems wonky, though...not sure what's going on.