Insert function seems to be working now.
Merge branch 'master' of https://github.com/digital-idoru/kd_trees