diff --git a/src/common/inc/blobs.h b/src/common/inc/blobs.h
index a9069f99..bc881051 100644
--- a/src/common/inc/blobs.h
+++ b/src/common/inc/blobs.h
@@ -20,6 +20,7 @@
 #include "pixytypes.h"
 #include "colorlut.h"
 #include "qqueue.h"
+#include <stdlib.h>  //added to get sort by area functionality
 
 #define MAX_BLOBS             100
 #define MAX_BLOBS_PER_MODEL   20
@@ -28,6 +29,7 @@
 #define MIN_COLOR_CODE_AREA   10
 #define MAX_CODED_DIST        8
 #define MAX_COLOR_CODE_MODELS 5
+#define MAX_FTCBLOBS          5
 
 #define BL_BEGIN_MARKER	      0xaa55
 #define BL_BEGIN_MARKER_CC    0xaa56
@@ -49,6 +51,7 @@ class Blobs
     uint16_t getBlock(uint8_t *buf, uint32_t buflen);
     uint16_t getCCBlock(uint8_t *buf, uint32_t buflen);
     BlobA *getMaxBlob(uint16_t signature=0, uint16_t *numBlobs=NULL);
+    void getMaxBlobs(uint16_t signature, uint16_t maxNumBlobs, BlobA **maxBlobs, uint16_t *numBlobs);
     void getBlobs(BlobA **blobs, uint32_t *len, BlobB **ccBlobs, uint32_t *ccLen);
     int setParams(uint16_t maxBlobs, uint16_t maxBlobsPerModel, uint32_t minArea, ColorCodeMode ccMode);
     int runlengthAnalysis();
@@ -69,6 +72,7 @@ class Blobs
     bool closeby(BlobA *blob0, BlobA *blob1);
     int16_t distance(BlobA *blob0, BlobA *blob1);
     void sort(BlobA *blobs[], uint16_t len, BlobA *firstBlob, bool horiz);
+    //static int compareBlobAreas(const void *a, const void *b);
     int16_t angle(BlobA *blob0, BlobA *blob1);
     int16_t distance(BlobA *blob0, BlobA *blob1, bool horiz);
     void processCC();
@@ -84,6 +88,9 @@ class Blobs
     uint16_t *m_blobs;
     uint16_t m_numBlobs;
 
+
+    uint16_t *m_ftcblobs;
+
     BlobB *m_ccBlobs;
     uint16_t m_numCCBlobs;
 
