diff --git a/src/interpolation/interpolationPivots.js b/src/interpolation/interpolationPivots.js
index 0ccd076..949a511 100644
--- a/src/interpolation/interpolationPivots.js
+++ b/src/interpolation/interpolationPivots.js
@@ -4,61 +4,62 @@ import {placeNearToNearestNeighbour} from "./interpCommon";
 export default function(sampleSet, remainderSet, numPivots, distanceFn) {
   // Pivot based parent finding
   let numBuckets = Math.floor(Math.sqrt(sampleSet.length));
-  let pivots = takeSampleFrom(sampleSet, numPivots);
-  console.log("Pivots", pivots);
-  // Common temporary variables
-  let i, j, pivot, sample, bucketWidth; // Temp var, declared seperately to avoid GC
+  let sets = takeSampleFrom(sampleSet, numPivots);
+  let pivots = sets.sample;
+  let nonPivotSamples = sets.remainder;
 
   let pivotsBuckets = []; // [ For each Pivot:[For each bucket:[each point in bucket]] ]
-
-  for (i = 0; i < numPivots; i++) {
+  for (let i = 0; i < numPivots; i++) {
     pivotsBuckets[i] = [];
-    for (j = 0; j < numBuckets; j++) {
+    for (let j = 0; j < numBuckets; j++) {
       pivotsBuckets[i][j] = [];
     }
   }
 
-  // Pre-calculate distance between each sample to each pivot
-  let distCache = [], // [ For each Sample:[For each Pivot: distance] ]
-    bucketWidths = []; // [ For each Pivot: width of each bucket ]
+  // Pre-calculate distance between each non-pivot sample to each pivot
+  // At the same time, determine the bucket width for each pivot based on furthest non-pivot sample
+  let distCache = []; // [ For each non-pivot Sample:[For each Pivot: distance] ]
+  let bucketWidths = []; // [ For each Pivot: width of each bucket ]
 
-  for (i = 0; i < sampleSet.length; i++)
+  for (let i = 0; i < nonPivotSamples.length; i++)
     distCache[i] = [];
 
-  for (j = 0; j < numPivots; j++) {
-    pivot = pivots[j],
-      maxDist = -1;
+  for (let j = 0; j < numPivots; j++) {
+    let pivot = pivots[j];
+    let maxDist = -1;
 
-    for (i = 0; i < sampleSet.length; i++) {
-      sample = sampleSet[i];
-      if (pivot !== sample) {
-        distCache[i][j] = distanceFn(pivot, sample);
-        if (distCache[i][j] > maxDist)
-          maxDist = distCache[i][j];
-      } else {
-        distCache[i][j] = 0;
-      }
+    for (let i = 0; i < nonPivotSamples.length; i++) {
+      let sample = nonPivotSamples[i];
+      distCache[i][j] = distanceFn(pivot, sample);
+      if (distCache[i][j] > maxDist)
+        maxDist = distCache[i][j];
     }
 
     bucketWidths.push(maxDist / numBuckets);
   }
+  // ---------------------------------------------------------------------
 
-  // console.log(distCaches);
-
-  // Put samples (pivot included) into buckets
-  for (j = 0; j < numPivots; j++) {
-    bucketWidth = bucketWidths[j];
-    for (i = 0; i < sampleSet.length; i++) {
-      sample = sampleSet[i];
-      pivotsBuckets[j][Math.floor(distCache[i][j] / bucketWidth)].push(sample);
+  // Put samples (pivot not included) into buckets
+  for (let j = 0; j < numPivots; j++) {
+    let bucketWidth = bucketWidths[j];
+    for (let i = 0; i < nonPivotSamples.length; i++) {
+      let sample = nonPivotSamples[i];
+      let bucketNumber = Math.floor(distCache[i][j] / bucketWidth);
+      if (bucketNumber >= numBuckets) {
+        bucketNumber = numBuckets - 1;
+      } else if (bucketNumber < 0) {  // Should never be negative anyway
+        bucketNumber = 0;
+      }
+      pivotsBuckets[j][bucketNumber].push(sample);
     }
   }
 
 
+  let sampleSubset = takeSampleFrom(sampleSet, Math.sqrt(sampleSet.length)).sample;
   //Plot each of the remainder nodes
   for (let node of remainderSet) {
     let sampleSubsetDistanceCache = [],
-      sampleSubset = takeSampleFrom(sampleSet, Math.sqrt(sampleSet.length)).sample;
+      minDist, nearSample;
 
     // Pivot based parent search
     for (let p = 0; p < numPivots; p++) {
@@ -67,8 +68,13 @@ export default function(sampleSet, remainderSet, numPivots, distanceFn) {
 
       let dist = distanceFn(node, pivot);
       let index = sampleSubset.indexOf(pivot);
-      if (index !== -1)
+      if (index !== -1) {
         sampleSubsetDistanceCache[index] = dist;
+      }
+      if (minDist === undefined || dist < minDist){
+        minDist = dist;
+        nearSample = pivot;
+      }
 
       let bucketNumber = Math.floor(dist / bucketWidth);
       if (bucketNumber >= numBuckets) {
@@ -77,22 +83,28 @@ export default function(sampleSet, remainderSet, numPivots, distanceFn) {
         bucketNumber = 0;
       }
 
-      let clDist, minNode;
       for (let candidateNode of pivotsBuckets[p][bucketNumber]) {
+        let index = sampleSubset.indexOf(candidateNode);
+        if (index !== -1 && sampleSubsetDistanceCache[index] !== undefined)
+          dist = sampleSubsetDistanceCache[index]
+        else {
+          dist = distanceFn(candidateNode, node);
+          if (index !== -1)
+            sampleSubsetDistanceCache[index] = dist;
+        }
 
-        dist = distanceFn(candidateNode, node);
-        if (candidateNode <= clDist) {
-          clDist = candidateNode;
-          minNode = bucketContents[w];
+        if (dist < minDist){
+          minDist = dist;
+          nearSample = candidateNode;
         }
       }
     }
 
+    // Fill in holes in cache
     for (let k = 0; k < sampleSubset.length; k++) {
-      if (sampleSubsetDistanceCache[k] !== undefined)
+      if (sampleSubsetDistanceCache[k] === undefined)
         sampleSubsetDistanceCache[k] = distanceFn(node, sampleSubset[k]);
     }
-    let radius = distanceFn(node, minNode);
-    placeNearToNearestNeighbour(node, minNode, radius, sampleSubset, sampleSubsetDistanceCache);
+    placeNearToNearestNeighbour(node, nearSample, minDist, sampleSubset, sampleSubsetDistanceCache);
   }
 }