/*
 * File         : thermal_blobber.cpp
 * Author       : Timothy Wiley
 * Version      :
 * Created      : 11/02/2010
 * Description  : Detects hot spots (blobs) on thermal images
 * Copyright    :
 */

#include <ros/ros.h>
#include <image_transport/image_transport.h>

#include "thermal_blobber/thermal_blobber.h"
#include "thermal_blobber/BlobMsg.h"
#include "thermal_blobber/BlobsMsg.h"

#include <algorithm>
#include <limits>
#include <map>

#define DEFAULT_MIN_AREA            50
#define DEFAULT_MINIMUM_THRESHOLD	195
#define DEFAULT_MIN_WIDTH           6
//#define DEFAULT_THERMAL_THRESHOLD   (215 << 8)
#define DEFAULT_THERMAL_THRESHOLD   215
#define LOG_START                   "ThermalBlobber ::"
//#define MAX_THERMAL_VALUE           65535
#define MAX_THERMAL_VALUE           255
#define NO_BLOB                     -1
#define THRESHOLD_AUTO              -1
#define THRESHOLD_UPDATE_LIMIT      0.95

// Macros
#define SEC2USEC(n) (n * 1000000)
#define CELL(x, y, wd) ((y) * (wd) + (x))
#define NEW_BLOB(cell) {blobIndex[cell] = ++blobCount; equiv.push_back(blobCount);}

// Typedefs
//typedef unsigned short* ThermalImage;
typedef unsigned char* ThermalImage;
typedef unsigned short* SwissImage;

using namespace crosbot;
using std::string;
using std::vector;
using std::map;

// Declare the class for ROS
PLUGINLIB_DECLARE_CLASS(thermal_blobber, thermal_blobber, casualty::ThermalBlobber, nodelet::Nodelet)