diff --git a/src/common/src/blobs.cpp b/src/common/src/blobs.cpp
index 8e7451d6..5225ffda 100644
--- a/src/common/src/blobs.cpp
+++ b/src/common/src/blobs.cpp
@@ -1,1118 +1,1245 @@
-//
-// begin license header
-//
-// This file is part of Pixy CMUcam5 or "Pixy" for short
-//
-// All Pixy source code is provided under the terms of the
-// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
-// Those wishing to use Pixy source code, software and/or
-// technologies under different licensing terms should contact us at
-// cmucam@cs.cmu.edu. Such licensing terms are available for
-// all portions of the Pixy codebase presented here.
-//
-// end license header
-//
-
-#ifndef PIXY
-#include "debug.h"
-#else
-#include "pixy_init.h"
-#endif
-
-#include "blobs.h"
-
-#define CC_SIGNATURE(s) (m_ccMode==CC_ONLY || m_clut.getType(s)==CL_MODEL_TYPE_COLORCODE)
-
-Blobs::Blobs(Qqueue *qq, uint8_t *lut) : m_clut(lut)
-{
-    int i;
-
-    m_mutex = false;
-    m_minArea = MIN_AREA;
-    m_maxBlobs = MAX_BLOBS;
-    m_maxBlobsPerModel = MAX_BLOBS_PER_MODEL;
-    m_mergeDist = MAX_MERGE_DIST;
-    m_maxBlob = NULL;
-
-	m_qq = qq;
-#ifdef PIXY
-    m_maxCodedDist = MAX_CODED_DIST;
-#else
-    m_maxCodedDist = MAX_CODED_DIST/2;
-    m_qvals = new uint32_t[0x8000];
-#endif
-    m_ccMode = DISABLED;
-
-    m_blobs = new uint16_t[MAX_BLOBS*5];
-    m_numBlobs = 0;
-    m_blobReadIndex = 0;
-    m_ccBlobReadIndex = 0;
-
-    // reset blob assemblers
-    for (i=0; i<CL_NUM_SIGNATURES; i++)
-        m_assembler[i].Reset();
-}
-
-int Blobs::setParams(uint16_t maxBlobs, uint16_t maxBlobsPerModel, uint32_t minArea, ColorCodeMode ccMode)
-{
-    if (maxBlobs<=MAX_BLOBS)
-        m_maxBlobs = maxBlobs;
-    else
-        m_maxBlobs = MAX_BLOBS;
-
-    m_maxBlobsPerModel = maxBlobsPerModel;
-    m_minArea = minArea;
-    m_ccMode = ccMode;
-
-    return 0;
-}
-
-Blobs::~Blobs()
-{
-    delete [] m_blobs;
-#ifndef PIXY
-    delete [] m_qvals;
-#endif
-}
-
-int Blobs::handleSegment(uint8_t signature, uint16_t row, uint16_t startCol, uint16_t length)
-{
-	SSegment s;
-
-    s.model = signature;
-    s.row = row;
-    s.startCol = startCol;
-    s.endCol = startCol+length;
-
-#ifndef PIXY
-    uint32_t qval;
-
-    qval = signature;
-    qval |= startCol<<3;
-    qval |= length<<12;
-
-    m_qvals[m_numQvals++] = qval;
-#endif
-
-    return m_assembler[signature-1].Add(s);
-}
-
-// Blob format:
-// 0: model
-// 1: left X edge
-// 2: right X edge
-// 3: top Y edge
-// 4: bottom Y edge
-int Blobs::runlengthAnalysis()
-{
-    int32_t row=-1, icount=0;
-    uint32_t startCol, sig, prevSig, prevStartCol, segmentStartCol, segmentEndCol, segmentSig=0;
-    bool merge;
-    Qval qval;
-    int32_t res=0;
-	register int32_t u, v, c;
-
-#ifndef PIXY
-    m_numQvals = 0;
-#endif
-
-    while(1)
-    {
-        while (m_qq->dequeue(&qval)==0);
-        if (qval.m_col>=0xfffe)
-            break;
-		if (res<0)
-			continue;
-        if (qval.m_col==0)
-        {
-            prevStartCol = 0xffff;
-            prevSig = 0;
-            if (segmentSig)
-            {
-                res = handleSegment(segmentSig, row, segmentStartCol-1, segmentEndCol - segmentStartCol+1);
-                segmentSig = 0;
-            }
-            row++;
-#ifndef PIXY
-            m_qvals[m_numQvals++] = 0;
-#else
-			if (icount++==5) // an interleave of every 5 lines or about every 175us seems good
-			{
-				g_chirpUsb->service();
-				icount = 0;
-			}
-#endif
-            continue;
-        }
-
-        sig = qval.m_col&0x07;
-
-        u = qval.m_u;
-        v = qval.m_v;
-
-        u <<= CL_LUT_ENTRY_SCALE;
-        v <<= CL_LUT_ENTRY_SCALE;
-        c = qval.m_y;
-        if (c==0)
-            c = 1;
-        u /= c;
-        v /= c;
-
-        if (m_clut.m_runtimeSigs[sig-1].m_uMin<u && u<m_clut.m_runtimeSigs[sig-1].m_uMax &&
-                m_clut.m_runtimeSigs[sig-1].m_vMin<v && v<m_clut.m_runtimeSigs[sig-1].m_vMax && c>=(int32_t)m_clut.m_miny)
-        {
-         	qval.m_col >>= 3;
-        	startCol = qval.m_col;
-           	merge = startCol-prevStartCol<=5 && prevSig==sig;
-            if (segmentSig==0 && merge)
-            {
-                segmentSig = sig;
-                segmentStartCol = prevStartCol;
-            }
-            else if (segmentSig!=0 && (segmentSig!=sig || !merge))
-            {
-                res = handleSegment(segmentSig, row, segmentStartCol-1, segmentEndCol - segmentStartCol+1);
-                segmentSig = 0;
-            }
-
-            if (segmentSig!=0 && merge)
-                segmentEndCol = startCol;
-            else if (segmentSig==0 && !merge)
-                res = handleSegment(sig, row, startCol-1, 2);
-            prevSig = sig;
-            prevStartCol = startCol;
-        }
-        else if (segmentSig!=0)
-        {
-            res = handleSegment(segmentSig, row, segmentStartCol-1, segmentEndCol - segmentStartCol+1);
-            segmentSig = 0;
-        }
-    }
-	endFrame();
-
-    if (qval.m_col==0xfffe) // error code, queue overrun
-		return -1;
-	return 0;
-}
-
-int Blobs::blobify()
-{
-    uint32_t i, j, k;
-    bool colorCode;
-    CBlob *blob;
-    uint16_t *blobsStart;
-    uint16_t numBlobsStart, invalid, invalid2;
-    uint16_t left, top, right, bottom;
-    //uint32_t timer, timer2=0;
-
-	if (runlengthAnalysis()<0)
-	{
-   	 	for (i=0; i<CL_NUM_SIGNATURES; i++)
-        	m_assembler[i].Reset();
-    	m_numBlobs = 0;
-		m_numCCBlobs = 0;
-		return -1;
-	}
-
-    // copy blobs into memory
-    invalid = 0;
-    // mutex keeps interrupt routine from stepping on us
-    m_mutex = true;
-
-    m_maxBlob = NULL;
-
-    for (i=0, m_numBlobs=0, m_numCCBlobs=0; i<CL_NUM_SIGNATURES; i++)
-    {
-        colorCode = CC_SIGNATURE(i+1);
-
-        for (j=m_numBlobs*5, k=0, blobsStart=m_blobs+j, numBlobsStart=m_numBlobs, blob=m_assembler[i].finishedBlobs;
-             blob && m_numBlobs<m_maxBlobs && k<m_maxBlobsPerModel; blob=blob->next, k++)
-        {
-            if ((colorCode && blob->GetArea()<MIN_COLOR_CODE_AREA) ||
-                (!colorCode && blob->GetArea()<(int)m_minArea))
-                continue;
-            blob->getBBox((short &)left, (short &)top, (short &)right, (short &)bottom);
-            if (bottom-top<=1) // blobs that are 1 line tall
-                continue;
-            m_blobs[j + 0] = i+1;
-            m_blobs[j + 1] = left;
-            m_blobs[j + 2] = right;
-            m_blobs[j + 3] = top;
-            m_blobs[j + 4] = bottom;
-            m_numBlobs++;
-            j += 5;
-
-        }
-        //setTimer(&timer);
-        if (!colorCode) // do not combine color code models
-        {
-            while(1)
-            {
-                invalid2 = combine2(blobsStart, m_numBlobs-numBlobsStart);
-                if (invalid2==0)
-                    break;
-                invalid += invalid2;
-            }
-        }
-        //timer2 += getTimer(timer);
-    }
-    //setTimer(&timer);
-    invalid += combine(m_blobs, m_numBlobs);
-    if (m_ccMode!=DISABLED)
-    {
-        m_ccBlobs = (BlobB *)(m_blobs + m_numBlobs*5);
-        // calculate number of codedblobs left
-        processCC();
-    }
-    if (invalid || m_ccMode!=DISABLED)
-    {
-        invalid2 = compress(m_blobs, m_numBlobs);
-        m_numBlobs -= invalid2;
-    }
-    //timer2 += getTimer(timer);
-    //cprintf("time=%d\n", timer2); // never seen this greater than 200us.  or 1% of frame period
-
-    // reset read indexes-- new frame
-    m_blobReadIndex = 0;
-    m_ccBlobReadIndex = 0;
-    m_mutex = false;
-
-    // free memory
-    for (i=0; i<CL_NUM_SIGNATURES; i++)
-        m_assembler[i].Reset();
-
-#if 0
-    static int frame = 0;
-    if (m_numBlobs>0)
-        cprintf("%d: blobs %d %d %d %d %d\n", frame, m_numBlobs, m_blobs[1], m_blobs[2], m_blobs[3], m_blobs[4]);
-    else
-        cprintf("%d: blobs 0\n", frame);
-    frame++;
-#endif
-	return 0;
-}
-
-#ifndef PIXY
-void Blobs::getRunlengths(uint32_t **qvals, uint32_t *len)
-{
-    *qvals = m_qvals;
-    *len = m_numQvals;
-}
-#endif
-
-uint16_t Blobs::getCCBlock(uint8_t *buf, uint32_t buflen)
-{
-    uint16_t *buf16 = (uint16_t *)buf;
-    uint16_t temp, width, height;
-    uint16_t checksum;
-    uint16_t len = 8;  // default
-
-    if (buflen<9*sizeof(uint16_t))
-        return 0;
-
+//
+// begin license header
+//
+// This file is part of Pixy CMUcam5 or "Pixy" for short
+//
+// All Pixy source code is provided under the terms of the
+// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
+// Those wishing to use Pixy source code, software and/or
+// technologies under different licensing terms should contact us at
+// cmucam@cs.cmu.edu. Such licensing terms are available for
+// all portions of the Pixy codebase presented here.
+//
+// end license header
+//
+
+#ifndef PIXY
+#include "debug.h"
+#else
+#include "pixy_init.h"
+#endif
+
+#include "blobs.h"
+#include <stdlib.h>  //added to get sort by area functionality
+
+#define CC_SIGNATURE(s) (m_ccMode==CC_ONLY || m_clut.getType(s)==CL_MODEL_TYPE_COLORCODE)
+
+Blobs::Blobs(Qqueue *qq, uint8_t *lut) : m_clut(lut)
+{
+    int i;
+
+    m_mutex = false;
+    m_minArea = MIN_AREA;
+    m_maxBlobs = MAX_BLOBS;
+    m_maxBlobsPerModel = MAX_BLOBS_PER_MODEL;
+    m_mergeDist = MAX_MERGE_DIST;
+    m_maxBlob = NULL;
+
+	m_qq = qq;
+#ifdef PIXY
+    m_maxCodedDist = MAX_CODED_DIST;
+#else
+    m_maxCodedDist = MAX_CODED_DIST/2;
+    m_qvals = new uint32_t[0x8000];
+#endif
+    m_ccMode = DISABLED;
+
+    m_blobs = new uint16_t[MAX_BLOBS*5];
+    m_ftcblobs = new uint16_t[MAX_FTCBLOBS*5];
+    m_numBlobs = 0;
+    m_blobReadIndex = 0;
+    m_ccBlobReadIndex = 0;
+
+    // reset blob assemblers
+    for (i=0; i<CL_NUM_SIGNATURES; i++)
+        m_assembler[i].Reset();
+}
+
+int Blobs::setParams(uint16_t maxBlobs, uint16_t maxBlobsPerModel, uint32_t minArea, ColorCodeMode ccMode)
+{
+    if (maxBlobs<=MAX_BLOBS)
+        m_maxBlobs = maxBlobs;
+    else
+        m_maxBlobs = MAX_BLOBS;
+
+    m_maxBlobsPerModel = maxBlobsPerModel;
+    m_minArea = minArea;
+    m_ccMode = ccMode;
+
+    return 0;
+}
+
+Blobs::~Blobs()
+{
+    delete [] m_blobs;
+#ifndef PIXY
+    delete [] m_qvals;
+#endif
+}
+
+int Blobs::handleSegment(uint8_t signature, uint16_t row, uint16_t startCol, uint16_t length)
+{
+	SSegment s;
+
+    s.model = signature;
+    s.row = row;
+    s.startCol = startCol;
+    s.endCol = startCol+length;
+
+#ifndef PIXY
+    uint32_t qval;
+
+    qval = signature;
+    qval |= startCol<<3;
+    qval |= length<<12;
+
+    m_qvals[m_numQvals++] = qval;
+#endif
+
+    return m_assembler[signature-1].Add(s);
+}
+
+// Blob format:
+// 0: model
+// 1: left X edge
+// 2: right X edge
+// 3: top Y edge
+// 4: bottom Y edge
+int Blobs::runlengthAnalysis()
+{
+    int32_t row=-1, icount=0;
+    uint32_t startCol, sig, prevSig, prevStartCol, segmentStartCol, segmentEndCol, segmentSig=0;
+    bool merge;
+    Qval qval;
+    int32_t res=0;
+	register int32_t u, v, c;
+
+#ifndef PIXY
+    m_numQvals = 0;
+#endif
+
+    while(1)
+    {
+        while (m_qq->dequeue(&qval)==0);
+        if (qval.m_col>=0xfffe)
+            break;
+		if (res<0)
+			continue;
+        if (qval.m_col==0)
+        {
+            prevStartCol = 0xffff;
+            prevSig = 0;
+            if (segmentSig)
+            {
+                res = handleSegment(segmentSig, row, segmentStartCol-1, segmentEndCol - segmentStartCol+1);
+                segmentSig = 0;
+            }
+            row++;
+#ifndef PIXY
+            m_qvals[m_numQvals++] = 0;
+#else
+			if (icount++==5) // an interleave of every 5 lines or about every 175us seems good
+			{
+				g_chirpUsb->service();
+				icount = 0;
+			}
+#endif
+            continue;
+        }
+
+        sig = qval.m_col&0x07;
+
+        u = qval.m_u;
+        v = qval.m_v;
+
+        u <<= CL_LUT_ENTRY_SCALE;
+        v <<= CL_LUT_ENTRY_SCALE;
+        c = qval.m_y;
+        if (c==0)
+            c = 1;
+        u /= c;
+        v /= c;
+
+        if (m_clut.m_runtimeSigs[sig-1].m_uMin<u && u<m_clut.m_runtimeSigs[sig-1].m_uMax &&
+                m_clut.m_runtimeSigs[sig-1].m_vMin<v && v<m_clut.m_runtimeSigs[sig-1].m_vMax && c>=(int32_t)m_clut.m_miny)
+        {
+         	qval.m_col >>= 3;
+        	startCol = qval.m_col;
+           	merge = startCol-prevStartCol<=5 && prevSig==sig;
+            if (segmentSig==0 && merge)
+            {
+                segmentSig = sig;
+                segmentStartCol = prevStartCol;
+            }
+            else if (segmentSig!=0 && (segmentSig!=sig || !merge))
+            {
+                res = handleSegment(segmentSig, row, segmentStartCol-1, segmentEndCol - segmentStartCol+1);
+                segmentSig = 0;
+            }
+
+            if (segmentSig!=0 && merge)
+                segmentEndCol = startCol;
+            else if (segmentSig==0 && !merge)
+                res = handleSegment(sig, row, startCol-1, 2);
+            prevSig = sig;
+            prevStartCol = startCol;
+        }
+        else if (segmentSig!=0)
+        {
+            res = handleSegment(segmentSig, row, segmentStartCol-1, segmentEndCol - segmentStartCol+1);
+            segmentSig = 0;
+        }
+    }
+	endFrame();
+
+    if (qval.m_col==0xfffe) // error code, queue overrun
+		return -1;
+	return 0;
+}
+
+int Blobs::blobify()
+{
+    uint32_t i, j, k;
+    bool colorCode;
+    CBlob *blob;
+    uint16_t *blobsStart;
+    uint16_t numBlobsStart, invalid, invalid2;
+    uint16_t left, top, right, bottom;
+    //uint32_t timer, timer2=0;
+
+	if (runlengthAnalysis()<0)
+	{
+   	 	for (i=0; i<CL_NUM_SIGNATURES; i++)
+        	m_assembler[i].Reset();
+    	m_numBlobs = 0;
+		m_numCCBlobs = 0;
+		return -1;
+	}
+
+    // copy blobs into memory
+    invalid = 0;
+    // mutex keeps interrupt routine from stepping on us
+    m_mutex = true;
+
+    m_maxBlob = NULL;
+
+    for (i=0, m_numBlobs=0, m_numCCBlobs=0; i<CL_NUM_SIGNATURES; i++)
+    {
+        colorCode = CC_SIGNATURE(i+1);
+
+        for (j=m_numBlobs*5, k=0, blobsStart=m_blobs+j, numBlobsStart=m_numBlobs, blob=m_assembler[i].finishedBlobs;
+             blob && m_numBlobs<m_maxBlobs && k<m_maxBlobsPerModel; blob=blob->next, k++)
+        {
+            if ((colorCode && blob->GetArea()<MIN_COLOR_CODE_AREA) ||
+                (!colorCode && blob->GetArea()<(int)m_minArea))
+                continue;
+            blob->getBBox((short &)left, (short &)top, (short &)right, (short &)bottom);
+            if (bottom-top<=1) // blobs that are 1 line tall
+                continue;
+            m_blobs[j + 0] = i+1;
+            m_blobs[j + 1] = left;
+            m_blobs[j + 2] = right;
+            m_blobs[j + 3] = top;
+            m_blobs[j + 4] = bottom;
+            m_numBlobs++;
+            j += 5;
+
+        }
+        //setTimer(&timer);
+        if (!colorCode) // do not combine color code models
+        {
+            while(1)
+            {
+                invalid2 = combine2(blobsStart, m_numBlobs-numBlobsStart);
+                if (invalid2==0)
+                    break;
+                invalid += invalid2;
+            }
+        }
+        //timer2 += getTimer(timer);
+    }
+    //setTimer(&timer);
+    invalid += combine(m_blobs, m_numBlobs);
+    if (m_ccMode!=DISABLED)
+    {
+        m_ccBlobs = (BlobB *)(m_blobs + m_numBlobs*5);
+        // calculate number of codedblobs left
+        processCC();
+    }
+    if (invalid || m_ccMode!=DISABLED)
+    {
+        invalid2 = compress(m_blobs, m_numBlobs);
+        m_numBlobs -= invalid2;
+    }
+    //timer2 += getTimer(timer);
+    //cprintf("time=%d\n", timer2); // never seen this greater than 200us.  or 1% of frame period
+
+    // reset read indexes-- new frame
+    m_blobReadIndex = 0;
+    m_ccBlobReadIndex = 0;
+    m_mutex = false;
+
+    // free memory
+    for (i=0; i<CL_NUM_SIGNATURES; i++)
+        m_assembler[i].Reset();
+
+#if 0
+    static int frame = 0;
+    if (m_numBlobs>0)
+        cprintf("%d: blobs %d %d %d %d %d\n", frame, m_numBlobs, m_blobs[1], m_blobs[2], m_blobs[3], m_blobs[4]);
+    else
+        cprintf("%d: blobs 0\n", frame);
+    frame++;
+#endif
+	return 0;
+}
+
+#ifndef PIXY
+void Blobs::getRunlengths(uint32_t **qvals, uint32_t *len)
+{
+    *qvals = m_qvals;
+    *len = m_numQvals;
+}
+#endif
+
+uint16_t Blobs::getCCBlock(uint8_t *buf, uint32_t buflen)
+{
+    uint16_t *buf16 = (uint16_t *)buf;
+    uint16_t temp, width, height;
+    uint16_t checksum;
+    uint16_t len = 8;  // default
+
+    if (buflen<9*sizeof(uint16_t))
+        return 0;
+
     if (m_mutex || m_ccBlobReadIndex>=m_numCCBlobs) // we're copying, so no CC blocks for now....
-		return 0;
-
-    if (m_blobReadIndex==0 && m_ccBlobReadIndex==0)	// beginning of frame, mark it with empty block
-    {
-        buf16[0] = BL_BEGIN_MARKER;
-        len++;
-        buf16++;
-    }
-
-    // beginning of block
-    buf16[0] = BL_BEGIN_MARKER_CC;
-
-    // model
-    temp = m_ccBlobs[m_ccBlobReadIndex].m_model;
-    checksum = temp;
-    buf16[2] = temp;
-
-    // width
-    width = m_ccBlobs[m_ccBlobReadIndex].m_right - m_ccBlobs[m_ccBlobReadIndex].m_left;
-    checksum += width;
-    buf16[5] = width;
-
-    // height
-    height = m_ccBlobs[m_ccBlobReadIndex].m_bottom - m_ccBlobs[m_ccBlobReadIndex].m_top;
-    checksum += height;
-    buf16[6] = height;
-
-    // x center
-    temp = m_ccBlobs[m_ccBlobReadIndex].m_left + width/2;
-    checksum += temp;
-    buf16[3] = temp;
-
-    // y center
-    temp = m_ccBlobs[m_ccBlobReadIndex].m_top + height/2;
-    checksum += temp;
-    buf16[4] = temp;
-
-    temp = m_ccBlobs[m_ccBlobReadIndex].m_angle;
-    checksum += temp;
-    buf16[7] = temp;
-
-    buf16[1] = checksum;
-
-    // next blob
-    m_ccBlobReadIndex++;
-
-    return len*sizeof(uint16_t);
-}
-
-
-uint16_t Blobs::getBlock(uint8_t *buf, uint32_t buflen)
-{
-    uint16_t *buf16 = (uint16_t *)buf;
-    uint16_t temp, width, height;
-    uint16_t checksum;
-    uint16_t len = 7;  // default
-    int i = m_blobReadIndex*5;
-
-    if (buflen<8*sizeof(uint16_t))
-        return 0;
-
-    if (m_blobReadIndex>=m_numBlobs && m_ccMode!=DISABLED)
-        return getCCBlock(buf, buflen);
-
-    if (m_mutex || m_blobReadIndex>=m_numBlobs) // we're copying, so no blocks for now....
 		return 0;
-
-    if (m_blobReadIndex==0)	// beginning of frame, mark it with empty block
-    {
-        buf16[0] = BL_BEGIN_MARKER;
-        len++;
-        buf16++;
-    }
-
-    // beginning of block
-    buf16[0] = BL_BEGIN_MARKER;
-
-    // model
-    temp = m_blobs[i];
-    checksum = temp;
-    buf16[2] = temp;
-
-    // width
-    width = m_blobs[i+2] - m_blobs[i+1];
-    checksum += width;
-    buf16[5] = width;
-
-    // height
-    height = m_blobs[i+4] - m_blobs[i+3];
-    checksum += height;
-    buf16[6] = height;
-
-    // x center
-    temp = m_blobs[i+1] + width/2;
-    checksum += temp;
-    buf16[3] = temp;
-
-    // y center
-    temp = m_blobs[i+3] + height/2;
-    checksum += temp;
-    buf16[4] = temp;
-
-    buf16[1] = checksum;
-
-    // next blob
-    m_blobReadIndex++;
-
-    return len*sizeof(uint16_t);
-}
-
-
-BlobA *Blobs::getMaxBlob(uint16_t signature, uint16_t *numBlobs)
-{
+
+    if (m_blobReadIndex==0 && m_ccBlobReadIndex==0)	// beginning of frame, mark it with empty block
+    {
+        buf16[0] = BL_BEGIN_MARKER;
+        len++;
+        buf16++;
+    }
+
+    // beginning of block
+    buf16[0] = BL_BEGIN_MARKER_CC;
+
+    // model
+    temp = m_ccBlobs[m_ccBlobReadIndex].m_model;
+    checksum = temp;
+    buf16[2] = temp;
+
+    // width
+    width = m_ccBlobs[m_ccBlobReadIndex].m_right - m_ccBlobs[m_ccBlobReadIndex].m_left;
+    checksum += width;
+    buf16[5] = width;
+
+    // height
+    height = m_ccBlobs[m_ccBlobReadIndex].m_bottom - m_ccBlobs[m_ccBlobReadIndex].m_top;
+    checksum += height;
+    buf16[6] = height;
+
+    // x center
+    temp = m_ccBlobs[m_ccBlobReadIndex].m_left + width/2;
+    checksum += temp;
+    buf16[3] = temp;
+
+    // y center
+    temp = m_ccBlobs[m_ccBlobReadIndex].m_top + height/2;
+    checksum += temp;
+    buf16[4] = temp;
+
+    temp = m_ccBlobs[m_ccBlobReadIndex].m_angle;
+    checksum += temp;
+    buf16[7] = temp;
+
+    buf16[1] = checksum;
+
+    // next blob
+    m_ccBlobReadIndex++;
+
+    return len*sizeof(uint16_t);
+}
+
+
+uint16_t Blobs::getBlock(uint8_t *buf, uint32_t buflen)
+{
+    uint16_t *buf16 = (uint16_t *)buf;
+    uint16_t temp, width, height;
+    uint16_t checksum;
+    uint16_t len = 7;  // default
+    int i = m_blobReadIndex*5;
+
+    if (buflen<8*sizeof(uint16_t))
+        return 0;
+
+    if (m_blobReadIndex>=m_numBlobs && m_ccMode!=DISABLED)
+        return getCCBlock(buf, buflen);
+
+    if (m_mutex || m_blobReadIndex>=m_numBlobs) // we're copying, so no blocks for now....
+		return 0;
+
+    if (m_blobReadIndex==0)	// beginning of frame, mark it with empty block
+    {
+        buf16[0] = BL_BEGIN_MARKER;
+        len++;
+        buf16++;
+    }
+
+    // beginning of block
+    buf16[0] = BL_BEGIN_MARKER;
+
+    // model
+    temp = m_blobs[i];
+    checksum = temp;
+    buf16[2] = temp;
+
+    // width
+    width = m_blobs[i+2] - m_blobs[i+1];
+    checksum += width;
+    buf16[5] = width;
+
+    // height
+    height = m_blobs[i+4] - m_blobs[i+3];
+    checksum += height;
+    buf16[6] = height;
+
+    // x center
+    temp = m_blobs[i+1] + width/2;
+    checksum += temp;
+    buf16[3] = temp;
+
+    // y center
+    temp = m_blobs[i+3] + height/2;
+    checksum += temp;
+    buf16[4] = temp;
+
+    buf16[1] = checksum;
+
+    // next blob
+    m_blobReadIndex++;
+
+    return len*sizeof(uint16_t);
+}
+
+
+BlobA *Blobs::getMaxBlob(uint16_t signature, uint16_t *numBlobs)
+{
     int i;
-	uint16_t blobs;
-    uint32_t area, maxArea;
-    BlobA *blob, *maxBlob;
+	uint16_t blobs;
+    uint32_t area, maxArea;
+    BlobA *blob, *maxBlob;
 	BlobB *ccBlob;
-
+
 	if (m_mutex)
 		return (BlobA *)-1;	 // busy!
-
-    if (signature==0) // 0 means return the biggest regardless of signature number
+
+    if (signature==0) // 0 means return the biggest regardless of signature number
     {
 		if (numBlobs)
 			*numBlobs = 1; // not really used in this mode, so return 1
-
-        // if we've already found it, return it
-        if (m_maxBlob)
-            return m_maxBlob;
-
-        // look through all blobs looking for the blob with the biggest area
-        for (i=0, maxArea=0; i<m_numBlobs; i++)
-        {
-            blob = (BlobA *)m_blobs + i;
-            area = (blob->m_right - blob->m_left)*(blob->m_bottom - blob->m_top);
-            if (area>maxArea)
-            {
-                maxArea = area;
-                m_maxBlob = blob;
-            }
-        }
-        for (i=0; i<m_numCCBlobs; i++)
-        {
-            ccBlob = (BlobB *)m_ccBlobs + i;
-            area = (ccBlob->m_right - ccBlob->m_left)*(ccBlob->m_bottom - ccBlob->m_top);
-            if (area>maxArea)
-            {
-                maxArea = area;
-                m_maxBlob = (BlobA *)ccBlob;
-            }
-        }
-		return m_maxBlob;
+
+        // if we've already found it, return it
+        if (m_maxBlob)
+            return m_maxBlob;
+
+        // look through all blobs looking for the blob with the biggest area
+        for (i=0, maxArea=0; i<m_numBlobs; i++)
+        {
+            blob = (BlobA *)m_blobs + i;
+            area = (blob->m_right - blob->m_left)*(blob->m_bottom - blob->m_top);
+            if (area>maxArea)
+            {
+                maxArea = area;
+                m_maxBlob = blob;
+            }
+        }
+        for (i=0; i<m_numCCBlobs; i++)
+        {
+            ccBlob = (BlobB *)m_ccBlobs + i;
+            area = (ccBlob->m_right - ccBlob->m_left)*(ccBlob->m_bottom - ccBlob->m_top);
+            if (area>maxArea)
+            {
+                maxArea = area;
+                m_maxBlob = (BlobA *)ccBlob;
+            }
+        }
+		return m_maxBlob;
     }
-	// look for a specific signature
-    else
+	// look for a specific signature
+    else
     {
 		// regular signature
 		if (signature<=CL_NUM_SIGNATURES)
-		{
-        	for (i=0, blobs=0, maxBlob=NULL; i<m_numBlobs; i++)
-        	{
-            	blob = (BlobA *)m_blobs + i;
+		{
+        	for (i=0, blobs=0, maxBlob=NULL; i<m_numBlobs; i++)
+        	{
+            	blob = (BlobA *)m_blobs + i;
            		if (blob->m_model==signature)
 				{
 					if (!maxBlob)
                 		maxBlob = blob;
 	 				blobs++;  // count
-				}
+				}
         	}
 		}
 		// color code 
 		else
 		{
         	for (i=0, blobs=0, maxBlob=NULL; i<m_numCCBlobs; i++)
-			{
-            	ccBlob = (BlobB *)m_ccBlobs + i;
+			{
+            	ccBlob = (BlobB *)m_ccBlobs + i;
            		if (ccBlob->m_model==signature)
 				{
 					if (!maxBlob)
                 		maxBlob = (BlobA *)ccBlob;
 	 				blobs++;  // count
 				}
-			}
-        }
+			}
+        }
 		if (numBlobs)
 			*numBlobs = blobs;
-		return maxBlob;
-    }
-
-    return NULL; // no blobs...
-}
-
-void Blobs::getBlobs(BlobA **blobs, uint32_t *len, BlobB **ccBlobs, uint32_t *ccLen)
-{
-    *blobs = (BlobA *)m_blobs;
-    *len = m_numBlobs;
-
-    *ccBlobs = m_ccBlobs;
-    *ccLen = m_numCCBlobs;
-}
-
-
-
-uint16_t Blobs::compress(uint16_t *blobs, uint16_t numBlobs)
-{
-    uint16_t i, ii;
-    uint16_t *destination, invalid;
-
-    // compress list
-    for (i=0, ii=0, destination=NULL, invalid=0; i<numBlobs; i++, ii+=5)
-    {
-        if (blobs[ii+0]==0)
-        {
-            if (destination==NULL)
-                destination = blobs+ii;
-            invalid++;
-            continue;
-        }
-        if (destination)
-        {
-            destination[0] = blobs[ii+0];
-            destination[1] = blobs[ii+1];
-            destination[2] = blobs[ii+2];
-            destination[3] = blobs[ii+3];
-            destination[4] = blobs[ii+4];
-            destination += 5;
-        }
-    }
-    return invalid;
-}
-
-uint16_t Blobs::combine(uint16_t *blobs, uint16_t numBlobs)
-{
-    uint16_t i, j, ii, jj, left0, right0, top0, bottom0;
-    uint16_t left, right, top, bottom;
-    uint16_t invalid;
-
-    // delete blobs that are fully enclosed by larger blobs
-    for (i=0, ii=0, invalid=0; i<numBlobs; i++, ii+=5)
-    {
-        if (blobs[ii+0]==0)
-            continue;
-        left0 = blobs[ii+1];
-        right0 = blobs[ii+2];
-        top0 = blobs[ii+3];
-        bottom0 = blobs[ii+4];
-
-        for (j=i+1, jj=ii+5; j<numBlobs; j++, jj+=5)
-        {
-            if (blobs[jj+0]==0)
-                continue;
-            left = blobs[jj+1];
-            right = blobs[jj+2];
-            top = blobs[jj+3];
-            bottom = blobs[jj+4];
-
-            if (left0<=left && right0>=right && top0<=top && bottom0>=bottom)
-            {
-                blobs[jj+0] = 0; // invalidate
-                invalid++;
-            }
-            else if (left<=left0 && right>=right0 && top<=top0 && bottom>=bottom0)
-            {
-                blobs[ii+0] = 0; // invalidate
-                invalid++;
-            }
-        }
-    }
-
-    return invalid;
-}
-
-uint16_t Blobs::combine2(uint16_t *blobs, uint16_t numBlobs)
-{
-    uint16_t i, j, ii, jj, left0, right0, top0, bottom0;
-    uint16_t left, right, top, bottom;
-    uint16_t invalid;
-
-    for (i=0, ii=0, invalid=0; i<numBlobs; i++, ii+=5)
-    {
-        if (blobs[ii+0]==0)
-            continue;
-        left0 = blobs[ii+1];
-        right0 = blobs[ii+2];
-        top0 = blobs[ii+3];
-        bottom0 = blobs[ii+4];
-
-        for (j=i+1, jj=ii+5; j<numBlobs; j++, jj+=5)
-        {
-            if (blobs[jj+0]==0)
-                continue;
-            left = blobs[jj+1];
-            right = blobs[jj+2];
-            top = blobs[jj+3];
-            bottom = blobs[jj+4];
-
-#if 1 // if corners touch....
-            if (left<=left0 && left0-right<=m_mergeDist &&
-                    ((top0<=top && top<=bottom0) || (top0<=bottom && bottom<=bottom0)))
-            {
-                blobs[ii+1] = left;
-                blobs[jj+0] = 0; // invalidate
-                invalid++;
-            }
-            else if (right>=right0 && left-right0<=m_mergeDist &&
-                     ((top0<=top && top<=bottom0) || (top0<=bottom && bottom<=bottom0)))
-            {
-                blobs[ii+2] = right;
-                blobs[jj+0] = 0; // invalidate
-                invalid++;
-            }
-            else if (top<=top0 && top0-bottom<=m_mergeDist &&
-                     ((left0<=left && left<=right0) || (left0<=right && right<=right0)))
-            {
-                blobs[ii+3] = top;
-                blobs[jj+0] = 0; // invalidate
-                invalid++;
-            }
-            else if (bottom>=bottom0 && top-bottom0<=m_mergeDist &&
-                     ((left0<=left && left<=right0) || (left0<=right && right<=right0)))
-            {
-                blobs[ii+4] = bottom;
-                blobs[jj+0] = 0; // invalidate
-                invalid++;
-            }
-#else // at least half of a side (the smaller adjacent side) has to overlap
-            if (left<=left0 && left0-right<=m_mergeDist &&
-                    ((top<=top0 && top0<=top+height) || (top+height<=bottom0 && bottom0<=bottom)))
-            {
-                blobs[ii+1] = left;
-                blobs[jj+0] = 0; // invalidate
-                invalid++;
-            }
-            else if (right>=right0 && left-right0<=m_mergeDist &&
-                     ((top<=top0 && top0<=top+height) || (top+height<=bottom0 && bottom0<=bottom)))
-            {
-                blobs[ii+2] = right;
-                blobs[jj+0] = 0; // invalidate
-                invalid++;
-            }
-            else if (top<=top0 && top0-bottom<=m_mergeDist &&
-                     ((left<=left0 && left0<=left+width) || (left+width<=right0 && right0<=right)))
-            {
-                blobs[ii+3] = top;
-                blobs[jj+0] = 0; // invalidate
-                invalid++;
-            }
-            else if (bottom>=bottom0 && top-bottom0<=m_mergeDist &&
-                     ((left<=left0 && left0<=left+width) || (left+width<=right0 && right0<=right)))
-            {
-                blobs[ii+4] = bottom;
-                blobs[jj+0] = 0; // invalidate
-                invalid++;
-            }
-#endif
-        }
-    }
-
-    return invalid;
-}
-
-int16_t Blobs::distance(BlobA *blob0, BlobA *blob1)
-{
-    int16_t left0, right0, top0, bottom0;
-    int16_t left1, right1, top1, bottom1;
-
-    left0 = blob0->m_left;
-    right0 = blob0->m_right;
-    top0 = blob0->m_top;
-    bottom0 = blob0->m_bottom;
-    left1 = blob1->m_left;
-    right1 = blob1->m_right;
-    top1 = blob1->m_top;
-    bottom1 = blob1->m_bottom;
-
-    if (left0>=left1 && ((top0<=top1 && top1<=bottom0) || (top0<=bottom1 && (bottom1<=bottom0 || top1<=top0))))
-        return left0-right1;
-
-    if (left1>=left0 && ((top0<=top1 && top1<=bottom0) || (top0<=bottom1 && (bottom1<=bottom0 || top1<=top0))))
-        return left1-right0;
-
-    if (top0>=top1 && ((left0<=left1 && left1<=right0) || (left0<=right1 && (right1<=right0 || left1<=left0))))
-        return top0-bottom1;
-
-    if (top1>=top0 && ((left0<=left1 && left1<=right0) || (left0<=right1 && (right1<=right0 || left1<=left0))))
-        return top1-bottom0;
-
-    return 0x7fff; // return a large number
-}
-
-bool Blobs::closeby(BlobA *blob0, BlobA *blob1)
-{
-    // check to see if blobs are invalid or equal
-    if (blob0->m_model==0 || blob1->m_model==0 || blob0->m_model==blob1->m_model)
-        return false;
-    // check to see that the blobs are from color code models.  If they aren't both
-    // color code blobs, we return false
-    if (!CC_SIGNATURE(blob0->m_model&0x07) || !CC_SIGNATURE(blob1->m_model&0x07))
-        return false;
-
-    return distance(blob0, blob1)<=m_maxCodedDist;
-}
-
-int16_t Blobs::distance(BlobA *blob0, BlobA *blob1, bool horiz)
-{
-    int16_t dist;
-
-    if (horiz)
-        dist = (blob0->m_right+blob0->m_left)/2 - (blob1->m_right+blob1->m_left)/2;
-    else
-        dist = (blob0->m_bottom+blob0->m_top)/2 - (blob1->m_bottom+blob1->m_top)/2;
-
-    if (dist<0)
-        return -dist;
-    else
-        return dist;
-}
-
-int16_t Blobs::angle(BlobA *blob0, BlobA *blob1)
-{
-    int acx, acy, bcx, bcy;
-    float res;
-
-    acx = (blob0->m_right + blob0->m_left)/2;
-    acy = (blob0->m_bottom + blob0->m_top)/2;
-    bcx = (blob1->m_right + blob1->m_left)/2;
-    bcy = (blob1->m_bottom + blob1->m_top)/2;
-
-    res = atan2((float)(acy-bcy), (float)(bcx-acx))*180/3.1415f;
-
-    return (int16_t)res;
-}
-
-void Blobs::sort(BlobA *blobs[], uint16_t len, BlobA *firstBlob, bool horiz)
-{
-    uint16_t i, td, distances[MAX_COLOR_CODE_MODELS*2];
-    bool done;
-    BlobA *tb;
-
-    // create list of distances
-    for (i=0; i<len && i<MAX_COLOR_CODE_MODELS*2; i++)
-        distances[i] = distance(firstBlob, blobs[i], horiz);
-
-    // sort -- note, we only have 5 maximum to sort, so no worries about efficiency
-    while(1)
-    {
-        for (i=1, done=true; i<len && i<MAX_COLOR_CODE_MODELS*2; i++)
-        {
-            if (distances[i-1]>distances[i])
-            {
-                // swap distances
-                td = distances[i];
-                distances[i] = distances[i-1];
-                distances[i-1] = td;
-                // swap blobs
-                tb = blobs[i];
-                blobs[i] = blobs[i-1];
-                blobs[i-1] = tb;
-
-                done = false;
-            }
-        }
-        if (done)
-            break;
-    }
-}
-
-bool Blobs::analyzeDistances(BlobA *blobs0[], int16_t numBlobs0, BlobA *blobs[], int16_t numBlobs, BlobA **blobA, BlobA **blobB)
-{
-    bool skip;
-    bool result = false;
-    int16_t dist, minDist, i, j, k;
-
-    for (i=0, minDist=0x7fff; i<numBlobs0; i++)
-    {
-        for (j=0; j<numBlobs; j++)
-        {
-            for (k=0, skip=false; k<numBlobs0; k++)
-            {
-                if (blobs0[k]==blobs[j] || (blobs0[k]->m_model&0x07)==(blobs[j]->m_model&0x07))
-                {
-                    skip = true;
-                    break;
-                }
-            }
-            if (skip)
-                continue;
-            dist = distance(blobs0[i], blobs[j]);
-            if (dist<minDist)
-            {
-                minDist = dist;
-                *blobA = blobs0[i];
-                *blobB = blobs[j];
-                result = true;
-            }
-        }
-    }
-#ifndef PIXY
-    if (!result)
-        DBG("not set!");
-#endif
-    return result;
-}
-
-#define TOL  400
-
-// impose weak size constraint
-void Blobs::cleanup(BlobA *blobs[], int16_t *numBlobs)
-{
-    int i, j;
-    bool set;
-    uint16_t maxEqual, numEqual, numNewBlobs;
-    BlobA *newBlobs[MAX_COLOR_CODE_MODELS*2];
-    uint32_t area0, area1, lowerArea, upperArea, maxEqualArea;
-
-    for (i=0, maxEqual=0, set=false; i<*numBlobs; i++)
-    {
-        area0 = (blobs[i]->m_right-blobs[i]->m_left) * (blobs[i]->m_bottom-blobs[i]->m_top);
-        lowerArea = (area0*100)/(100+TOL);
-        upperArea = area0 + (area0*TOL)/100;
-
-        for (j=0, numEqual=0; j<*numBlobs; j++)
-        {
-            if (i==j)
-                continue;
-            area1 = (blobs[j]->m_right-blobs[j]->m_left) * (blobs[j]->m_bottom-blobs[j]->m_top);
-            if (lowerArea<=area1 && area1<=upperArea)
-                numEqual++;
-        }
-        if (numEqual>maxEqual)
-        {
-            maxEqual = numEqual;
-            maxEqualArea = area0;
-            set = true;
-        }
-    }
-
-    if (!set)
-        *numBlobs = 0;
-
-    for (i=0, numNewBlobs=0; i<*numBlobs && numNewBlobs<MAX_COLOR_CODE_MODELS*2; i++)
-    {
-        area0 = (blobs[i]->m_right-blobs[i]->m_left) * (blobs[i]->m_bottom-blobs[i]->m_top);
-        lowerArea = (area0*100)/(100+TOL);
-        upperArea = area0 + (area0*TOL)/100;
-        if (lowerArea<=maxEqualArea && maxEqualArea<=upperArea)
-            newBlobs[numNewBlobs++] = blobs[i];
-#ifndef PIXY
-        else if (*numBlobs>=5 && (blobs[i]->m_model&0x07)==2)
-            DBG("eliminated!");
-#endif
-    }
-
-    // copy new blobs over
-    for (i=0; i<numNewBlobs; i++)
-        blobs[i] = newBlobs[i];
-    *numBlobs = numNewBlobs;
-}
-
-
-// eliminate duplicate and adjacent signatures
-void Blobs::cleanup2(BlobA *blobs[], int16_t *numBlobs)
-{
-    BlobA *newBlobs[MAX_COLOR_CODE_MODELS*2];
-    int i, j;
-    uint16_t numNewBlobs;
-    bool set;
-
-    for (i=0, numNewBlobs=0, set=false; i<*numBlobs && numNewBlobs<MAX_COLOR_CODE_MODELS*2; i=j)
-    {
-        newBlobs[numNewBlobs++] = blobs[i];
-        for (j=i+1; j<*numBlobs; j++)
-        {
-            if ((blobs[j]->m_model&0x07)==(blobs[i]->m_model&0x07))
-                set = true;
-            else
-                break;
-        }
-    }
-    if (set)
-    {
-        // copy new blobs over
-        for (i=0; i<numNewBlobs; i++)
-            blobs[i] = newBlobs[i];
-        *numBlobs = numNewBlobs;
-    }
-}
-
-
-void Blobs::printBlobs()
-{
-#ifndef PIXY
-    int i;
-    BlobA *blobs = (BlobA *)m_blobs;
-    for (i=0; i<m_numBlobs; i++)
-        DBG("blob %d: %d %d %d %d %d", i, blobs[i].m_model, blobs[i].m_left, blobs[i].m_right, blobs[i].m_top, blobs[i].m_bottom);
-#endif
-}
-
-void Blobs::mergeClumps(uint16_t scount0, uint16_t scount1)
-{
-    int i;
-    BlobA *blobs = (BlobA *)m_blobs;
-    for (i=0; i<m_numBlobs; i++)
-    {
-        if ((blobs[i].m_model&~0x07)==scount1)
-            blobs[i].m_model = (blobs[i].m_model&0x07) | scount0;
-    }
-}
-
-void Blobs::processCC()
-{
-    int16_t i, j, k;
-    uint16_t scount, scount1, count = 0;
-    int16_t left, right, top, bottom;
-    uint16_t codedModel0, codedModel;
-    int32_t width, height, avgWidth, avgHeight;
-    BlobB *codedBlob, *endBlobB;
-    BlobA *blob0, *blob1, *endBlob;
-    BlobA *blobs[MAX_COLOR_CODE_MODELS*2];
-
-#if 0
-    BlobA b0(1, 1, 20, 40, 50);
-    BlobA b1(1, 1, 20, 52, 60);
-    BlobA b2(1, 1, 20, 62, 70);
-    BlobA b3(2, 22, 30, 40, 50);
-    BlobA b4(2, 22, 30, 52, 60);
-    BlobA b5(3, 32, 40, 40, 50);
-    BlobA b6(4, 42, 50, 40, 50);
-    BlobA b7(4, 42, 50, 52, 60);
-    BlobA b8(6, 22, 30, 52, 60);
-    BlobA b9(6, 22, 30, 52, 60);
-    BlobA b10(7, 22, 30, 52, 60);
-
-    BlobA *testBlobs[] =
-    {
-        &b0, &b1, &b2, &b3, &b4, &b5, &b6, &b7 //, &b8, &b9, &b10
-    };
-    int16_t ntb = 8;
-    cleanup(testBlobs, &ntb);
-#endif
-
-    endBlob = (BlobA *)m_blobs + m_numBlobs;
-
-    // 1st pass: mark all closeby blobs
-    for (blob0=(BlobA *)m_blobs; blob0<endBlob; blob0++)
-    {
-        for (blob1=(BlobA *)blob0+1; blob1<endBlob; blob1++)
-        {
-            if (closeby(blob0, blob1))
-            {
-                if (blob0->m_model<=CL_NUM_SIGNATURES && blob1->m_model<=CL_NUM_SIGNATURES)
-                {
-                    count++;
-                    scount = count<<3;
-                    blob0->m_model |= scount;
-                    blob1->m_model |= scount;
-                }
-                else if (blob0->m_model>CL_NUM_SIGNATURES && blob1->m_model<=CL_NUM_SIGNATURES)
-                {
-                    scount = blob0->m_model & ~0x07;
-                    blob1->m_model |= scount;
-                }
-                else if (blob1->m_model>CL_NUM_SIGNATURES && blob0->m_model<=CL_NUM_SIGNATURES)
-                {
-                    scount = blob1->m_model & ~0x07;
-                    blob0->m_model |= scount;
-                }
-            }
-        }
-    }
-
-#if 1
-    // 2nd pass: merge blob clumps
-    for (blob0=(BlobA *)m_blobs; blob0<endBlob; blob0++)
-    {
-        if (blob0->m_model<=CL_NUM_SIGNATURES) // skip normal blobs
-            continue;
-        scount = blob0->m_model&~0x07;
-        for (blob1=(BlobA *)blob0+1; blob1<endBlob; blob1++)
-        {
-            if (blob1->m_model<=CL_NUM_SIGNATURES)
-                continue;
-
-            scount1 = blob1->m_model&~0x07;
-            if (scount!=scount1 && closeby(blob0, blob1))
-                mergeClumps(scount, scount1);
-        }
-    }
-#endif
-
-    // 3rd and final pass, find each blob clean it up and add it to the table
-    endBlobB = (BlobB *)((BlobA *)m_blobs + MAX_BLOBS)-1;
-    for (i=1, codedBlob = m_ccBlobs, m_numCCBlobs=0; i<=count && codedBlob<endBlobB; i++)
-    {
-        scount = i<<3;
-        // find all blobs with index i
-        for (j=0, blob0=(BlobA *)m_blobs; blob0<endBlob && j<MAX_COLOR_CODE_MODELS*2; blob0++)
-        {
-            if ((blob0->m_model&~0x07)==scount)
-                blobs[j++] = blob0;
-        }
-
-#if 1
-        // cleanup blobs, deal with cases where there are more blobs than models
-        cleanup(blobs, &j);
-#endif
-
-        if (j<2)
-            continue;
-
-        // find left, right, top, bottom of color coded block
-        for (k=0, left=right=top=bottom=avgWidth=avgHeight=0; k<j; k++)
-        {
-            //DBG("* cc %x %d i %d: %d %d %d %d %d", blobs[k], m_numCCBlobs, k, blobs[k]->m_model, blobs[k]->m_left, blobs[k]->m_right, blobs[k]->m_top, blobs[k]->m_bottom);
-            if (blobs[left]->m_left > blobs[k]->m_left)
-                left = k;
-            if (blobs[top]->m_top > blobs[k]->m_top)
-                top = k;
-            if (blobs[right]->m_right < blobs[k]->m_right)
-                right = k;
-            if (blobs[bottom]->m_bottom < blobs[k]->m_bottom)
-                bottom = k;
-            avgWidth += blobs[k]->m_right - blobs[k]->m_left;
-            avgHeight += blobs[k]->m_bottom - blobs[k]->m_top;
-        }
-        avgWidth /= j;
-        avgHeight /= j;
-        codedBlob->m_left = blobs[left]->m_left;
-        codedBlob->m_right = blobs[right]->m_right;
-        codedBlob->m_top = blobs[top]->m_top;
-        codedBlob->m_bottom = blobs[bottom]->m_bottom;
-
-#if 1
-        // is it more horizontal than vertical?
-        width = (blobs[right]->m_right - blobs[left]->m_left)*100;
-        width /= avgWidth; // scale by average width because our swatches might not be square
-        height = (blobs[bottom]->m_bottom - blobs[top]->m_top)*100;
-        height /= avgHeight; // scale by average height because our swatches might not be square
-
-        if (width > height)
-            sort(blobs, j, blobs[left], true);
-        else
-            sort(blobs, j, blobs[top], false);
-
-#if 1
-        cleanup2(blobs, &j);
-        if (j<2)
-            continue;
-        else if (j>5)
-            j = 5;
-#endif
-        // create new blob, compare the coded models, pick the smaller one
-        for (k=0, codedModel0=0; k<j; k++)
-        {
-            codedModel0 <<= 3;
-            codedModel0 |= blobs[k]->m_model&0x07;
-        }
-        for (k=j-1, codedModel=0; k>=0; k--)
-        {
-            codedModel <<= 3;
-            codedModel |= blobs[k]->m_model&0x07;
-            blobs[k]->m_model = 0; // invalidate
-        }
-
-        if (codedModel0<codedModel)
-        {
-            codedBlob->m_model = codedModel0;
-            codedBlob->m_angle = angle(blobs[0], blobs[j-1]);
-        }
-        else
-        {
-            codedBlob->m_model = codedModel;
-            codedBlob->m_angle = angle(blobs[j-1], blobs[0]);
-        }
-#endif
-        //DBG("cc %d %d %d %d %d", m_numCCBlobs, codedBlob->m_left, codedBlob->m_right, codedBlob->m_top, codedBlob->m_bottom);
-        codedBlob++;
-        m_numCCBlobs++;
-    }
-
-    // 3rd pass, invalidate blobs
-    for (blob0=(BlobA *)m_blobs; blob0<endBlob; blob0++)
-    {
-        if (m_ccMode==MIXED)
-        {
-            if (blob0->m_model>CL_NUM_SIGNATURES)
-                blob0->m_model = 0;
-        }
-        else if (blob0->m_model>CL_NUM_SIGNATURES || CC_SIGNATURE(blob0->m_model))
-            blob0->m_model = 0; // invalidate-- not part of a color code
-    }
-}
-
-void Blobs::endFrame()
-{
-    int i;
-    for (i=0; i<CL_NUM_SIGNATURES; i++)
-    {
-        m_assembler[i].EndFrame();
-        m_assembler[i].SortFinished();
-    }
-}
-
+		return maxBlob;
+    }
+
+    return NULL; // no blobs...
+}
+
+static int compareBlobAreas(const void *a, const void *b)
+{
+	BlobA *ia = (struct BlobA *)a;
+	BlobA *ib = (struct BlobA *)b;
+	int areaA = (ia->m_right - ia->m_left)*(ia->m_bottom - ia->m_top);
+	int areaB = (ib->m_right - ib->m_left)*(ib->m_bottom - ib->m_top);
+	if (areaA < areaB)
+	  return -1;
+    else if (areaA > areaB)
+	  return 1;
+    else
+	  return 0;
+
+}
+
+void Blobs::getMaxBlobs(uint16_t signature, uint16_t maxNumBlobs, BlobA **maxBlobs, uint16_t *numBlobs)
+{
+    int i;
+	uint16_t blobs;
+    BlobA *blob;
+	BlobB *ccBlob;
+	BlobA *ftcblob;
+
+	if (m_mutex){
+		*maxBlobs = (BlobA *)-1;
+		*numBlobs =  0;
+		return;
+	}
+
+    if (signature==0) // 0 means return the top maxNumBlobs largest area of and signature number <= 7
+    {
+    	//sort by Area
+    	qsort(m_blobs, m_numBlobs, sizeof (BlobA), &compareBlobAreas);
+		for (i=0; i<maxNumBlobs && i<m_numBlobs; i++)
+		{
+			blob = (BlobA *)m_blobs + i;
+			ftcblob = (BlobA *)m_ftcblobs + i;
+			ftcblob->m_model = blob->m_model;
+			ftcblob->m_left = blob->m_left;
+			ftcblob->m_right = blob->m_right;
+			ftcblob->m_top = blob->m_top;
+			ftcblob->m_bottom = blob->m_bottom;
+
+		}
+
+		*maxBlobs = (BlobA *)m_ftcblobs;
+		*numBlobs =  m_numBlobs;
+		return;
+    }else if (signature==8) // 8 means return the top maxNumBlobs largest area of all cc signature number > 7
+    {
+    	//sort by Area
+    	qsort(m_ccBlobs, m_numCCBlobs, sizeof (BlobB), &compareBlobAreas);
+		for (i=0; i<maxNumBlobs && i<m_numCCBlobs; i++)
+		{
+			//ccBlob = (BlobB *)m_ccBlobs + i;
+			blob = (BlobA *)m_ccBlobs + i;
+			ftcblob = (BlobA *)m_ftcblobs + i;
+			ftcblob->m_model = blob->m_model;
+			ftcblob->m_left = blob->m_left;
+			ftcblob->m_right = blob->m_right;
+			ftcblob->m_top = blob->m_top;
+			ftcblob->m_bottom = blob->m_bottom;
+		}
+		*maxBlobs = (BlobA *)m_ftcblobs;
+		*numBlobs =  m_numCCBlobs;
+		return;
+    }
+	// look for a specific signature
+    else
+    {
+		// regular signatures
+		if (signature<=CL_NUM_SIGNATURES)
+		{
+			qsort(m_blobs, m_numBlobs, sizeof (BlobA), &compareBlobAreas);
+        	for (i=0, blobs=0; i<m_numBlobs; i++)
+        	{
+            	blob = (BlobA *)m_blobs + i;
+           		if (blob->m_model==signature)
+				{
+					if (blobs < maxNumBlobs){
+						ftcblob = (BlobA *)m_ftcblobs + i;
+						ftcblob->m_model = blob->m_model;
+						ftcblob->m_left = blob->m_left;
+						ftcblob->m_right = blob->m_right;
+						ftcblob->m_top = blob->m_top;
+						ftcblob->m_bottom = blob->m_bottom;
+					}
+	 				blobs++;  // count
+				}
+        	}
+
+		}
+		// color code
+		else
+		{
+			qsort(m_ccBlobs, m_numCCBlobs, sizeof (BlobB), &compareBlobAreas);
+        	for (i=0, blobs=0; i<m_numCCBlobs; i++)
+			{
+            	ccBlob = (BlobB *)m_ccBlobs + i;
+           		if (ccBlob->m_model==signature)
+				{
+           			if (blobs < maxNumBlobs){
+           				ftcblob = (BlobA *)m_ftcblobs + i;
+           				ftcblob->m_model = ccBlob->m_model;
+           				ftcblob->m_left = ccBlob->m_left;
+           				ftcblob->m_right = ccBlob->m_right;
+           				ftcblob->m_top = ccBlob->m_top;
+           				ftcblob->m_bottom = ccBlob->m_bottom;
+           			}
+	 				blobs++;  // count
+				}
+			}
+        }
+		*maxBlobs = (BlobA *)m_ftcblobs;
+		*numBlobs = blobs;
+		return;
+    }
+
+    *maxBlobs = (BlobA *)0;
+    *numBlobs =  0;
+    return;
+}
+
+
+
+void Blobs::getBlobs(BlobA **blobs, uint32_t *len, BlobB **ccBlobs, uint32_t *ccLen)
+{
+    *blobs = (BlobA *)m_blobs;
+    *len = m_numBlobs;
+
+    *ccBlobs = m_ccBlobs;
+    *ccLen = m_numCCBlobs;
+}
+
+
+
+uint16_t Blobs::compress(uint16_t *blobs, uint16_t numBlobs)
+{
+    uint16_t i, ii;
+    uint16_t *destination, invalid;
+
+    // compress list
+    for (i=0, ii=0, destination=NULL, invalid=0; i<numBlobs; i++, ii+=5)
+    {
+        if (blobs[ii+0]==0)
+        {
+            if (destination==NULL)
+                destination = blobs+ii;
+            invalid++;
+            continue;
+        }
+        if (destination)
+        {
+            destination[0] = blobs[ii+0];
+            destination[1] = blobs[ii+1];
+            destination[2] = blobs[ii+2];
+            destination[3] = blobs[ii+3];
+            destination[4] = blobs[ii+4];
+            destination += 5;
+        }
+    }
+    return invalid;
+}
+
+uint16_t Blobs::combine(uint16_t *blobs, uint16_t numBlobs)
+{
+    uint16_t i, j, ii, jj, left0, right0, top0, bottom0;
+    uint16_t left, right, top, bottom;
+    uint16_t invalid;
+
+    // delete blobs that are fully enclosed by larger blobs
+    for (i=0, ii=0, invalid=0; i<numBlobs; i++, ii+=5)
+    {
+        if (blobs[ii+0]==0)
+            continue;
+        left0 = blobs[ii+1];
+        right0 = blobs[ii+2];
+        top0 = blobs[ii+3];
+        bottom0 = blobs[ii+4];
+
+        for (j=i+1, jj=ii+5; j<numBlobs; j++, jj+=5)
+        {
+            if (blobs[jj+0]==0)
+                continue;
+            left = blobs[jj+1];
+            right = blobs[jj+2];
+            top = blobs[jj+3];
+            bottom = blobs[jj+4];
+
+            if (left0<=left && right0>=right && top0<=top && bottom0>=bottom)
+            {
+                blobs[jj+0] = 0; // invalidate
+                invalid++;
+            }
+            else if (left<=left0 && right>=right0 && top<=top0 && bottom>=bottom0)
+            {
+                blobs[ii+0] = 0; // invalidate
+                invalid++;
+            }
+        }
+    }
+
+    return invalid;
+}
+
+uint16_t Blobs::combine2(uint16_t *blobs, uint16_t numBlobs)
+{
+    uint16_t i, j, ii, jj, left0, right0, top0, bottom0;
+    uint16_t left, right, top, bottom;
+    uint16_t invalid;
+
+    for (i=0, ii=0, invalid=0; i<numBlobs; i++, ii+=5)
+    {
+        if (blobs[ii+0]==0)
+            continue;
+        left0 = blobs[ii+1];
+        right0 = blobs[ii+2];
+        top0 = blobs[ii+3];
+        bottom0 = blobs[ii+4];
+
+        for (j=i+1, jj=ii+5; j<numBlobs; j++, jj+=5)
+        {
+            if (blobs[jj+0]==0)
+                continue;
+            left = blobs[jj+1];
+            right = blobs[jj+2];
+            top = blobs[jj+3];
+            bottom = blobs[jj+4];
+
+#if 1 // if corners touch....
+            if (left<=left0 && left0-right<=m_mergeDist &&
+                    ((top0<=top && top<=bottom0) || (top0<=bottom && bottom<=bottom0)))
+            {
+                blobs[ii+1] = left;
+                blobs[jj+0] = 0; // invalidate
+                invalid++;
+            }
+            else if (right>=right0 && left-right0<=m_mergeDist &&
+                     ((top0<=top && top<=bottom0) || (top0<=bottom && bottom<=bottom0)))
+            {
+                blobs[ii+2] = right;
+                blobs[jj+0] = 0; // invalidate
+                invalid++;
+            }
+            else if (top<=top0 && top0-bottom<=m_mergeDist &&
+                     ((left0<=left && left<=right0) || (left0<=right && right<=right0)))
+            {
+                blobs[ii+3] = top;
+                blobs[jj+0] = 0; // invalidate
+                invalid++;
+            }
+            else if (bottom>=bottom0 && top-bottom0<=m_mergeDist &&
+                     ((left0<=left && left<=right0) || (left0<=right && right<=right0)))
+            {
+                blobs[ii+4] = bottom;
+                blobs[jj+0] = 0; // invalidate
+                invalid++;
+            }
+#else // at least half of a side (the smaller adjacent side) has to overlap
+            if (left<=left0 && left0-right<=m_mergeDist &&
+                    ((top<=top0 && top0<=top+height) || (top+height<=bottom0 && bottom0<=bottom)))
+            {
+                blobs[ii+1] = left;
+                blobs[jj+0] = 0; // invalidate
+                invalid++;
+            }
+            else if (right>=right0 && left-right0<=m_mergeDist &&
+                     ((top<=top0 && top0<=top+height) || (top+height<=bottom0 && bottom0<=bottom)))
+            {
+                blobs[ii+2] = right;
+                blobs[jj+0] = 0; // invalidate
+                invalid++;
+            }
+            else if (top<=top0 && top0-bottom<=m_mergeDist &&
+                     ((left<=left0 && left0<=left+width) || (left+width<=right0 && right0<=right)))
+            {
+                blobs[ii+3] = top;
+                blobs[jj+0] = 0; // invalidate
+                invalid++;
+            }
+            else if (bottom>=bottom0 && top-bottom0<=m_mergeDist &&
+                     ((left<=left0 && left0<=left+width) || (left+width<=right0 && right0<=right)))
+            {
+                blobs[ii+4] = bottom;
+                blobs[jj+0] = 0; // invalidate
+                invalid++;
+            }
+#endif
+        }
+    }
+
+    return invalid;
+}
+
+int16_t Blobs::distance(BlobA *blob0, BlobA *blob1)
+{
+    int16_t left0, right0, top0, bottom0;
+    int16_t left1, right1, top1, bottom1;
+
+    left0 = blob0->m_left;
+    right0 = blob0->m_right;
+    top0 = blob0->m_top;
+    bottom0 = blob0->m_bottom;
+    left1 = blob1->m_left;
+    right1 = blob1->m_right;
+    top1 = blob1->m_top;
+    bottom1 = blob1->m_bottom;
+
+    if (left0>=left1 && ((top0<=top1 && top1<=bottom0) || (top0<=bottom1 && (bottom1<=bottom0 || top1<=top0))))
+        return left0-right1;
+
+    if (left1>=left0 && ((top0<=top1 && top1<=bottom0) || (top0<=bottom1 && (bottom1<=bottom0 || top1<=top0))))
+        return left1-right0;
+
+    if (top0>=top1 && ((left0<=left1 && left1<=right0) || (left0<=right1 && (right1<=right0 || left1<=left0))))
+        return top0-bottom1;
+
+    if (top1>=top0 && ((left0<=left1 && left1<=right0) || (left0<=right1 && (right1<=right0 || left1<=left0))))
+        return top1-bottom0;
+
+    return 0x7fff; // return a large number
+}
+
+bool Blobs::closeby(BlobA *blob0, BlobA *blob1)
+{
+    // check to see if blobs are invalid or equal
+    if (blob0->m_model==0 || blob1->m_model==0 || blob0->m_model==blob1->m_model)
+        return false;
+    // check to see that the blobs are from color code models.  If they aren't both
+    // color code blobs, we return false
+    if (!CC_SIGNATURE(blob0->m_model&0x07) || !CC_SIGNATURE(blob1->m_model&0x07))
+        return false;
+
+    return distance(blob0, blob1)<=m_maxCodedDist;
+}
+
+int16_t Blobs::distance(BlobA *blob0, BlobA *blob1, bool horiz)
+{
+    int16_t dist;
+
+    if (horiz)
+        dist = (blob0->m_right+blob0->m_left)/2 - (blob1->m_right+blob1->m_left)/2;
+    else
+        dist = (blob0->m_bottom+blob0->m_top)/2 - (blob1->m_bottom+blob1->m_top)/2;
+
+    if (dist<0)
+        return -dist;
+    else
+        return dist;
+}
+
+int16_t Blobs::angle(BlobA *blob0, BlobA *blob1)
+{
+    int acx, acy, bcx, bcy;
+    float res;
+
+    acx = (blob0->m_right + blob0->m_left)/2;
+    acy = (blob0->m_bottom + blob0->m_top)/2;
+    bcx = (blob1->m_right + blob1->m_left)/2;
+    bcy = (blob1->m_bottom + blob1->m_top)/2;
+
+    res = atan2((float)(acy-bcy), (float)(bcx-acx))*180/3.1415f;
+
+    return (int16_t)res;
+}
+
+void Blobs::sort(BlobA *blobs[], uint16_t len, BlobA *firstBlob, bool horiz)
+{
+    uint16_t i, td, distances[MAX_COLOR_CODE_MODELS*2];
+    bool done;
+    BlobA *tb;
+
+    // create list of distances
+    for (i=0; i<len && i<MAX_COLOR_CODE_MODELS*2; i++)
+        distances[i] = distance(firstBlob, blobs[i], horiz);
+
+    // sort -- note, we only have 5 maximum to sort, so no worries about efficiency
+    while(1)
+    {
+        for (i=1, done=true; i<len && i<MAX_COLOR_CODE_MODELS*2; i++)
+        {
+            if (distances[i-1]>distances[i])
+            {
+                // swap distances
+                td = distances[i];
+                distances[i] = distances[i-1];
+                distances[i-1] = td;
+                // swap blobs
+                tb = blobs[i];
+                blobs[i] = blobs[i-1];
+                blobs[i-1] = tb;
+
+                done = false;
+            }
+        }
+        if (done)
+            break;
+    }
+}
+
+bool Blobs::analyzeDistances(BlobA *blobs0[], int16_t numBlobs0, BlobA *blobs[], int16_t numBlobs, BlobA **blobA, BlobA **blobB)
+{
+    bool skip;
+    bool result = false;
+    int16_t dist, minDist, i, j, k;
+
+    for (i=0, minDist=0x7fff; i<numBlobs0; i++)
+    {
+        for (j=0; j<numBlobs; j++)
+        {
+            for (k=0, skip=false; k<numBlobs0; k++)
+            {
+                if (blobs0[k]==blobs[j] || (blobs0[k]->m_model&0x07)==(blobs[j]->m_model&0x07))
+                {
+                    skip = true;
+                    break;
+                }
+            }
+            if (skip)
+                continue;
+            dist = distance(blobs0[i], blobs[j]);
+            if (dist<minDist)
+            {
+                minDist = dist;
+                *blobA = blobs0[i];
+                *blobB = blobs[j];
+                result = true;
+            }
+        }
+    }
+#ifndef PIXY
+    if (!result)
+        DBG("not set!");
+#endif
+    return result;
+}
+
+#define TOL  400
+
+// impose weak size constraint
+void Blobs::cleanup(BlobA *blobs[], int16_t *numBlobs)
+{
+    int i, j;
+    bool set;
+    uint16_t maxEqual, numEqual, numNewBlobs;
+    BlobA *newBlobs[MAX_COLOR_CODE_MODELS*2];
+    uint32_t area0, area1, lowerArea, upperArea, maxEqualArea;
+
+    for (i=0, maxEqual=0, set=false; i<*numBlobs; i++)
+    {
+        area0 = (blobs[i]->m_right-blobs[i]->m_left) * (blobs[i]->m_bottom-blobs[i]->m_top);
+        lowerArea = (area0*100)/(100+TOL);
+        upperArea = area0 + (area0*TOL)/100;
+
+        for (j=0, numEqual=0; j<*numBlobs; j++)
+        {
+            if (i==j)
+                continue;
+            area1 = (blobs[j]->m_right-blobs[j]->m_left) * (blobs[j]->m_bottom-blobs[j]->m_top);
+            if (lowerArea<=area1 && area1<=upperArea)
+                numEqual++;
+        }
+        if (numEqual>maxEqual)
+        {
+            maxEqual = numEqual;
+            maxEqualArea = area0;
+            set = true;
+        }
+    }
+
+    if (!set)
+        *numBlobs = 0;
+
+    for (i=0, numNewBlobs=0; i<*numBlobs && numNewBlobs<MAX_COLOR_CODE_MODELS*2; i++)
+    {
+        area0 = (blobs[i]->m_right-blobs[i]->m_left) * (blobs[i]->m_bottom-blobs[i]->m_top);
+        lowerArea = (area0*100)/(100+TOL);
+        upperArea = area0 + (area0*TOL)/100;
+        if (lowerArea<=maxEqualArea && maxEqualArea<=upperArea)
+            newBlobs[numNewBlobs++] = blobs[i];
+#ifndef PIXY
+        else if (*numBlobs>=5 && (blobs[i]->m_model&0x07)==2)
+            DBG("eliminated!");
+#endif
+    }
+
+    // copy new blobs over
+    for (i=0; i<numNewBlobs; i++)
+        blobs[i] = newBlobs[i];
+    *numBlobs = numNewBlobs;
+}
+
+
+// eliminate duplicate and adjacent signatures
+void Blobs::cleanup2(BlobA *blobs[], int16_t *numBlobs)
+{
+    BlobA *newBlobs[MAX_COLOR_CODE_MODELS*2];
+    int i, j;
+    uint16_t numNewBlobs;
+    bool set;
+
+    for (i=0, numNewBlobs=0, set=false; i<*numBlobs && numNewBlobs<MAX_COLOR_CODE_MODELS*2; i=j)
+    {
+        newBlobs[numNewBlobs++] = blobs[i];
+        for (j=i+1; j<*numBlobs; j++)
+        {
+            if ((blobs[j]->m_model&0x07)==(blobs[i]->m_model&0x07))
+                set = true;
+            else
+                break;
+        }
+    }
+    if (set)
+    {
+        // copy new blobs over
+        for (i=0; i<numNewBlobs; i++)
+            blobs[i] = newBlobs[i];
+        *numBlobs = numNewBlobs;
+    }
+}
+
+
+void Blobs::printBlobs()
+{
+#ifndef PIXY
+    int i;
+    BlobA *blobs = (BlobA *)m_blobs;
+    for (i=0; i<m_numBlobs; i++)
+        DBG("blob %d: %d %d %d %d %d", i, blobs[i].m_model, blobs[i].m_left, blobs[i].m_right, blobs[i].m_top, blobs[i].m_bottom);
+#endif
+}
+
+void Blobs::mergeClumps(uint16_t scount0, uint16_t scount1)
+{
+    int i;
+    BlobA *blobs = (BlobA *)m_blobs;
+    for (i=0; i<m_numBlobs; i++)
+    {
+        if ((blobs[i].m_model&~0x07)==scount1)
+            blobs[i].m_model = (blobs[i].m_model&0x07) | scount0;
+    }
+}
+
+void Blobs::processCC()
+{
+    int16_t i, j, k;
+    uint16_t scount, scount1, count = 0;
+    int16_t left, right, top, bottom;
+    uint16_t codedModel0, codedModel;
+    int32_t width, height, avgWidth, avgHeight;
+    BlobB *codedBlob, *endBlobB;
+    BlobA *blob0, *blob1, *endBlob;
+    BlobA *blobs[MAX_COLOR_CODE_MODELS*2];
+
+#if 0
+    BlobA b0(1, 1, 20, 40, 50);
+    BlobA b1(1, 1, 20, 52, 60);
+    BlobA b2(1, 1, 20, 62, 70);
+    BlobA b3(2, 22, 30, 40, 50);
+    BlobA b4(2, 22, 30, 52, 60);
+    BlobA b5(3, 32, 40, 40, 50);
+    BlobA b6(4, 42, 50, 40, 50);
+    BlobA b7(4, 42, 50, 52, 60);
+    BlobA b8(6, 22, 30, 52, 60);
+    BlobA b9(6, 22, 30, 52, 60);
+    BlobA b10(7, 22, 30, 52, 60);
+
+    BlobA *testBlobs[] =
+    {
+        &b0, &b1, &b2, &b3, &b4, &b5, &b6, &b7 //, &b8, &b9, &b10
+    };
+    int16_t ntb = 8;
+    cleanup(testBlobs, &ntb);
+#endif
+
+    endBlob = (BlobA *)m_blobs + m_numBlobs;
+
+    // 1st pass: mark all closeby blobs
+    for (blob0=(BlobA *)m_blobs; blob0<endBlob; blob0++)
+    {
+        for (blob1=(BlobA *)blob0+1; blob1<endBlob; blob1++)
+        {
+            if (closeby(blob0, blob1))
+            {
+                if (blob0->m_model<=CL_NUM_SIGNATURES && blob1->m_model<=CL_NUM_SIGNATURES)
+                {
+                    count++;
+                    scount = count<<3;
+                    blob0->m_model |= scount;
+                    blob1->m_model |= scount;
+                }
+                else if (blob0->m_model>CL_NUM_SIGNATURES && blob1->m_model<=CL_NUM_SIGNATURES)
+                {
+                    scount = blob0->m_model & ~0x07;
+                    blob1->m_model |= scount;
+                }
+                else if (blob1->m_model>CL_NUM_SIGNATURES && blob0->m_model<=CL_NUM_SIGNATURES)
+                {
+                    scount = blob1->m_model & ~0x07;
+                    blob0->m_model |= scount;
+                }
+            }
+        }
+    }
+
+#if 1
+    // 2nd pass: merge blob clumps
+    for (blob0=(BlobA *)m_blobs; blob0<endBlob; blob0++)
+    {
+        if (blob0->m_model<=CL_NUM_SIGNATURES) // skip normal blobs
+            continue;
+        scount = blob0->m_model&~0x07;
+        for (blob1=(BlobA *)blob0+1; blob1<endBlob; blob1++)
+        {
+            if (blob1->m_model<=CL_NUM_SIGNATURES)
+                continue;
+
+            scount1 = blob1->m_model&~0x07;
+            if (scount!=scount1 && closeby(blob0, blob1))
+                mergeClumps(scount, scount1);
+        }
+    }
+#endif
+
+    // 3rd and final pass, find each blob clean it up and add it to the table
+    endBlobB = (BlobB *)((BlobA *)m_blobs + MAX_BLOBS)-1;
+    for (i=1, codedBlob = m_ccBlobs, m_numCCBlobs=0; i<=count && codedBlob<endBlobB; i++)
+    {
+        scount = i<<3;
+        // find all blobs with index i
+        for (j=0, blob0=(BlobA *)m_blobs; blob0<endBlob && j<MAX_COLOR_CODE_MODELS*2; blob0++)
+        {
+            if ((blob0->m_model&~0x07)==scount)
+                blobs[j++] = blob0;
+        }
+
+#if 1
+        // cleanup blobs, deal with cases where there are more blobs than models
+        cleanup(blobs, &j);
+#endif
+
+        if (j<2)
+            continue;
+
+        // find left, right, top, bottom of color coded block
+        for (k=0, left=right=top=bottom=avgWidth=avgHeight=0; k<j; k++)
+        {
+            //DBG("* cc %x %d i %d: %d %d %d %d %d", blobs[k], m_numCCBlobs, k, blobs[k]->m_model, blobs[k]->m_left, blobs[k]->m_right, blobs[k]->m_top, blobs[k]->m_bottom);
+            if (blobs[left]->m_left > blobs[k]->m_left)
+                left = k;
+            if (blobs[top]->m_top > blobs[k]->m_top)
+                top = k;
+            if (blobs[right]->m_right < blobs[k]->m_right)
+                right = k;
+            if (blobs[bottom]->m_bottom < blobs[k]->m_bottom)
+                bottom = k;
+            avgWidth += blobs[k]->m_right - blobs[k]->m_left;
+            avgHeight += blobs[k]->m_bottom - blobs[k]->m_top;
+        }
+        avgWidth /= j;
+        avgHeight /= j;
+        codedBlob->m_left = blobs[left]->m_left;
+        codedBlob->m_right = blobs[right]->m_right;
+        codedBlob->m_top = blobs[top]->m_top;
+        codedBlob->m_bottom = blobs[bottom]->m_bottom;
+
+#if 1
+        // is it more horizontal than vertical?
+        width = (blobs[right]->m_right - blobs[left]->m_left)*100;
+        width /= avgWidth; // scale by average width because our swatches might not be square
+        height = (blobs[bottom]->m_bottom - blobs[top]->m_top)*100;
+        height /= avgHeight; // scale by average height because our swatches might not be square
+
+        if (width > height)
+            sort(blobs, j, blobs[left], true);
+        else
+        	sort(blobs, j, blobs[top], false);
+
+#if 1
+        cleanup2(blobs, &j);
+        if (j<2)
+            continue;
+        else if (j>5)
+            j = 5;
+#endif
+        // create new blob, compare the coded models, pick the smaller one
+        for (k=0, codedModel0=0; k<j; k++)
+        {
+            codedModel0 <<= 3;
+            codedModel0 |= blobs[k]->m_model&0x07;
+        }
+        for (k=j-1, codedModel=0; k>=0; k--)
+        {
+            codedModel <<= 3;
+            codedModel |= blobs[k]->m_model&0x07;
+            blobs[k]->m_model = 0; // invalidate
+        }
+
+        if (codedModel0<codedModel)
+        {
+            codedBlob->m_model = codedModel0;
+            codedBlob->m_angle = angle(blobs[0], blobs[j-1]);
+        }
+        else
+        {
+            codedBlob->m_model = codedModel;
+            codedBlob->m_angle = angle(blobs[j-1], blobs[0]);
+        }
+#endif
+        //DBG("cc %d %d %d %d %d", m_numCCBlobs, codedBlob->m_left, codedBlob->m_right, codedBlob->m_top, codedBlob->m_bottom);
+        codedBlob++;
+        m_numCCBlobs++;
+    }
+
+    // 3rd pass, invalidate blobs
+    for (blob0=(BlobA *)m_blobs; blob0<endBlob; blob0++)
+    {
+        if (m_ccMode==MIXED)
+        {
+            if (blob0->m_model>CL_NUM_SIGNATURES)
+                blob0->m_model = 0;
+        }
+        else if (blob0->m_model>CL_NUM_SIGNATURES || CC_SIGNATURE(blob0->m_model))
+            blob0->m_model = 0; // invalidate-- not part of a color code
+    }
+}
+
+void Blobs::endFrame()
+{
+    int i;
+    for (i=0; i<CL_NUM_SIGNATURES; i++)
+    {
+        m_assembler[i].EndFrame();
+        m_assembler[i].SortFinished();
+    }
+}
+
diff --git a/src/device/build_this_first/CopyFiles.cmd b/src/device/build_this_first/CopyFiles.cmd
new file mode 100644
index 00000000..b219f234
--- /dev/null
+++ b/src/device/build_this_first/CopyFiles.cmd
@@ -0,0 +1,6 @@
+@Echo Off
+set sourcedir="D:\Users\adevries\sources\GitHub\pixy\src"
+copy %sourcedir%\device\common\src\*.*  %sourcedir%\device\libpixy_m0\src
+copy %sourcedir%\device\common\src\*.*  %sourcedir%\device\libpixy_m4\src
+copy %sourcedir%\common\src\*.*  %sourcedir%\device\libpixy_m4\src
+pause
\ No newline at end of file
diff --git a/src/device/main_m0/Release/main_m0_Release.ld b/src/device/main_m0/Release/main_m0_Release.ld
index 1aa5f8e7..324ba818 100755
--- a/src/device/main_m0/Release/main_m0_Release.ld
+++ b/src/device/main_m0/Release/main_m0_Release.ld
@@ -1,207 +1,87 @@
 /*
  * GENERATED FILE - DO NOT EDIT
- * (c) Code Red Technologies Ltd, 2008-13
- * (c) NXP Semiconductors 2013-2015
+ * (c) Code Red Technologies Ltd, 2008-2013
+ * (c) NXP Semiconductors 2013-2017
  * Generated linker script file for LPC4330-M0
- * Created from generic_c.ld (LPCXpresso v7.6 (2 [Build 326] [2015-02-02] ))
- * By LPCXpresso v7.6.2 [Build 326] [2015-02-02]  on Wed Apr 01 17:57:00 CDT 2015
+ * Created from linkscript.ldt by FMCreateLinkLibraries
+ * Using Freemarker v2.3.23
+ * LPCXpresso v8.2.2 [Build 650] [2016-09-09]  on Nov 24, 2017 2:21:33 PM
  */
 
