Fixed some bugs, added more diagnostic code, changed distance functions