/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install, copy or use the software.
//
// Copyright (C) 2009, Farhad Dadgostar
// Intel Corporation and third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of Intel Corporation may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/

#include "precomp.hpp"

#define ASD_INTENSITY_SET_PIXEL(pointer, qq) {(*pointer) = (unsigned char)qq;}

#define ASD_IS_IN_MOTION(pointer, v, threshold)	((abs((*(pointer)) - (v)) > (threshold)) ? true : false)

void CvAdaptiveSkinDetector::initData(IplImage *src, int widthDivider, int heightDivider)
{
	CvSize imageSize = cvSize(src->width/widthDivider, src->height/heightDivider);

	imgHueFrame = cvCreateImage(imageSize, IPL_DEPTH_8U, 1);
	imgShrinked = cvCreateImage(imageSize, IPL_DEPTH_8U, src->nChannels);
	imgSaturationFrame = cvCreateImage(imageSize, IPL_DEPTH_8U, 1);
	imgMotionFrame = cvCreateImage(imageSize, IPL_DEPTH_8U, 1);
	imgTemp = cvCreateImage(imageSize, IPL_DEPTH_8U, 1);
	imgFilteredFrame = cvCreateImage(imageSize, IPL_DEPTH_8U, 1);
	imgGrayFrame = cvCreateImage(imageSize, IPL_DEPTH_8U, 1);
	imgLastGrayFrame = cvCreateImage(imageSize, IPL_DEPTH_8U, 1);
	imgHSVFrame = cvCreateImage(imageSize, IPL_DEPTH_8U, 3);
};

CvAdaptiveSkinDetector::CvAdaptiveSkinDetector(int samplingDivider, int morphingMethod)
{
	nSkinHueLowerBound = GSD_HUE_LT;
	nSkinHueUpperBound = GSD_HUE_UT;

	fHistogramMergeFactor = 0.05;  	// empirical result
	fHuePercentCovered = 0.95;		// empirical result

	nMorphingMethod = morphingMethod;
	nSamplingDivider = samplingDivider;

	nFrameCount = 0;
	nStartCounter = 0;

	imgHueFrame = NULL;
	imgMotionFrame = NULL;
	imgTemp = NULL;
	imgFilteredFrame = NULL;
	imgShrinked = NULL;
	imgGrayFrame = NULL;
	imgLastGrayFrame = NULL;
	imgSaturationFrame = NULL;
	imgHSVFrame = NULL;
};

CvAdaptiveSkinDetector::~CvAdaptiveSkinDetector()
{
	cvReleaseImage(&imgHueFrame);
	cvReleaseImage(&imgSaturationFrame);
	cvReleaseImage(&imgMotionFrame);
	cvReleaseImage(&imgTemp);
	cvReleaseImage(&imgFilteredFrame);
	cvReleaseImage(&imgShrinked);
	cvReleaseImage(&imgGrayFrame);
	cvReleaseImage(&imgLastGrayFrame);
	cvReleaseImage(&imgHSVFrame);
};