-
-INCLUDE "main_m0_Release_lib.ld"
-INCLUDE "main_m0_Release_mem.ld"
+INCLUDE "main_m0_Release_library.ld"
+INCLUDE "main_m0_Release_memory.ld"
 
 ENTRY(ResetISR)
 
 SECTIONS
 {
-
-    /* MAIN TEXT SECTION */    
+    /* MAIN TEXT SECTION */
     .text : ALIGN(4)
     {
         FILL(0xff)
         __vectors_start__ = ABSOLUTE(.) ;
         KEEP(*(.isr_vector))
-        
         /* Global Section Table */
-        . = ALIGN(4) ;
+        . = ALIGN(4) ; 
         __section_table_start = .;
         __data_section_table = .;
         LONG(LOADADDR(.data));
         LONG(    ADDR(.data));
         LONG(  SIZEOF(.data));
-        LONG(LOADADDR(.data_RAM2));
-        LONG(    ADDR(.data_RAM2));
-        LONG(  SIZEOF(.data_RAM2));
-        LONG(LOADADDR(.data_RAM3));
-        LONG(    ADDR(.data_RAM3));
-        LONG(  SIZEOF(.data_RAM3));
-        LONG(LOADADDR(.data_RAM4));
-        LONG(    ADDR(.data_RAM4));
-        LONG(  SIZEOF(.data_RAM4));
-        LONG(LOADADDR(.data_RAM5));
-        LONG(    ADDR(.data_RAM5));
-        LONG(  SIZEOF(.data_RAM5));
         __data_section_table_end = .;
         __bss_section_table = .;
         LONG(    ADDR(.bss));
         LONG(  SIZEOF(.bss));
-        LONG(    ADDR(.bss_RAM2));
-        LONG(  SIZEOF(.bss_RAM2));
-        LONG(    ADDR(.bss_RAM3));
-        LONG(  SIZEOF(.bss_RAM3));
-        LONG(    ADDR(.bss_RAM4));
-        LONG(  SIZEOF(.bss_RAM4));
-        LONG(    ADDR(.bss_RAM5));
-        LONG(  SIZEOF(.bss_RAM5));
         __bss_section_table_end = .;
         __section_table_end = . ;
-        /* End of Global Section Table */
-        
+	    /* End of Global Section Table */
 
         *(.after_vectors*)
-        
+
     } >RamAHB16
-    
+
     .text : ALIGN(4)    
     {
-         *(.text*)
+        *(.text*)
         *(.rodata .rodata.* .constdata .constdata.*)
         . = ALIGN(4);
-        
     } > RamAHB16
-
     /*
      * for exception handling/unwind - some Newlib functions (in common
      * with C++ and STDC++) use this. 
      */
-    .ARM.extab : ALIGN(4)
+    .ARM.extab : ALIGN(4) 
     {
-    	*(.ARM.extab* .gnu.linkonce.armextab.*)
+        *(.ARM.extab* .gnu.linkonce.armextab.*)
     } > RamAHB16
     __exidx_start = .;
-    
+
     .ARM.exidx : ALIGN(4)
     {
-    	*(.ARM.exidx* .gnu.linkonce.armexidx.*)
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
     } > RamAHB16
     __exidx_end = .;
-    
+
     _etext = .;
         
-    
-    /* DATA section for RamAHB32 */
-    .data_RAM2 : ALIGN(4)
-    {
-       FILL(0xff)
-       PROVIDE(__start_data_RAM2 = .) ;
-       *(.ramfunc.$RAM2)
-       *(.ramfunc.$RamAHB32)
-    	*(.data.$RAM2*)
-    	*(.data.$RamAHB32*)
-       . = ALIGN(4) ;
-       PROVIDE(__end_data_RAM2 = .) ;
-    } > RamAHB32 AT>RamAHB16
-    
-    /* DATA section for RamLoc128 */
-    .data_RAM3 : ALIGN(4)
-    {
-       FILL(0xff)
-       PROVIDE(__start_data_RAM3 = .) ;
-       *(.ramfunc.$RAM3)
-       *(.ramfunc.$RamLoc128)
-    	*(.data.$RAM3*)
-    	*(.data.$RamLoc128*)
-       . = ALIGN(4) ;
-       PROVIDE(__end_data_RAM3 = .) ;
-    } > RamLoc128 AT>RamAHB16
-    
-    /* DATA section for RamLoc72 */
-    .data_RAM4 : ALIGN(4)
-    {
-       FILL(0xff)
-       PROVIDE(__start_data_RAM4 = .) ;
-       *(.ramfunc.$RAM4)
-       *(.ramfunc.$RamLoc72)
-    	*(.data.$RAM4*)
-    	*(.data.$RamLoc72*)
-       . = ALIGN(4) ;
-       PROVIDE(__end_data_RAM4 = .) ;
-    } > RamLoc72 AT>RamAHB16
-    
-    /* DATA section for RamAHB_ETB16 */
-    .data_RAM5 : ALIGN(4)
-    {
-       FILL(0xff)
-       PROVIDE(__start_data_RAM5 = .) ;
-       *(.ramfunc.$RAM5)
-       *(.ramfunc.$RamAHB_ETB16)
-    	*(.data.$RAM5*)
-    	*(.data.$RamAHB_ETB16*)
-       . = ALIGN(4) ;
-       PROVIDE(__end_data_RAM5 = .) ;
-    } > RamAHB_ETB16 AT>RamAHB16
-    
     /* MAIN DATA SECTION */
-    
-
     .uninit_RESERVED : ALIGN(4)
     {
         KEEP(*(.bss.$RESERVED*))
         . = ALIGN(4) ;
         _end_uninit_RESERVED = .;
     } > RamAHB16
-
-	
-	/* Main DATA section (RamAHB16) */
-	.data : ALIGN(4)
-	{
-	   FILL(0xff)
-	   _data = . ;
-	   *(vtable)
-	   *(.ramfunc*)
-	   *(.data*)
-	   . = ALIGN(4) ;
-	   _edata = . ;
-	} > RamAHB16 AT>RamAHB16
-
-    /* BSS section for RamAHB32 */
-    .bss_RAM2 : ALIGN(4)
-    {
-       PROVIDE(__start_bss_RAM2 = .) ;
-    	*(.bss.$RAM2*)
-    	*(.bss.$RamAHB32*)
-       . = ALIGN(4) ;
-       PROVIDE(__end_bss_RAM2 = .) ;
-    } > RamAHB32
-    /* BSS section for RamLoc128 */
-    .bss_RAM3 : ALIGN(4)
+    /* Main DATA section (RamAHB16) */
+    .data : ALIGN(4)
     {
-       PROVIDE(__start_bss_RAM3 = .) ;
-    	*(.bss.$RAM3*)
-    	*(.bss.$RamLoc128*)
-       . = ALIGN(4) ;
-       PROVIDE(__end_bss_RAM3 = .) ;
-    } > RamLoc128
-    /* BSS section for RamLoc72 */
-    .bss_RAM4 : ALIGN(4)
-    {
-       PROVIDE(__start_bss_RAM4 = .) ;
-    	*(.bss.$RAM4*)
-    	*(.bss.$RamLoc72*)
-       . = ALIGN(4) ;
-       PROVIDE(__end_bss_RAM4 = .) ;
-    } > RamLoc72
-    /* BSS section for RamAHB_ETB16 */
-    .bss_RAM5 : ALIGN(4)
-    {
-       PROVIDE(__start_bss_RAM5 = .) ;
-    	*(.bss.$RAM5*)
-    	*(.bss.$RamAHB_ETB16*)
+       FILL(0xff)
+       _data = . ;
+       *(vtable)
+       *(.ramfunc*)
+       *(.data*)
        . = ALIGN(4) ;
-       PROVIDE(__end_bss_RAM5 = .) ;
-    } > RamAHB_ETB16
-
+       _edata = . ;
+    } > RamAHB16 AT>RamAHB16
     /* MAIN BSS SECTION */
     .bss : ALIGN(4)
     {
@@ -212,36 +92,6 @@ SECTIONS
         _ebss = .;
         PROVIDE(end = .);
     } > RamAHB16