namespace casualty {

ThermalBlobber::ThermalBlobber() :
        savedData(false)
{}

void ThermalBlobber::callbackTimer(const ros::TimerEvent& tEvent) {
	//LOG("%s callbackTimer \n", LOG_START);
	executeOnce();
}

void ThermalBlobber::callbackThermalImage(const sensor_msgs::ImageConstPtr& latestImage) {
    Lock lock(loadMutex);
	//LOG("%s callbackThermalImage()\n", LOG_START);
    //LOG("%s callbackThermalImage :: %s\n", LOG_START, latestImage->encoding.c_str());
	nextThermalImage = latestImage;
}

void ThermalBlobber::callbackDistanceImage(const sensor_msgs::ImageConstPtr& latestImage) {
	Lock lock(loadMutex);
	//LOG("%s callbackDistanceImage()\n", LOG_START);
	nextDistanceImage = latestImage;
}

bool ThermalBlobber::callbackBlobsService(
		thermal_blobber::BlobsServiceRequest& request,
		thermal_blobber::BlobsServiceResponse& response) {
    //LOG("%s callbackBlobsService()\n", LOG_START);
    Lock lock(saveMutex);

    if (!savedData) {
        return false;
    }

	// Save the blobs
    //LOG("%s \t saveBlobs\n", LOG_START);
	for(vector<BlobPtr>::iterator it = savedBlobs.begin(); it != savedBlobs.end(); ++it) {
		thermal_blobber::BlobMsg blob = (*it)->toBlobMsg();
		response.blobs.blobs.push_back(blob);
	}

	// Save additional image data
	//ImagePtr img16bit = new Image(*thresholdImg, Image::Mono16);
	response.blobs.encodedThermal = *(savedEncodedImage->toROS());
	response.blobs.distance = *(savedDistanceImage->toROS());
	response.blobs.thermalTimestamp = savedThermalTimestamp;
	response.blobs.blobFrame = frameThermal;
	response.blobs.threshold = savedThreshold;

	// Save to response
	//LOG("%s \t\t response.blobs.size(): %ld\n", LOG_START, response.blobs.blobs.size());

	return true;
}

void ThermalBlobber::onInit() {
	LOG("%s init\n", LOG_START);

	// Setup variables
    thresholdCal = new ThresholdCal();
    cameraCorrection = new CameraCorrection();
    blobs.clear();
    equiv.clear();
    savedData = false;
    initialised = false;
    thermalImage = NULL;
    distanceImage = NULL;

	// Load configuration
	ros::NodeHandle nhConfig("/thermal_blobber");
	ConfigElementPtr config = new ROSConfigElement(nhConfig);
	configure(config);

	// Setup callbacks
    LOG("%s Frequency: %lf\n", LOG_START, frequency);
    ros::NodeHandle nh = getNodeHandle();
	timerCallback = nh.createTimer(ros::Duration(frequency), &ThermalBlobber::callbackTimer, this);

	// Setup subscribe/publish
	image_transport::ImageTransport it(nh);
	blobsService = nh.advertiseService("out_blobs", &ThermalBlobber::callbackBlobsService, this);
	blobsImagePub = it.advertise("out_blobs_image", 1);
	thresholdImagePub = it.advertise("out_threshold_image", 1);
	thermalSub = it.subscribe("thermal", 1, &ThermalBlobber::callbackThermalImage, this);
	distanceSub = it.subscribe("distance", 1, &ThermalBlobber::callbackDistanceImage, this);
}

void ThermalBlobber::configure(ConfigElementPtr config) {
    LOG("%s configure\n", LOG_START);

    autoThreshold           = config->getParamAsBool("autoThreshold", true);
    frameThermal            = config->getParam("frameThermal", "/head");
    frequency               = config->getParamAsDouble("frequency", 1);
    string initialTmp       = config->getParam("initialThreshold");
    initialThreshold        = (initialTmp == "auto") ? THRESHOLD_AUTO : config->getParamAsInt("initialThreshold", THRESHOLD_AUTO);
    invert                  = config->getParamAsBool("invert", false);
    learningRate            = config->getParamAsDouble("learningRate", 0.1);
    //minimumThreshold        = config->getParamAsInt("minimumThreshold", DEFAULT_MINIMUM_THRESHOLD) << 8;
    minimumArea             = config->getParamAsInt("minimumArea", DEFAULT_MIN_AREA);
    minimumThreshold        = config->getParamAsInt("minimumThreshold", DEFAULT_MINIMUM_THRESHOLD);
    minimumWidth            = config->getParamAsInt("minimumWidth", DEFAULT_MIN_WIDTH);

    // Load padding
    removePadding = false;
    ConfigElementPtr imageBoundsConfig = config->getChild("imageBounds");
    if (imageBoundsConfig != NULL) {
        removePadding = true;
        removePaddingTop = imageBoundsConfig->getParamAsInt("top", 0);
        removePaddingRight = imageBoundsConfig->getParamAsInt("right", 500);
        removePaddingBottom = imageBoundsConfig->getParamAsInt("bottom", 500);
        removePaddingLeft = imageBoundsConfig->getParamAsInt("left", 0);
    }

    // Configure children
    cameraCorrection->configure(config);
}

void ThermalBlobber::executeOnce() {
    // Further initialisation that can only happen with images
    if (!initialised) {
        // Initialise trig table and camera correction
        //LOG("%s Initialise\n", LOG_START);
        loadNewImages();
        if (thermalImage == NULL) {
        	//LOG("%s No Thermal Image\n", LOG_START);
        	return;
        } else if (distanceImage == NULL) {
        	//LOG("%s No Distance Image\n", LOG_START);
        	//return;
        }

        // Initialise Camera Correction
        cameraCorrection->configureCorrection(thermalImage, distanceImage);

        // Set initial threshold if necessary
        setInitialThreshold();

        initialised = true;
        LOG("%s Initialised\n", LOG_START);
    }

    if (!initialised) {
    	LOG("%s Haven't received initialisation images yet.\n", LOG_START);
    }

	// Load the new image
	//LOG("%s Loading new images\n", LOG_START);
	loadNewImages();

	// Setup image to process
	//LOG("%s Process Image\n", LOG_START);
	ImagePtr processImage = new Image(*thermalImage, crosbot::Image::Mono8);
	blobs.clear();
	int blobIndex[thermalImage->width * thermalImage->height];

	// Invert
	if (invert) invertImage(processImage);

	// Threshold
	//LOG("%s Threshold: %d\n", LOG_START, threshold >> 8);
	//LOG("%s Threshold: %d\n", LOG_START, threshold);
	simpleThreshold(processImage, threshold);
	ImagePtr thresholdImg = new Image(*processImage, crosbot::Image::Mono8);

	// Process blobs
	//LOG("%s Find Blobs\n", LOG_START);
	findBlobs(processImage, blobs, equiv, blobIndex);
	trimBlobs(minimumArea, minimumWidth, blobs);
	//LOG("\t%s Blobs found: %ld\n", LOG_START, blobs.size());

	// Generate encoded image
	encodedBlobsImage(processImage, blobs, blobIndex);
	//drawBlobBorders(processImage, blobs);

	//LOG("%s Calculate Blob Points\n", LOG_START);
	calculateBlobPoints(blobs, blobIndex);


	// Publish the data
	//LOG("%s Save Data\n", LOG_START);
	saveData(blobs, thresholdImg, processImage, distanceImage);

	// Update the threshold
	if (autoThreshold) {
	    //LOG("%s Update Threshold\n", LOG_START);
		updateThreshold(thermalImage);
	}

	//LOG("%s executeOnce() done\n", LOG_START);
}

void ThermalBlobber::loadNewImages() {
	// Lock next variable versions for loading
	Lock lock(loadMutex);
	if (nextThermalImage) {
		thermalImage = new Image(nextThermalImage, crosbot::Image::Mono8);
		//thermalImage = removePaddingFromImage(tmpThermal);
		//thermalTimestamp = nextThermalImage->header.stamp;
		// TODO check timestamp
		thermalTimestamp = ros::Time::now();
		//LOG("%s Thermal timestamp: %.3lf\n", LOG_START, thermalTimestamp.toSec());
	}

	// Debug setup for bags wo/distance image
	if (nextDistanceImage) {
	    distanceImage = new Image(nextDistanceImage, crosbot::Image::Mono16);
	} else if (nextThermalImage) {
	    //LOG("%s Load distance as thermal\n", LOG_START);
	    distanceImage = new Image(nextThermalImage, crosbot::Image::Mono8);
	}
}

ImagePtr ThermalBlobber::removePaddingFromImage(ImagePtr image) {
    if (removePadding) {
        //LOG("%s \t Removing padding\n", LOG_START);

        unsigned int newWidth = removePaddingRight - removePaddingLeft;
        unsigned int newHeight = removePaddingBottom - removePaddingTop;

        // NOTE: MUST PRODUCE A WIDTH DIVISIBLE BY 4 DUE TO COMPRESSION ISSUES
        unsigned int imageWidth = image->width;
        unsigned int imageHeight = image->height;

        ImagePtr newImage = new Image(image->encoding, newWidth, newHeight);
        //LOG("%s \t\t orig width: %d, height: %d fmt: %d\n", LOG_START, imageWidth, imageHeight, image->getFormat());
        //LOG("%s \t\t width: %d height: %d fmt: %d\n", LOG_START, newWidth, newHeight, newImage->getFormat());

        // Load data/values
        ThermalImage imgData = (ThermalImage) image->data;
        ThermalImage newImgData = (ThermalImage) newImage->data;

        // Create new image
        for (unsigned int y = 0; y < newHeight; y++) {
            for (unsigned int x = 0; x < newWidth; x++) {
                unsigned int relX = x + removePaddingLeft;
                unsigned int relY = y + removePaddingTop;
                unsigned int cell = CELL(x, y, newWidth);
                unsigned int relCell = CELL(relX, relY, imageWidth);
                //LOG("%s \t\t (x,y): (%d, %d)(%d) (relX,relY): (%d, %d)(%d)\n", LOG_START, x, y, cell, relX, relY, relCell);
                if (relX < imageWidth &&
                    relY < imageHeight) {
                    newImgData[cell] = imgData[relCell];
                } else {
                    newImgData[cell] = 0;
                    //if (relX >= imageWidth) LOG("%s \t\t out of bounds X: %d\n", LOG_START, relX);
                }
            }
        }

        return newImage;
    } else {
        return image;
    }
}

double ThermalBlobber::blobCoverage(vector<BlobPtr>& blobs, ImagePtr processImage) {
    // Calculate percentage of largest blob
    double imageArea = processImage->height * processImage->width;
    double blobArea = 0;
    double blobCoverage = 0;
    double coverage = 0;
    for(vector<BlobPtr>::iterator it = blobs.begin(); it != blobs.end(); ++it) {
        blobArea = (*it)->area();
        blobCoverage = blobArea / imageArea;
        if (blobCoverage > coverage) {
            coverage = blobCoverage;
        }
    }

    return coverage;
}

void ThermalBlobber::setInitialThreshold() {
    if (initialThreshold != THRESHOLD_AUTO) {
        //threshold = initialThreshold << 8;
        threshold = initialThreshold;
    } else {
        // Set the threshold based on the environment
        ImagePtr processImage = new Image(*thermalImage);
        unsigned short possibleThres = thresholdCal->threshold(processImage);

        // Blob on this threshold
        vector<BlobPtr> blobs;
        vector<int> equiv;
        int blobIndex[thermalImage->width * thermalImage->height];
        simpleThreshold(processImage, possibleThres);
        findBlobs(processImage, blobs, equiv, blobIndex);
        trimBlobs(minimumArea, minimumWidth, blobs);

        double coverage = blobCoverage(blobs, processImage);
        if (coverage <= THRESHOLD_UPDATE_LIMIT) {
            threshold = possibleThres;
        } else {
            threshold = getHighestGreyLevel(thermalImage) + 1;
        }

        // Keep above minimum
        if (threshold < minimumThreshold) {
        	threshold = minimumThreshold;
        }
    }
}

unsigned short ThermalBlobber::getHighestGreyLevel(ImagePtr processImage) {
    unsigned short highestGreyLevel = 0;
    ThermalImage imageData = (ThermalImage) (processImage->data);
    unsigned int numPixels = processImage->width * processImage->height;
    for (unsigned int i = 0; i < numPixels; ++i) {
        if (imageData[i] > highestGreyLevel) {
            highestGreyLevel = imageData[i];
        }
    }
    return highestGreyLevel;
}

void ThermalBlobber::simpleThreshold(ImagePtr image, unsigned short threshold) {
    //LOG("\t%s, Threshold: %d\n", LOG_START, threshold >> 8);
    ThermalImage imageData = (ThermalImage) (image->data);
    unsigned int numPixels = image->width * image->height;
    for (unsigned int i = 0; i < numPixels; ++i) {
        if (imageData[i] > threshold) {
            // Do nothing for now
        } else {
            imageData[i] = BELOW_THRESHOLD;
        }
    }
}

void ThermalBlobber::findBlobs(ImagePtr image, vector<BlobPtr>& blobs, vector<int>& equiv, int blobIndex[]) {
    int blobCount = 0;
    ThermalImage imageData = (ThermalImage) (image->data);
    int width = image->width;
    int height = image->height;

    // Use an array to maintain pointers to the blobs
    // Clear blobIndex before starting
    for (int y = 0; y < height; ++y) {
        for (int x = 0; x < width; ++x) {
            blobIndex[CELL(x, y, width)] = NO_BLOB;
        }
    }

    // Use this vector to maintain all the blobs that are equivalent
    equiv.clear();
    equiv.push_back(0);

    // Process first cell so it is setup correctly
    if (imageData[0] != 0) {
        NEW_BLOB(0);
    }

    // Process first row for upward referencing
    for(int x = 1; x < width; ++x) {
        int y = 0;
        int cell = CELL(x, y, width);
        if (imageData[cell] != 0) {
            int cellPrev = CELL(x - 1, y, width);

            // Check previous cell
            if (imageData[cellPrev] == 0) {
                // Start of new blob
                NEW_BLOB(cell);
            } else {
                // Continuation of previous blob
                blobIndex[cell] = blobIndex[CELL(x-1, y, width)];
            }
        }
    }

    // Process first column for backward referencing
    for(int y = 1; y < height; ++y) {
        int x = 0;
        int cell = CELL(x, y, width);
        if (imageData[cell] != 0) {
            int upCell = CELL(x, y - 1, width);
            if (imageData[upCell] == 0) {
                NEW_BLOB(cell);
            } else {
                blobIndex[cell] = blobIndex[upCell];
            }
        }
    }

    // Go through each image cell
    for (int y = 1; y < height; ++y) {
        for (int x = 1; x < width; ++x) {
            int cell = CELL(x, y, width);

            if (imageData[cell] != 0) {
                // Check previous cell
                int cellPrev = CELL(x-1, y, width);
                int cellUp = CELL(x, y-1, width);
                if (imageData[cellPrev] == 0 && imageData[cellUp] == 0) {
                    // Start of new blob
                    NEW_BLOB(cell);
                } else if (imageData[cellPrev] != 0 && imageData[cellUp] == 0) {
                    // Continuation of previous blob
                    blobIndex[cell] = blobIndex[cellPrev];
                } else if (imageData[cellPrev] == 0 && imageData[cellUp] != 0) {
                    // Continuation of up blob
                    blobIndex[cell] = blobIndex[cellUp];
                } else {
                    //LOG("\t%sProcess with equiv's\n", LOG_START);
                    // Set blobNo to previous
                    blobIndex[cell] = blobIndex[cellPrev];

                    // Update equivalent blobs
                    //LOG("\t%scellPrev: %d - cellUp: %d\n", LOG_START, blobIndex[cellPrev], blobIndex[cellUp]);
                    int upEquiv = equivalentBlob(blobIndex[cellUp], equiv);
                    int prevEquiv = equivalentBlob(blobIndex[cellPrev], equiv);
                    //LOG("\t%sprevEquiv: %d - upEqiv: %d\n", LOG_START, prevEquiv, upEquiv);
                    if (upEquiv != prevEquiv) {
                        equiv[prevEquiv] = upEquiv;
                    }
                    //LOG("\t%sDone process with equiv's\n", LOG_START);
                }
            }
        }
    }

    // Extract the blobs from the array of indicies
    extractBlobs(blobIndex, width, height, blobs, equiv);
}
void ThermalBlobber::extractBlobs(int blobIndex[], int width, int height, vector<BlobPtr>& blobs, vector<int>& equiv) {
    // Setup map for maintaining blobs
    map<int, BlobPtr> blobMap;

    for (int y = 0; y < height; ++y) {
        for (int x = 0; x < width; ++x) {
            int cell = CELL(x, y, width);
            if (blobIndex[cell] != NO_BLOB) {
                int blobNo = equivalentBlob(blobIndex[cell], equiv);
                if (blobMap.find(blobNo) == blobMap.end()) {
                    // Create new blob
                    BlobPtr blob = new Blob(blobNo, x, y, x, y);
                    blobMap[blobNo] = blob;
                } else {
                    // Extend existing blob
                    BlobPtr blob = blobMap[blobNo];
                    if (x < blob->minx) blob->minx = x;
                    if (y < blob->miny) blob->miny = y;
                    if (x > blob->maxx) blob->maxx = x;
                    if (y > blob->maxy) blob->maxy = y;
                }
            }
        }
    }

    // Add blobs to the blob vector
    blobs.clear();
    for(map<int, BlobPtr>::iterator it = blobMap.begin(); it != blobMap.end(); ++it) {
        blobs.push_back(it->second);
        it->second->setCentre();
    }
}

void ThermalBlobber::trimBlobs(int minArea, int minWidth, vector<BlobPtr>& blobs) {
    vector<BlobPtr> toKeep;
    for (vector<BlobPtr>::iterator it = blobs.begin(); it != blobs.end(); ++it) {
        BlobPtr blob = *it;

        if (blobSizeOk(blob, minArea, minWidth) &&
            !containedInside(blob, blobs)) {
            toKeep.push_back(blob);
        }
    }

    // Delete
    blobs.clear();
    blobs.insert(blobs.begin(), toKeep.begin(), toKeep.end());
}

void ThermalBlobber::encodedBlobsImage(ImagePtr image, vector<BlobPtr>& blobs, int blobIndex[]) {
    ThermalImage imageData = (ThermalImage) (image->data);
    int width = image->width;
    int height = image->height;

	for (int y = 0; y < height; ++y) {
		for (int x = 0; x < width; ++x) {
			int index = CELL(x, y, width);
            int cellVal = imageData[index];
            if (cellVal != 0) {
                // Check that blob is valid
                // Check index
                int id = equivalentBlob(blobIndex[index], equiv);
                bool idValid = false;
                for (int i = 0; !idValid && i != blobs.size(); ++i) {
                    if (id == blobs[i]->id) idValid = true;
                }

                if (idValid) {
                    int newVal = Blob::encodeId(id);
                    imageData[index] = newVal;
                } else {
                    imageData[index] = 0;
                }
            }
		}
	}
}

void ThermalBlobber::drawBlobBorders(ImagePtr image, vector<BlobPtr>& blobs) {
    ThermalImage imageData = (ThermalImage) (image->data);
    int width = image->width;

    for(vector<BlobPtr>::iterator it = blobs.begin(); it != blobs.end(); ++it) {
        BlobPtr blob = *it;
        for(int y = blob->miny; y <= blob->maxy; ++y) {
            imageData[CELL(blob->minx, y, width)] = MAX_THERMAL_VALUE;
            imageData[CELL(blob->maxx, y, width)] = MAX_THERMAL_VALUE;
        }
        for (int x = blob->minx; x <= blob->maxx; ++x) {
            imageData[CELL(x, blob->miny, width)] = MAX_THERMAL_VALUE;
            imageData[CELL(x, blob->maxy, width)] = MAX_THERMAL_VALUE;
        }
    }
}

void ThermalBlobber::saveData(vector<BlobPtr>& blobs, ImagePtr thresholdImg,
        ImagePtr encodedImage, ImagePtr distanceImage) {
	Lock lock(saveMutex);
	savedBlobs = blobs;
	savedEncodedImage = encodedImage;
	savedDistanceImage = distanceImage;
	savedThermalTimestamp = thermalTimestamp;
	savedThreshold = threshold;

	// Publish output image
	sensor_msgs::Image blobsImage = *(encodedImage->toROS());
	blobsImagePub.publish(blobsImage);
    sensor_msgs::Image thresholdImage = *(thresholdImg->toROS());
    thresholdImagePub.publish(thresholdImage);

    savedData = true;
}

void ThermalBlobber::updateThreshold(ImagePtr originalImage) {
	ImagePtr processImage = new Image(*originalImage);
    unsigned char possibleThres = thresholdCal->threshold(processImage);

    // Blob on this threshold
    vector<BlobPtr> blobs;
    vector<int> equiv;
    int blobIndex[thermalImage->width * thermalImage->height];
    simpleThreshold(processImage, possibleThres);
    findBlobs(processImage, blobs, equiv, blobIndex);
    trimBlobs(minimumArea, minimumWidth, blobs);

    double coverage = blobCoverage(blobs, processImage);
    if (coverage <= THRESHOLD_UPDATE_LIMIT) {
        threshold = learningRate * possibleThres + (1-learningRate) * threshold;
    }

    // Keep above minimum
    if (threshold < minimumThreshold) {
        threshold = minimumThreshold;
    }
}

inline int ThermalBlobber::equivalentBlob(int blobNo, vector<int> equiv) {
    while (equiv[blobNo] != blobNo) {
        blobNo = equiv[blobNo];
    }
    return blobNo;
}

inline bool ThermalBlobber::blobSizeOk(BlobPtr blob, int minArea, int minWidth) {
    return (blob->area() >= minArea && blob->height() >= minWidth && blob->width() >= minWidth);
}

bool ThermalBlobber::containedInside(BlobPtr toTest, vector<BlobPtr>& blobs) {
    bool inside = false;
    for(vector<BlobPtr>::iterator it = blobs.begin(); !inside && it != blobs.end(); ++it) {
        BlobPtr blob = *it;
        if (blob != toTest &&
            toTest->minx >= blob->minx && toTest->miny >= blob->miny &&
            toTest->maxx <= blob->maxx && toTest->maxy <= blob->maxy) {
            inside = true;
        }
    }
    return inside;
}

void ThermalBlobber::invertImage(ImagePtr image) {
    ThermalImage imageData = (ThermalImage) (image->data);
    unsigned int width = image->width;
    unsigned int height = image->height;

    // Currently 16 bit data
    unsigned short maxValue = 65535;

    for (unsigned int y = 0; y < height; ++y) {
        for (unsigned int x = 0; x < width; ++x) {
            imageData[CELL(x, y, width)] = maxValue - imageData[CELL(x, y, width)];
        }
    }
}

void ThermalBlobber::calculateBlobPoints(std::vector<BlobPtr>& blobs, int blobIndex[]) {
    // Perform distance calculation for this blob
    // Swiss Image data
    SwissImage distData = (SwissImage) distanceImage->data;
    //LOG("%s dist: w(%d) h(%d)\n", LOG_START, distanceImage->width, distanceImage->height);

    // Thermal image data
    ThermalImage thData = (ThermalImage) thermalImage->data;

    // Accumulative map
    map<int, double> accumulative;
    map<int, int> counts;

    // Variables
    int id;

    double totalDistImage = 0;
    int totalDistPixels = 0;

    // Iterate through each thermal point
    for(unsigned int x = 0; x < thermalImage->width; x++) {
        for(unsigned int y = 0; y < thermalImage->height; y++) {
            //LOG("%s Thermal Pos: (%d,%d)", LOG_START, x, y);

            // Get distance
            Point2D corrP = cameraCorrection->correctThermalPixel(Point2D(x, y));
            double dist = 0;
            if (corrP.x > 0 && corrP.y > 0 &&
                corrP.x < distanceImage->width && corrP.y < distanceImage->height) {
                dist = (double) distData[CELL((int) corrP.x, (int) corrP.y, distanceImage->width)];

                // Depths are in mm
                dist = dist / 1000.0;

                totalDistImage += dist;
                totalDistPixels++;
            }
            //LOG(" d:%lf, corrP:(%.0lf, %.0lf)", dist, corrP.x, corrP.y);

            // Add to accumulative
            if (dist > 0) {
                id = blobIndex[CELL(x, y, thermalImage->width)];
                //LOG(" id:%d", id);
                accumulative[id] += dist;
                counts[id]++;
            }

            //LOG("\n");
        }
    }

    // If the image is too close then we just set a default value
    double totalDistAverage = totalDistImage / totalDistPixels;
    //LOG("%s Over Dist Image: total(%.3lf), pixels(%d), average(%.3lf)\n", LOG_START, totalDistImage, totalDistPixels, totalDistAverage);

    // Process distance for each blob
    for(vector<BlobPtr>::iterator it = blobs.begin(); it != blobs.end(); ++it) {
        BlobPtr blob = *it;
        double averageDistance = (counts[blob->id] > 0) ? accumulative[blob->id] / counts[blob->id] : 0;
        if (totalDistAverage < 0.4) {
            averageDistance = 0.4;
        }

        blob->setCentre();
        blob->averageDistance = averageDistance;

        // Position the blob
        blob->p = cameraCorrection->projectThermal(blob->centre, blob->averageDistance);
        //LOG("%s \t Blob(%d), centre(%.0lf, %.0lf), p(%.3lf, %.3lf, %.3lf), dist(%.4lf)\n", LOG_START, blob->id, blob->centre.x, blob->centre.y, blob->p.x, blob->p.y,  blob->p.z, blob->averageDistance);
        //LOG("%s \t\t min: (%d, %d) max: (%d, %d)\n", LOG_START, blob->minx, blob->miny, blob->maxx, blob->maxy);
    }
}

} // namespace casualty
