Fixes after CR

Additionally:
Distance comparision method changed to more precise one without performance degradation.