-        
-    /* NOINIT section for RamAHB32 */
-    .noinit_RAM2 (NOLOAD) : ALIGN(4)
-    {
-    	*(.noinit.$RAM2*)
-    	*(.noinit.$RamAHB32*)
-       . = ALIGN(4) ;
-    } > RamAHB32 
-    /* NOINIT section for RamLoc128 */
-    .noinit_RAM3 (NOLOAD) : ALIGN(4)
-    {
-    	*(.noinit.$RAM3*)
-    	*(.noinit.$RamLoc128*)
-       . = ALIGN(4) ;
-    } > RamLoc128 
-    /* NOINIT section for RamLoc72 */
-    .noinit_RAM4 (NOLOAD) : ALIGN(4)
-    {
-    	*(.noinit.$RAM4*)
-    	*(.noinit.$RamLoc72*)
-       . = ALIGN(4) ;
-    } > RamLoc72 
-    /* NOINIT section for RamAHB_ETB16 */
-    .noinit_RAM5 (NOLOAD) : ALIGN(4)
-    {
-    	*(.noinit.$RAM5*)
-    	*(.noinit.$RamAHB_ETB16*)
-       . = ALIGN(4) ;
-    } > RamAHB_ETB16 
-    
     /* DEFAULT NOINIT SECTION */
     .noinit (NOLOAD): ALIGN(4)
     {
@@ -250,7 +100,16 @@ SECTIONS
          . = ALIGN(4) ;
         _end_noinit = .;
     } > RamAHB16
