added some helper functions for debugging and for improved voxel protocol

- added outputBufferBits() which will output an entire buffer as debug bits/bytes
- tweak to outputBits() to allow caller to ask for NewLine or not
- added insertIntoSortedArrays() which keeps a set of arrays sorted by a key

libraries/shared/src/SharedUtil.cpp

@@ -52,14 +52,30 @@ bool randomBoolean() {
     return rand() % 2;
 }
 
-void outputBits(unsigned char byte) {
+void outputBufferBits(unsigned char* buffer, int length, bool withNewLine) {
-    printLog("%d: ", byte);
+    for (int i = 0; i < length; i++) {
+        outputBits(buffer[i],false);
+    }
+    if (withNewLine) {
+        printLog("\n");
+    }
+}
+
+void outputBits(unsigned char byte, bool withNewLine) {
+    if (isalnum(byte)) {
+        printLog("[ %d (%c): ", byte, byte);
+    } else {
+        printLog("[ %d (0x%x): ", byte, byte);
+    }
     
     for (int i = 0; i < 8; i++) {
         printLog("%d", byte >> (7 - i) & 1);
     }
+    printLog(" ] ");
     
+    if (withNewLine) {
         printLog("\n");
+    }
 }
 
 int numberOfOnes(unsigned char byte) {
@@ -356,3 +372,34 @@ void printVoxelCode(unsigned char* voxelCode) {
     }
 #endif
 
+
+// Inserts the value and key into three arrays sorted by the key array, the first array is the value,
+// the second array is a sorted key for the value, the third array is the index for the value in it original
+// non-sorted array 
+// returns -1 if size exceeded
+int insertIntoSortedArrays(void* value, float key, int originalIndex, 
+            void** valueArray, float* keyArray, int* originalIndexArray, int currentCount, int maxCount) {
+            
+    if (currentCount < maxCount) {
+        int i=0;
+        if (currentCount > 0) {
+            while (i<currentCount && key > keyArray[i]) {
+                i++;
+            }
+            // i is our desired location
+            // shift array elements to the right
+            if (i < currentCount && i+1 < maxCount) {
+                memcpy(&valueArray[i+1],&valueArray[i],sizeof(void*) * (currentCount-i));
+                memcpy(&keyArray[i+1],&keyArray[i],sizeof(float) * (currentCount-i));
+                memcpy(&originalIndexArray[i+1],&originalIndexArray[i],sizeof(int) * (currentCount-i));
+            }
+        }
+        // place new element at i
+        valueArray[i]=value;
+        keyArray[i]=key;
+        originalIndexArray[i]=originalIndex;
+        return currentCount+1;
+    }
+    return -1; // error case
+}
+

libraries/shared/src/SharedUtil.h

@@ -42,7 +42,8 @@ float randFloatInRange (float min,float max);
 unsigned char randomColorValue(int minimum);
 bool randomBoolean();
 
-void outputBits(unsigned char byte);
+void outputBufferBits(unsigned char* buffer, int length, bool withNewLine = true);
+void outputBits(unsigned char byte, bool withNewLine = true);
 void printVoxelCode(unsigned char* voxelCode);
 int numberOfOnes(unsigned char byte);
 bool oneAtBit(unsigned char byte, int bitIndex);
@@ -70,5 +71,7 @@ bool createVoxelEditMessage(unsigned char command, short int sequence,
 void usleep(int waitTime);
 #endif
 
+int insertIntoSortedArrays(void* value, float key, int originalIndex, 
+            void** valueArray, float* keyArray, int* originalIndexArray, int currentCount, int maxCount);
 
 #endif /* defined(__hifi__SharedUtil__) */