Added additional logging, fixed bugs. Hunting bug which results in NaN
distances.