-    
+
     PROVIDE(_pvHeapStart = DEFINED(__user_heap_base) ? __user_heap_base : .);
     PROVIDE(_vStackTop = DEFINED(__user_stack_top) ? __user_stack_top : __top_RamAHB16 - 0);
-}
+
+    /* ## Create checksum value (used in startup) ## */
+    PROVIDE(__valid_user_code_checksum = 0 - 
+                                         (_vStackTop 
+                                         + (ResetISR + 1) 
+                                         + (( DEFINED(NMI_Handler) ? NMI_Handler : M0_NMI_Handler ) + 1) 
+                                         + (( DEFINED(HardFault_Handler) ? HardFault_Handler : M0_HardFault_Handler ) + 1) 
+                                         )
+           );
+}
\ No newline at end of file
diff --git a/src/device/main_m4/inc/exec.h b/src/device/main_m4/inc/exec.h
index 5674c130..e6c77aac 100644
--- a/src/device/main_m4/inc/exec.h
+++ b/src/device/main_m4/inc/exec.h
@@ -23,7 +23,7 @@
 
 #define FW_MAJOR_VER		2
 #define FW_MINOR_VER		0
-#define FW_BUILD_VER		19
+#define FW_BUILD_VER		20
 #ifdef LEGO
 #define FW_TYPE             "LEGO"
 #else
