It compiles again now.
Added a reference for each point that is added, as well as did a better job of implementing the find nearest function.