calculateCentroids.h

// returns a WORD packing two signed chars. The high bytes is the last active sensor in the last centroid,
// while the low byte is the first active sensor of the last centroid
WORD calculateCentroids(WORD *centroidBuffer, WORD *sizeBuffer, BYTE maxNumCentroids, BYTE minSensor, BYTE maxSensor, BYTE numSensors) {
        signed char lastActiveSensor = -1;
        BYTE centroidIndex = 0, sensorIndex, actualHardwareIndex;
        BYTE wrappedAround = 0;
        BYTE inCentroid = 0;
        WORD peakValue = 0, troughDepth = 0;
        BYTE counter;
        long temp;

        WORD lastSensorVal, currentSensorVal, currentWeightedSum, currentUnweightedSum;
        BYTE currentStart, currentLength;

        for(sensorIndex = 0; sensorIndex < maxNumCentroids; sensorIndex++) {
                centroidBuffer[sensorIndex] = 0xFFFF;
                sizeBuffer[sensorIndex] = 0;
        }

        currentSensorVal = 0;

        for(sensorIndex = 0, actualHardwareIndex = minSensor; sensorIndex < numSensors; sensorIndex++)
        {
                lastSensorVal = currentSensorVal;

                currentSensorVal = CSD_waSnsDiff[actualHardwareIndex++];
                if(currentSensorVal > 0) {
                        lastActiveSensor = sensorIndex;
                }
                // if we get to the end, and there is more to go, wrap around
                if(actualHardwareIndex == maxSensor)
                {
                        actualHardwareIndex = minSensor;
                        // once we wrap around, if we find ourselves out of a centroid,
                        // any centroids detected after the then current point onwards
                        // would be equal or worse than the ones we already got earlier for
                        // the same sensors, so we will have to break
                        wrappedAround = 1;
                }

                if(inCentroid) {
                        // Currently in the middle of a group of sensors constituting a centroid.  Use a zero sample
                        // or a spike above a certain magnitude to indicate the end of the centroid.

                        if(currentSensorVal == 0) {
                                if(currentUnweightedSum > wMinimumCentroidSize)
                                {
                                        temp = ((long)currentWeightedSum << SLIDER_BITS) / currentUnweightedSum;
                                        centroidBuffer[centroidIndex] = (currentStart << SLIDER_BITS) + (WORD)temp;
                                        sizeBuffer[centroidIndex] = currentUnweightedSum;
                                        centroidIndex++;
                                }

                                inCentroid = 0;
                                if(wrappedAround) {
                                        break;
                                }
                                if(centroidIndex >= maxNumCentroids)
                                        break;
                                continue;
                        }

                        if(currentSensorVal > peakValue)  // Keep tabs on max and min values
                                peakValue = currentSensorVal;
                        if(peakValue - currentSensorVal > troughDepth)
                                troughDepth = peakValue - currentSensorVal;

                        // If this sensor value is a significant increase over the last one, AND the last one was decreasing, then start a new centroid.
                        // In other words, identify a trough in the values and use it to segment into two centroids.

                        if(sensorIndex >= 2) {
                                if(troughDepth > wAdjacentCentroidNoiseThreshold && currentSensorVal > lastSensorVal + wAdjacentCentroidNoiseThreshold) {
                                        if(currentUnweightedSum > wMinimumCentroidSize)
                                        {
                                                temp = ((long)currentWeightedSum << SLIDER_BITS) / currentUnweightedSum;
                                                centroidBuffer[centroidIndex] = (currentStart << SLIDER_BITS) + (WORD)temp;
                                                sizeBuffer[centroidIndex] = currentUnweightedSum;
                                                centroidIndex++;
                                        }
                                        inCentroid = 0;
                                        if(wrappedAround){
                                                break;
                                        }
                                        if(centroidIndex >= maxNumCentroids)
                                                break;
                                        inCentroid = 1;
                                        currentStart = sensorIndex;
                                        currentUnweightedSum = peakValue = currentSensorVal;
                                        currentLength = 1;
                                        currentWeightedSum = 0;
                                        troughDepth = 0;
                                        continue;
                                }
                        }

                        currentUnweightedSum += currentSensorVal;
                        currentWeightedSum += currentLength * currentSensorVal;
                        currentLength++;
                }
                else {
                        // Currently not in a centroid (zeros between centroids).  Look for a new sample to initiate centroid.
                        if(currentSensorVal > 0) {
                                currentStart = sensorIndex;
                                currentUnweightedSum = peakValue = currentSensorVal;
                                currentLength = 1;
                                currentWeightedSum = 0;
                                troughDepth = 0;
                                inCentroid = 1;
                        }
                }
                if(!inCentroid && wrappedAround){
                        break;
                }
        }

        // Finish up the calculation on the last centroid, if necessary
        if(inCentroid && currentUnweightedSum > wMinimumCentroidSize)
        {
                temp = ((long)currentWeightedSum << SLIDER_BITS) / currentUnweightedSum;
                centroidBuffer[centroidIndex] = (currentStart << SLIDER_BITS) + (WORD)temp;
                sizeBuffer[centroidIndex] = currentUnweightedSum;
                centroidIndex++;
        }

        return (lastActiveSensor << 8) | currentStart;
}