diff --git a/src/bench.rs b/src/bench.rs
index 83f89e4..ecb7da7 100644
--- a/src/bench.rs
+++ b/src/bench.rs
@@ -3,6 +3,7 @@ extern crate kdtree;
 extern crate rand;
 
 use bencher::Bencher;
+use rand::Rng;
 
 
 #[derive(Copy, Clone, PartialEq)]
@@ -47,13 +48,23 @@ impl kdtree::kdtree::KdtreePointTrait for Point3WithId {
     }
 }
 
+fn gen_random() -> f64 {
+    rand::thread_rng().gen_range(0., 10000.)
+}
+
+fn generate_points(point_count : usize) -> Vec<Point3WithId> {
+    let mut points : Vec<Point3WithId> = vec![];
+
+    for i in 0 .. point_count {
+        points.push(Point3WithId::new(i as i32, gen_random(),gen_random(),gen_random()));
+    }
+
+    points
+}
+
 fn bench_creating_1000_node_tree(b: &mut Bencher) {
     let len = 1000usize;
-    let mut points: Vec<Point2WithId> = vec![];
-    for id in 0..len {
-        let x: f64 = rand::random();
-        points.push(Point2WithId::new(id as i32, x, x));
-    }
+    let mut points = generate_points(len);
 
     b.iter(|| {
         kdtree::kdtree::Kdtree::new(&mut points.clone());
@@ -62,11 +73,7 @@ fn bench_creating_1000_node_tree(b: &mut Bencher) {
 
 fn bench_single_loop_times_for_1000_node_tree(b: &mut Bencher) {
     let len = 1000usize;
-    let mut points: Vec<Point3WithId> = vec![];
-
-    for i in 0..len {
-        points.push(Point3WithId::new(i as i32, rand::random(), rand::random(), rand::random()))
-    }
+    let mut points = generate_points(len);
 
     let tree = kdtree::kdtree::Kdtree::new(&mut points.clone());
 
@@ -76,16 +83,27 @@ fn bench_single_loop_times_for_1000_node_tree(b: &mut Bencher) {
 
 fn bench_creating_1000_000_node_tree(b: &mut Bencher) {
     let len = 1000_000usize;
-    let mut points: Vec<Point2WithId> = vec![];
-    for id in 0..len {
-        let x: f64 = rand::random();
-        points.push(Point2WithId::new(id as i32, x, x));
-    }
+    let mut points = generate_points(len);
 
     b.iter(|| {
         kdtree::kdtree::Kdtree::new(&mut points.clone());
     });
 }
 
-benchmark_group!(benches, bench_creating_1000_node_tree,bench_single_loop_times_for_1000_node_tree);
+fn bench_adding_to_1000_tree(b: &mut Bencher) {
+    let len = 1000usize;
+    let mut points = generate_points(len);
+    let mut tree = kdtree::kdtree::Kdtree::new(&mut points);
+
+    let point = Point3WithId::new(-1 as i32, gen_random(),gen_random(),gen_random());
+    println!("before ..");
+    b.iter(|| {
+        println!("in lam ..");
+        tree.insert_node(point);
+    });
+}
+
+
+
+benchmark_group!(benches, bench_creating_1000_node_tree,bench_single_loop_times_for_1000_node_tree, bench_adding_to_1000_tree);
 benchmark_main!(benches);
\ No newline at end of file
diff --git a/src/kdtree/mod.rs b/src/kdtree/mod.rs
index 5391f41..dddde96 100644
--- a/src/kdtree/mod.rs
+++ b/src/kdtree/mod.rs
@@ -9,7 +9,7 @@ mod bounds;
 use self::bounds::*;
 use self::distance::*;
 
-pub trait KdtreePointTrait: Copy {
+pub trait KdtreePointTrait: Copy + PartialEq {
     fn dims(&self) -> &[f64];
 }
 
@@ -58,6 +58,7 @@ impl<KdtreePoint: KdtreePointTrait> Kdtree<KdtreePoint> {
         let dimension = self.node_adding_dimension;
         let index_of_new_node = self.add_node(node_to_add,dimension,node_to_add.dims()[dimension]);
         self.node_adding_dimension = ( dimension + 1) % node_to_add.dims().len();
+        let mut should_pop_node = false;
 
         loop {
 
@@ -67,18 +68,30 @@ impl<KdtreePoint: KdtreePointTrait> Kdtree<KdtreePoint> {
                 if let Some(left_node_index) = current_node.left_node {
                     current_index = left_node_index
                 } else {
-                    current_node.left_node = Some(index_of_new_node);
+                    if current_node.point.eq(&node_to_add) {
+                        should_pop_node = true;
+                    } else {
+                        current_node.left_node = Some(index_of_new_node);
+                    }
                     break;
                 }
             } else {
                 if let Some(right_node_index) = current_node.right_node {
                     current_index = right_node_index
                 } else {
-                    current_node.right_node = Some(index_of_new_node);
+                    if current_node.point.eq(&node_to_add) {
+                        should_pop_node = true;
+                    } else {
+                        current_node.right_node = Some(index_of_new_node);
+                    }
                     break;
                 }
             }
         }
+
+        if should_pop_node {
+            self.nodes.pop();
+        }
     }
 
     fn nearest_search_impl(&self, p: &KdtreePoint, searched_index: usize, best_distance_squared: &mut f64, best_leaf_found: &mut usize) {
diff --git a/tests/integration_tests.rs b/tests/integration_tests.rs
index b1f9051..263d58d 100644
--- a/tests/integration_tests.rs
+++ b/tests/integration_tests.rs
@@ -4,6 +4,7 @@ extern crate rand;
 use rand::Rng;
 
 use kdtree::kdtree::*;
+use kdtree::kdtree::test_common::tests_utils::*;
 
 //these could be taken from test_common, but I dont fully understand the module thingy yet.
 #[derive(Copy, Clone, PartialEq)]
@@ -66,7 +67,7 @@ fn generate_points(point_count : usize) -> Vec<Point3WithId> {
 #[test]
 fn test_against_1000_random_points() {
     let point_count = 1000usize;
-    let mut points = generate_points(point_count);
+    let points = generate_points(point_count);
 
     let tree = kdtree::kdtree::Kdtree::new(&mut points.clone());