Merge closely spaced points

and refactoring:)