diff --git a/src/device/main_m4/inc/serial.h b/src/device/main_m4/inc/serial.h
index abaa4100..617955e8 100644
--- a/src/device/main_m4/inc/serial.h
+++ b/src/device/main_m4/inc/serial.h
@@ -25,6 +25,7 @@
 #define SER_INTERFACE_ADX             4
 #define SER_INTERFACE_ADY             5 
 #define	SER_INTERFACE_LEGO            6
+#define	SER_INTERFACE_FTC             7
 
 
 int ser_init();
diff --git a/src/device/main_m4/src/serial.cpp b/src/device/main_m4/src/serial.cpp
index b993d534..a5bf763a 100644
--- a/src/device/main_m4/src/serial.cpp
+++ b/src/device/main_m4/src/serial.cpp
@@ -28,11 +28,13 @@ uint8_t g_interface = 0;
 int8_t g_angle = 0;
 static Iserial *g_serial = 0;
 
+
 uint16_t lego_getData(uint8_t *buf, uint32_t buflen)
 {
 	uint8_t c;
 	uint16_t d;
 	uint16_t numBlobs;
+	BlobA* ftcBlobs;
 	uint32_t temp, width, height;
 	Iserial *serial = ser_getSerial();
 
@@ -173,8 +175,400 @@ uint16_t lego_getData(uint8_t *buf, uint32_t buflen)
 #endif
 		return 6;
 	}
