/*
 * File         : thresholdCal.cpp
 * Author       : Timothy Wiley
 * Version      :
 * Created      : 31/03/2010
 * Description  : Calculate the threshold for thermal images
 * Copyright    :
 */

#include <limits>

#include "thermal_blobber/thresholdCal.h"

using namespace std;
using namespace crosbot;

namespace casualty {

typedef unsigned char* ThermalImage;

#define ATTRIB_BLOBBER "blobber"
#define ATTRIB_THERMAL "thermal"
#define LOG_START "_ThresholdCal :: "

ThresholdCal::ThresholdCal()
{
}

ThresholdCal::~ThresholdCal() {
}

unsigned char ThresholdCal::threshold(ImagePtr image) {
	ImagePtr processImage = new Image(*image, Image::Mono8); //image->toFormat(CASIMAGE_FORMAT_MONO8);
    generateHistogram(processImage, pi);
    //unsigned short threshold = ostuThreshold(pi) << 8;
    unsigned char threshold = ostuThreshold(pi);

    return threshold;
}

void ThresholdCal::generateHistogram(ImagePtr image, double pi[]) {
    ThermalImage imageData = (ThermalImage) (image->data);
    int numPixels = image->width * image->height;

    // Clear pixel data
    for (int i = 0;  i != NO_GREY_LEVELS; ++i) {
        pi[i] = 0;
    }

    // Count
    for (int i = 0;  i != numPixels; ++i) {
        ++pi[imageData[i]];
    }

    // Calculate probability
    for (int i = 0; i != NO_GREY_LEVELS; ++i) {
        pi[i] = pi[i] / numPixels;
    }
}

unsigned char ThresholdCal::ostuThreshold(double pi[]) {
    //unsigned short threshold = 0;
    unsigned char threshold = 0;

    // equation variables
    double wk = 0;
    double uk = 0;
    double ut = 0;
    double sigma2;
    double maxSigma2 = 0;

    // Calculate constant ut
    ut = 0;
    for(int i = 0; i != NO_GREY_LEVELS; ++i) {
        ut += i * pi[i];
    }

    // Run max algorithm
    wk = 0;
    uk = 0;
    maxSigma2 = numeric_limits<double>::min();
    threshold = 0;
    double numerator;
    double denominator;
    for(int k = 0; k != NO_GREY_LEVELS; ++k) {
        // Update sums
        wk = wk + pi[k];
        uk = uk + k * pi[k];

        // Calculate sigma2
        numerator = ut * wk - uk;
        numerator *= numerator;
        denominator = wk * (1 - wk);
        sigma2 = numerator / denominator;

        // do max
        if (sigma2 >= maxSigma2) {
            maxSigma2 = sigma2;
            threshold = k;
        }
    }

    return threshold;
}

} // namespace casualty
