//---------------------------------------------------------------------------
#include <stdio>
#include <Dialogs.hpp>
#pragma hdrstop
#include "DetectorESF.h"
#include "ImgTools.h"

//---------------------------------------------------------------------------
#pragma package(smart_init)

//---------------------------------------------------------------------------
TImgQueue *TDetectorESF::Slides = NULL;

//---------------------------------------------------------------------------
void TDetectorESF::SetupSlides(TImgQueue *_Slides)
{
	Slides = _Slides;
}

//---------------------------------------------------------------------------
// Constants
//const int LOCALIZATION_STEP = 12*40;
//const int PRESENTATION_STEP = 20*40;

const int PROCESS_IMAGE_WIDTH = 600;
// restrictive conditions
const float MIN_BLOB_SIZE = 0.015f*0.03f;
const float MAX_BLOB_SIZE = 0.3f*0.5f;
const float MIN_BLOB_FLATTENING = 0.7f;
const float MAX_BLOB_ETENSION = 2;

//---------------------------------------------------------------------------
using namespace ee;

//---------------------------------------------------------------------------
TDetectorESF::TDetectorESF(int Type)
{
	Name = "ESF";
	imgColor = NULL;
	imgPrezentation = NULL;
	imgColorDown = NULL;
	imgColortmp = NULL;
	this->Type = Type;
}

//---------------------------------------------------------------------------
// Vola se pred prvnim zavolanim Do
void TDetectorESF::Init()
{
	Clear();
	Initialized = false;
	if (Type & ESF_TYPE_TRACKING)
	{
		TParameters Parameters;
		Parameters.push_back(TParameter("Person", "A"));
		InsertMessage(0, "Start Speaking", Parameters);
	}
}

