Refactor find closest points algprithm implementation