Add extra debugging/testing controls
Fix some distance bugs
Merge branch 'master' into prakhar