//---------------------------------------------------------------------------
void TDetectorESF::InitA(int Width, int Height)
{
	string cfgfile = GetParameterString(Configuration, "Config").c_str();

	 // image size
	frameW = Width;
	frameH = Height;
	SAMPLE = 1;
	if (frameW/PROCESS_IMAGE_WIDTH!=1)
	{
		SAMPLE = (float)frameW/PROCESS_IMAGE_WIDTH;
	}
	FrameW2 = int(frameW/SAMPLE);
	FrameH2 = int(frameH/SAMPLE);
	imagesize = frameW*frameH;
	if (imgColor)
	{
		cvReleaseImage(&imgColor);
	}
	// create header for input image
	imgColor = cvCreateImageHeader(cvSize(frameW,frameH), IPL_DEPTH_8U, 4);
	if (imgColortmp)
	{
		cvReleaseImage(&imgColortmp);
	}
	// create header for input image
	imgColortmp = cvCreateImage(cvSize(frameW,frameH), IPL_DEPTH_8U, 3);

	if (imgColorDown)
	{
	   cvReleaseImage(&imgColorDown);
	}
	// resized image for processing
	imgColorDown  = cvCreateImage(cvSize(FrameW2,FrameH2), IPL_DEPTH_8U, 3);
	ConfigFile *cfgFile;
	try
	{
		cfgFile = new ConfigFile(cfgfile);
		Threshold = cfgFile->read( "Moving", 50 );
		Difference = cfgFile->read( "Difference", 10.0 );
		PresWidth = cfgFile->read( "PresWidth", 640 );
		PresHeight = cfgFile->read( "PresHeight", 480 );
		Points[0].x = (float)cfgFile->read( "PX1", -1 );
		Points[0].y = (float)cfgFile->read( "PY1", -1 );
		Points[1].x = (float)cfgFile->read( "PX2", -1 );
		Points[1].y = (float)cfgFile->read( "PY2", -1 );
		Points[2].x = (float)cfgFile->read( "PX3", -1 );
		Points[2].y = (float)cfgFile->read( "PY3", -1 );
		Points[3].x = (float)cfgFile->read( "PX4", -1 );
		Points[3].y = (float)cfgFile->read( "PY4", -1 );
		LOCALIZATION_STEP = cfgFile->read("LocalizationStep", 400);
		PRESENTATION_STEP = cfgFile->read("PresentationStep", 800);
	}
	catch (...)
	{
		//printf("Could not open config file: %s\n", cfgfile.c_str());
		ShowMessage(AnsiString("Could not open config file: ") + cfgfile.c_str());
		Threshold = 50;
		Difference = 10.0;
		PresWidth = 640;
		PresHeight = 480;
		LOCALIZATION_STEP = 400;
		PRESENTATION_STEP = 800;
	}

	if (imgPrezentation)
	{
		cvReleaseImage(&imgPrezentation);
	}
	// image with extracted presentation table
	imgPrezentation	= cvCreateImage(cvSize(PresWidth, PresHeight), IPL_DEPTH_8U, 3);

	memcpy(OrigPoints, Points, sizeof(CvPoint2D32f)*4);

	TParameters Parameters;
	Parameters.push_back(TParameter("X1", FloatToStr(Points[0].x)));
	Parameters.push_back(TParameter("Y1", FloatToStr(Points[0].y)));
	Parameters.push_back(TParameter("X2", FloatToStr(Points[1].x)));
	Parameters.push_back(TParameter("Y2", FloatToStr(Points[1].y)));
	Parameters.push_back(TParameter("X3", FloatToStr(Points[2].x)));
	Parameters.push_back(TParameter("Y3", FloatToStr(Points[2].y)));
	Parameters.push_back(TParameter("X4", FloatToStr(Points[3].x)));
	Parameters.push_back(TParameter("Y4", FloatToStr(Points[3].y)));
	InsertMessage(0, "Prezentation Board", Parameters);


	Points[0].x = Points[0].x/SAMPLE;  Points[0].y = Points[0].y/SAMPLE;
	Points[1].x = Points[1].x/SAMPLE;  Points[1].y = Points[1].y/SAMPLE;
	Points[2].x = Points[2].x/SAMPLE;  Points[2].y = Points[2].y/SAMPLE;
	Points[3].x = Points[3].x/SAMPLE;  Points[3].y = Points[3].y/SAMPLE;
	TeacherTracking.Init(imgColorDown, Points);
	// initialization of the presentation detection
	EPres.Init(PresWidth, PresHeight);

	Initialized = true;
}

