First prototype for KD tree implemented, supports, insert, delete, find, findNearestNeigbour and find all nodes within radius