void CvAdaptiveSkinDetector::process(IplImage *inputBGRImage, IplImage *outputHueMask)
{
	IplImage *src = inputBGRImage;

	int h, v, i, l;
	bool isInit = false;

	nFrameCount++;

	if (imgHueFrame == NULL)
	{
		isInit = true;
		initData(src, nSamplingDivider, nSamplingDivider);
	}

	unsigned char *pShrinked, *pHueFrame, *pMotionFrame, *pLastGrayFrame, *pFilteredFrame, *pGrayFrame;
	pShrinked = (unsigned char *)imgShrinked->imageData;
	pHueFrame = (unsigned char *)imgHueFrame->imageData;
	pMotionFrame = (unsigned char *)imgMotionFrame->imageData;
	pLastGrayFrame = (unsigned char *)imgLastGrayFrame->imageData;
	pFilteredFrame = (unsigned char *)imgFilteredFrame->imageData;
	pGrayFrame = (unsigned char *)imgGrayFrame->imageData;

	if ((src->width != imgHueFrame->width) || (src->height != imgHueFrame->height))
	{
		cvResize(src, imgShrinked);
		cvCvtColor(imgShrinked, imgHSVFrame, CV_BGR2HSV);
	}
	else
	{
		cvCvtColor(src, imgHSVFrame, CV_BGR2HSV);
	}

	cvSplit(imgHSVFrame, imgHueFrame, imgSaturationFrame, imgGrayFrame, 0);

	cvSetZero(imgMotionFrame);
	cvSetZero(imgFilteredFrame);

	l = imgHueFrame->height * imgHueFrame->width;

	for (i = 0; i < l; i++)
	{
		v = (*pGrayFrame);
		if ((v >= GSD_INTENSITY_LT) && (v <= GSD_INTENSITY_UT))
		{
			h = (*pHueFrame);
			if ((h >= GSD_HUE_LT) && (h <= GSD_HUE_UT))
			{
				if ((h >= nSkinHueLowerBound) && (h <= nSkinHueUpperBound))
					ASD_INTENSITY_SET_PIXEL(pFilteredFrame, h);

				if (ASD_IS_IN_MOTION(pLastGrayFrame, v, 7))
					ASD_INTENSITY_SET_PIXEL(pMotionFrame, h);
			}
		}
		pShrinked += 3;
		pGrayFrame++;
		pLastGrayFrame++;
		pMotionFrame++;
		pHueFrame++;
		pFilteredFrame++;
	}

	if (isInit)
		cvCalcHist(&imgHueFrame, skinHueHistogram.fHistogram);

	cvCopy(imgGrayFrame, imgLastGrayFrame);

	cvErode(imgMotionFrame, imgTemp);  // eliminate disperse pixels, which occur because of the camera noise
	cvDilate(imgTemp, imgMotionFrame);

	cvCalcHist(&imgMotionFrame, histogramHueMotion.fHistogram);

	skinHueHistogram.mergeWith(&histogramHueMotion, fHistogramMergeFactor);

	skinHueHistogram.findCurveThresholds(nSkinHueLowerBound, nSkinHueUpperBound, 1 - fHuePercentCovered);

	switch (nMorphingMethod)
	{
		case MORPHING_METHOD_ERODE :
			cvErode(imgFilteredFrame, imgTemp);
			cvCopy(imgTemp, imgFilteredFrame);
			break;
		case MORPHING_METHOD_ERODE_ERODE :
			cvErode(imgFilteredFrame, imgTemp);
			cvErode(imgTemp, imgFilteredFrame);
			break;
		case MORPHING_METHOD_ERODE_DILATE :
			cvErode(imgFilteredFrame, imgTemp);
			cvDilate(imgTemp, imgFilteredFrame);
			break;
	}

	if (outputHueMask != NULL)
		cvCopy(imgFilteredFrame, outputHueMask);
};


//------------------------- Histogram for Adaptive Skin Detector -------------------------//

CvAdaptiveSkinDetector::Histogram::Histogram()
{
	int histogramSize[] = { HistogramSize };
	float range[] = { GSD_HUE_LT, GSD_HUE_UT };
	float *ranges[] = { range };
	fHistogram = cvCreateHist(1, histogramSize, CV_HIST_ARRAY, ranges, 1);
	cvClearHist(fHistogram);
};

CvAdaptiveSkinDetector::Histogram::~Histogram()
{
	cvReleaseHist(&fHistogram);
};

int CvAdaptiveSkinDetector::Histogram::findCoverageIndex(double surfaceToCover, int defaultValue)
{
	double s = 0;
	for (int i = 0; i < HistogramSize; i++)
	{
		s += cvGetReal1D( fHistogram->bins, i );
		if (s >= surfaceToCover)
		{
			return i;
		}
	}
	return defaultValue;
};

void CvAdaptiveSkinDetector::Histogram::findCurveThresholds(int &x1, int &x2, double percent)
{
	double sum = 0;

	for (int i = 0; i < HistogramSize; i++)
	{
		sum += cvGetReal1D( fHistogram->bins, i );
	}

	x1 = findCoverageIndex(sum * percent, -1);
	x2 = findCoverageIndex(sum * (1-percent), -1);

	if (x1 == -1)
		x1 = GSD_HUE_LT;
	else
		x1 += GSD_HUE_LT;

	if (x2 == -1)
		x2 = GSD_HUE_UT;
	else
		x2 += GSD_HUE_LT;
};

void CvAdaptiveSkinDetector::Histogram::mergeWith(CvAdaptiveSkinDetector::Histogram *source, double weight)
{
	float myweight = (float)(1-weight);
	float maxVal1 = 0, maxVal2 = 0, *f1, *f2, ff1, ff2;

	cvGetMinMaxHistValue(source->fHistogram, NULL, &maxVal2);

	if (maxVal2 > 0 )
	{
		cvGetMinMaxHistValue(fHistogram, NULL, &maxVal1);
		if (maxVal1 <= 0)
		{
			for (int i = 0; i < HistogramSize; i++)
			{
				f1 = (float*)cvPtr1D(fHistogram->bins, i);
				f2 = (float*)cvPtr1D(source->fHistogram->bins, i);
				(*f1) = (*f2);
			}
		}
		else
		{
			for (int i = 0; i < HistogramSize; i++)
			{
				f1 = (float*)cvPtr1D(fHistogram->bins, i);
				f2 = (float*)cvPtr1D(source->fHistogram->bins, i);

				ff1 = ((*f1)/maxVal1)*myweight;
				if (ff1 < 0)
					ff1 = -ff1;

				ff2 = (float)(((*f2)/maxVal2)*weight);
				if (ff2 < 0)
					ff2 = -ff2;

				(*f1) = (ff1 + ff2);

			}
		}
	}
};