-	else  
+
+
+else{
+#if 0
+		static uint8_t c = 0;
+
+		buf[0] = c++;
+#else
+		//printf("%x\n", c);
+
+		if (c==0x42) // this works in port view mode on the ev3's LCD
+		{
+			BlobA *max;
+			max = g_blobs->getMaxBlob();
+			if (max==0 || max==(BlobA *)-1)
+				buf[0] = 0;
+			else
+			{
+				width = max->m_right - max->m_left;
+				temp = ((max->m_left + width/2)*819)>>10;
+				buf[0] = temp;
+			}
+		}
+		else
+			buf[0] = 1;	 // need to return nonzero value for other inquiries or LEGO brick will think we're an analog sensor
+
+#endif
+		return 1;
+	}
+}
+
+uint16_t ftc_getData(uint8_t *buf, uint32_t buflen)
+{
+	uint8_t c;
+	uint16_t d;
+	uint16_t numBlobs;
+	BlobA* ftcBlobs;
+	uint32_t temp, width, height;
+	Iserial *serial = ser_getSerial();
+
+	if (serial->receive(&c, 1)==0)
+		return 0;
+
+#if 1
+	if (c==0x00)
+	{
+		//printf("0\n");
+		char *str = "V0.1";
+		strcpy((char *)buf, str);
+		return 5;
+		//return strlen((char *)str);
+	}
+	if (c==0x08)
+	{
+		//printf("8\n");
+		char *str = "Pixy";
+		strcpy((char *)buf, str);
+		return 5;
+		//return strlen((char *)str);
+	}
+	else if (c==0x10)
+	{
+		//printf("10\n");
+		char *str = "FTC";
+		strcpy((char *)buf, str);
+		return 4;
+		//return strlen((char *)str);
+	}
+	else
+#endif
+	if (c==0x50)
+	{
+		BlobB *max;
+#if 0
+		buf[0] = 1;
+		buf[1] = 2;
+		buf[2] = 3;
+		buf[3] = 4;
+		buf[4] = 5;
+		buf[5] = 6;
+		buf[6] = 7;
+#else
+		max = (BlobB *)g_blobs->getMaxBlob();
+		if (max==0)
+			memset(buf, 0, 7);
+		else if (max==(BlobB *)-1)
+			memset(buf, -1, 7);
+		else
+		{
+			width = max->m_right - max->m_left;
+			height = max->m_bottom - max->m_top;
+			*(uint16_t *)buf = max->m_model; // signature
+			temp = ((max->m_left + width/2)*819)>>10;
+			buf[2] = temp; // x
+			buf[3] = max->m_top + height/2; // y
+			temp = (width*819)>>10;
+			buf[4] = temp; // width
+			buf[5] = height; // height
+			if (max->m_model>CL_NUM_SIGNATURES)
+			{
+				temp = ((int32_t)max->m_angle*91)>>7;
+				g_angle = temp;
+			}
+		}
+#endif
+		return 6;
+	}
+	else if (c==0x60)
+	{
+		buf[0] = g_angle;
+		return 1;
+	}
+	else if (c>=0x51 && c<=0x57)
+	{
+#if 0
+		buf[0] = 1;
+		buf[1] = 2;
+		buf[2] = 3;
+		buf[3] = 4;
+		buf[4] = 5;
+#else
+		BlobA *max;
+		max = g_blobs->getMaxBlob(c-0x50, &numBlobs);
+		if (max==0)
+			memset(buf, 0, 5);
+		else if (max==(BlobA *)-1)
+			memset(buf, -1, 5);
+		else
+		{
+			width = max->m_right - max->m_left;
+			height = max->m_bottom - max->m_top;
+			buf[0] = numBlobs; // number of blocks that match signature
+			temp = ((max->m_left + width/2)*819)>>10;
+			buf[1] = temp; // x
+			buf[2] = max->m_top + height/2;	// y
+			temp = (width*819)>>10;
+			buf[3] = temp; // width
+			buf[4] = height; // height
+		}
+#endif
+		return 5;
+	}
+	else if (c==0x58)
+	{
+		BlobB *max;
+		if (serial->receive((uint8_t *)&d, 2)<2) // receive cc signature to look for
+			return 0;
+#if 0
+		buf[0] = 1;
+		buf[1] = 2;
+		buf[2] = 3;
+		buf[3] = 4;
+		buf[4] = 5;
+		buf[5] = 6;
+#else
+		max = (BlobB *)g_blobs->getMaxBlob(d, &numBlobs);
+		if (max==0)
+			memset(buf, 0, 6);
+		else if (max==(BlobB *)-1)
+			memset(buf, -1, 6);
+		else
+		{
+			width = max->m_right - max->m_left;
+			height = max->m_bottom - max->m_top;
+			buf[0] = numBlobs; // number of cc blocks that match
+			temp = ((max->m_left + width/2)*819)>>10;
+			buf[1] = temp; // x
+			buf[2] = max->m_top + height/2; // y
+			temp = (width*819)>>10;
+			buf[3] = temp; // width
+			buf[4] = height; // height
+			temp = ((int32_t)max->m_angle*91)>>7;
+			buf[5] = temp; // angle
+		}
+#endif
+		return 6;
+	}
+	else if (c==0x70)  //FTC Extension return top 5 Largest Signatures 27 byte limit need 5 bytes per sig 5 * 5 = 25 + NumBlocks in view = 26
+		{
+
+
+	#if 0
+			buf[0] = 1;
+			buf[1] = 2;
+			buf[2] = 3;
+			buf[3] = 4;
+			buf[4] = 5;
+			buf[5] = 6;
+			buf[6] = 7;
+			buf[7] = 8;
+			buf[8] = 9;
+			buf[9] = 10;
+			buf[10] = 11;
+			buf[11] = 12;
+			buf[12] = 13;
+			buf[13] = 14;
+			buf[14] = 15;
+			buf[15] = 16;
+			buf[16] = 17;
+			buf[17] = 18;
+			buf[18] = 19;
+			buf[19] = 20;
+			buf[20] = 21;
+			buf[21] = 22;
+			buf[22] = 23;
+			buf[23] = 24;
+			buf[24] = 25;
+			buf[25] = 26;
+	#else
+
+			g_blobs->getMaxBlobs(0, 5, &ftcBlobs, &numBlobs);
+			if (numBlobs==0)
+				memset(buf, 0, 26);
+//			else if (maxBlobs[0]==(BlobA *)-1)
+//				memset(buf, -1, 25);
+			else
+			{
+				if(numBlobs > 255){
+					buf[0] = 255; // Max is 255 for a single byte
+				}else{
+					buf[0] = numBlobs; // number of total blocks in view
+				}
+				for(uint8_t i=0; i < 5; i++){
+					uint8_t buffOffset = (i*5) + 1;
+					if(i < numBlobs){
+						BlobA *max = (BlobA *) ftcBlobs + i;
+						width = max->m_right - max->m_left;
+						height = max->m_bottom - max->m_top;
+						buf[buffOffset] = max->m_model; // signature 1-7 only need one byte
+						temp = ((max->m_left + width/2)*819)>>10;
+						buf[buffOffset + 1] = temp; // x
+						buf[buffOffset + 2] = max->m_top + height/2; // y
+						temp = (width*819)>>10;
+						buf[buffOffset + 3] = temp; // width
+						buf[buffOffset + 4] = height; // height
+					}else{
+						buf[buffOffset] = 0; // signature 1-7 only need one byte
+						buf[buffOffset + 1] = 0; // x
+						buf[buffOffset + 2] = 0; // y
+						buf[buffOffset + 3] = 0; // width
+						buf[buffOffset + 4] = 0; //height
+					}
+
+				}
+			}
+	#endif
+			return 26;
+		}
+
+	else if (c>=0x71 && c<=0x77)  //FTC Extension return top 6 Largest Signatures 27 byte limit need 4 bytes per sig 6 * 4 = 24 + NumBlocks in view = 25
+	{
+
+
+#if 0
+		buf[0] = 1;
+		buf[1] = 2;
+		buf[2] = 3;
+		buf[3] = 4;
+		buf[4] = 5;
+		buf[5] = 6;
+		buf[6] = 7;
+		buf[7] = 8;
+		buf[8] = 9;
+		buf[9] = 10;
+		buf[10] = 11;
+		buf[11] = 12;
+		buf[12] = 13;
+		buf[13] = 14;
+		buf[14] = 15;
+		buf[15] = 16;
+		buf[16] = 17;
+		buf[17] = 18;
+		buf[18] = 19;
+		buf[19] = 20;
+		buf[20] = 21;
+		buf[21] = 22;
+		buf[22] = 23;
+		buf[23] = 24;
+		buf[24] = 25;
+
+#else
+
+		g_blobs->getMaxBlobs(c-0x70, 6, &ftcBlobs, &numBlobs);
+		if (numBlobs==0)
+			memset(buf, 0, 25);
+//			else if (maxBlobs[0]==(BlobA *)-1)
+//				memset(buf, -1, 25);
+		else
+		{
+			if(numBlobs > 255){
+				buf[0] = 255; // Max is 255 for a single byte
+			}else{
+				buf[0] = numBlobs; // number of total blocks in view
+			}
+			for(uint8_t i=0; i < 6; i++){
+				uint8_t buffOffset = (i*4) + 1;
+				if(i < numBlobs){
+					BlobA *max = (BlobA *) ftcBlobs + i;
+					width = max->m_right - max->m_left;
+					height = max->m_bottom - max->m_top;
+					temp = ((max->m_left + width/2)*819)>>10;
+					buf[buffOffset] = temp; // x
+					buf[buffOffset + 1] = max->m_top + height/2; // y
+					temp = (width*819)>>10;
+					buf[buffOffset + 2] = temp; // width
+					buf[buffOffset + 3] = height; // height
+				}else{
+					buf[buffOffset] = 0; // x
+					buf[buffOffset + 1] = 0; // y
+					buf[buffOffset + 2] = 0; // width
+					buf[buffOffset + 3] = 0; // height
+				}
+			}
+		}
+#endif
+		return 25;
+}
+
+else if (c==0x78)  //FTC Extension return top 5 Largest Color Code Signatures 27 byte limit need 6 bytes per sig 4 * 6 = 24 + NumBlocks in view = 25
+{
+
+
+#if 0
+	buf[0] = 1;
+	buf[1] = 2;
+	buf[2] = 3;
+	buf[3] = 4;
+	buf[4] = 5;
+	buf[5] = 6;
+	buf[6] = 7;
+	buf[7] = 8;
+	buf[8] = 9;
+	buf[9] = 10;
+	buf[10] = 11;
+	buf[11] = 12;
+	buf[12] = 13;
+	buf[13] = 14;
+	buf[14] = 15;
+	buf[15] = 16;
+	buf[16] = 17;
+	buf[17] = 18;
+	buf[18] = 19;
+	buf[19] = 20;
+	buf[20] = 21;
+	buf[21] = 22;
+	buf[22] = 23;
+	buf[23] = 24;
+	buf[24] = 25;
+
+#else
+
+	g_blobs->getMaxBlobs(8, 4, &ftcBlobs, &numBlobs);
+	if (numBlobs==0)
+		memset(buf, 0, 25);
+//			else if (maxBlobs[0]==(BlobA *)-1)
+//				memset(buf, -1, 25);
+	else
 	{
+		if(numBlobs > 255){
+			buf[0] = 255; // Max is 255 for a single byte
+		}else{
+			buf[0] = numBlobs; // number of total blocks in view
+		}
+		for(uint8_t i=0; i < 4; i++){
+			uint8_t buffOffset = (i*6) + 1;
+			if(i < numBlobs){
+				BlobA *max = (BlobA *) ftcBlobs + i;
+				width = max->m_right - max->m_left;
+				height = max->m_bottom - max->m_top;
+				buf[buffOffset] = (max->m_model & 0xFF); // cc signature need two bytes Lower
+				buf[buffOffset+1] = ((max->m_model & 0xFF00) >> 8); // cc signature need two bytes Higher
+				temp = ((max->m_left + width/2)*819)>>10;
+				buf[buffOffset + 2] = temp; // x
+				buf[buffOffset + 3] = max->m_top + height/2; // y
+				temp = (width*819)>>10;
+				buf[buffOffset + 4] = temp; // width
+				buf[buffOffset + 5] = height; // height
+				//temp = ((int32_t)max->m_angle*91)>>7;
+				//buf[buffOffset + 5] = temp; // angle
+			}else{
+				buf[buffOffset] = 0; // signature lb
+				buf[buffOffset + 1] = 0; // signature hb
+				buf[buffOffset + 2] = 0; // x
+				buf[buffOffset + 3] = 0; // y
+				buf[buffOffset + 4] = 0; // width
+				buf[buffOffset + 5] = 0; // height
+			}
+		}
+	}
+#endif
+	return 25;
+}
+
+else{
 #if 0
 		static uint8_t c = 0;
 
@@ -206,8 +600,12 @@ uint16_t lego_getData(uint8_t *buf, uint32_t buflen)
 
 uint32_t callback(uint8_t *data, uint32_t len)
 {
-	if (g_interface==SER_INTERFACE_LEGO)
+	if (g_interface==SER_INTERFACE_LEGO){
 		return lego_getData(data, len);
+	}
+	else if (g_interface==SER_INTERFACE_FTC){
+		return ftc_getData(data, len);
+	}
 	else
 		return g_blobs->getBlock(data, len);
 }
@@ -229,7 +627,7 @@ void ser_loadParams()
 {
 #ifndef LEGO
 	prm_add("Data out port", 0, 
-		"Selects the port that's used to output data (default Arduino ICSP SPI) @c Interface @s 0=Arduino_ICSP_SPI @s 1=SPI_with_SS @s 2=I2C @s 3=UART @s 4=analog/digital_x @s 5=analog/digital_y @s 6=LEGO_I2C", UINT8(0), END);
+		"Selects the port that's used to output data (default Arduino ICSP SPI) @c Interface @s 0=Arduino_ICSP_SPI @s 1=SPI_with_SS @s 2=I2C @s 3=UART @s 4=analog/digital_x @s 5=analog/digital_y @s 6=LEGO_I2C @s 7=FTC_I2C", UINT8(0), END);
 	prm_add("I2C address", PRM_FLAG_HEX_FORMAT, 
 		"@c Interface Sets the I2C address if you are using I2C data out port. (default 0x54)", UINT8(I2C_DEFAULT_SLAVE_ADDR), END);
 	prm_add("UART baudrate", 0, 
@@ -293,7 +691,12 @@ int ser_setInterface(uint8_t interface)
  		g_i2c0->setSlaveAddr(0x01);
 		g_i2c0->setFlags(true, false);
 		break;
-		
+	case SER_INTERFACE_FTC:
+		g_serial = g_i2c0;
+	 	// Use the Address set via PixyMon g_i2c0->setSlaveAddr(0x01);
+		g_i2c0->setFlags(true, false);
+		break;
+
 	default:
 	case SER_INTERFACE_ARDUINO_SPI:
 		g_serial = g_spi;