//---------------------------------------------------------------------------
// Vola se pro kazdy snimek
void TDetectorESF::Do(int Time, TImg &Img)
{
	if (!Initialized)
	{
		InitA(Img.GetWidth(), Img.GetHeight());
	}

	double x,y, divx,divy;
	int Size;

	// zrojovy obrazek
	cvSetImageData(imgColor, Img.GetBitmap(), Img.GetLineWidth());

	// prezentace
	if ((Type & ESF_TYPE_PRESENTATION) && Time%PRESENTATION_STEP==0)
	{
		GetPrezentation(imgColor, imgPrezentation, OrigPoints);
		bool diff = false;
		diff = EPres.CalcDifference(imgPrezentation, Difference);
		// the presentation board is new (changed)
		if (diff)
		{
			TParameters Parameters;
			InsertMessage(Time, "Prezentation Exchange", Parameters);
			if (Slides && Time > 0)
			{
				IplImage *SrcSlide = imgPrezentation;
				//cvSaveImage("test.jpg", SrcSlide);
				TImg* Slide = new TImg(SrcSlide->width, SrcSlide->height);
				IplImage *Tmp = cvCreateImageHeader(cvSize(Slide->GetWidth(), Slide->GetHeight()), IPL_DEPTH_8U, 4);
				cvSetImageData(Tmp, Slide->GetBitmap(), Slide->GetLineWidth());
				cvCvtColor(SrcSlide, Tmp, CV_RGB2RGBA);
				//cvCvtColor(imgColor, Tmp, CV_RGBA2RGBA);
				cvReleaseImageHeader(&Tmp);
				Slides->push(Slide);
			}
		}
	}

	// tracking
	if ((Type & ESF_TYPE_TRACKING) && Time%LOCALIZATION_STEP==0)
	{
		if (SAMPLE == 1)
		{
			cvCvtColor(imgColor, imgColorDown, CV_RGBA2RGB);
		}
		else
		{
			cvCvtColor(imgColor, imgColortmp, CV_RGBA2RGB);
			// resize input image to do processing on the subsampled image
			cvResize(imgColortmp, imgColorDown, CV_INTER_LINEAR);
		}

		// teacher tracking //
		TeacherTracking.Localize(imgColorDown, x, y, divx, divy, Size, Threshold);
		bool Correct = true;
		if ((float)Size/imagesize<MIN_BLOB_SIZE)
		{
			printf("Noise object %f\n",(float)Size/imagesize);
			Correct = false;
		}
		if ((float)Size/imagesize>MAX_BLOB_SIZE)
		{
			printf("Large object %f\n", (float)Size/imagesize);
			Correct = false;
		}
		if (Correct)
		{
			x*=SAMPLE;	y*=SAMPLE;	Size*=SAMPLE*SAMPLE;
			divx*=SAMPLE*SAMPLE;	 divy*=SAMPLE*SAMPLE;

			TParameters Parameters;
			Parameters.push_back(TParameter("Object", "1"));
			Parameters.push_back(TParameter("CenterX", FloatToStr(x)));
			Parameters.push_back(TParameter("CenterY", FloatToStr(y)));
			Parameters.push_back(TParameter("DivergenceX", FloatToStr(divx)));
			Parameters.push_back(TParameter("DivergenceY", FloatToStr(divy)));
			Parameters.push_back(TParameter("Size", FloatToStr(Size)));
			InsertMessage(Time, "Teacher", Parameters);
		}
	}
}

void GetPrezentation(IplImage *imgColor, IplImage *imgPrezentation, CvPoint2D32f *Points)
{
	CvPoint2D32f Dest[4];
	int owidth = imgPrezentation->width;
	int oheight = imgPrezentation->height;
	CvPoint2D32f ImagePoint, OutputPoint;
	Dest[0].x = 0;
	Dest[0].y = 0;
	Dest[1].x = owidth;
	Dest[1].y = 0;
	Dest[2].x = owidth;
	Dest[2].y = oheight;
	Dest[3].x = 0;
	Dest[3].y = oheight;
	c_Homography Projection;
	Projection.computeHomo4(Points, Dest);

	for (int j=0; j < oheight; j++)
	{
		for (int i=0; i < owidth; i++)
		{
			OutputPoint.x = i;
			OutputPoint.y = j;
			Projection.UtoX(&OutputPoint, &ImagePoint);
			int r,g,b;
			//bilinear((unsigned char *)imgColor->imageData, imgColor->width, imgColor->height, ImagePoint.x, ImagePoint.y, b, g, r);

			int Pixel = Bilinear(imgColor->imageData, imgColor->width, imgColor->height, imgColor->widthStep, ImagePoint.x, ImagePoint.y);
			b = Pixel & 0xff;
			g = (Pixel & 0xff00) >> 8;
			r = (Pixel & 0xff0000) >> 16;
			/*
			if (j > 10 && j < 100 && i > 10 & i < 100)
			{
				r = g = b = 255;
			}
			*/
			((unsigned char *)(imgPrezentation->imageData + j*imgPrezentation->widthStep))[i*imgPrezentation->nChannels + 0]=b; // B
			((unsigned char *)(imgPrezentation->imageData + j*imgPrezentation->widthStep))[i*imgPrezentation->nChannels + 1]=g; // G
			((unsigned char *)(imgPrezentation->imageData + j*imgPrezentation->widthStep))[i*imgPrezentation->nChannels + 2]=r; // R
		}
	}
}

