/*!
\file tracker.cpp

\author Juraj Blaho
\modified by Michal Hradis 17.11.2008

\brief Tracker.
*/

#include <cfloat>
#include <list>

#include "BlahoTracker.h"

using namespace std;
vector< TrackedState> Tracker::NewFrame(IplImage *image){
	/*
	Preprocess frame
	*/
	cvThreshold( image,image, _value_threshold, 255, CV_THRESH_BINARY);
	
	if(_use_morphology){
		cvErode(image,image);
		cvDilate(image,image);
		cvDilate(image,image);
		cvErode(image,image);
	}
	
	/*
	Detect blobs
	*/
	int componentID = 0;

	vector< CvRect> newRectangles;

	for( int y = 0; y < image->height; y++){

		unsigned char *endPtr = cvPtr2D( image, y, image->width - 1);
		unsigned char *startPtr = cvPtr2D( image, y, 0);

		for( unsigned char * ptr = startPtr; ptr <= endPtr; ptr++){
			if( *ptr == 255 && componentID < 255){

				CvConnectedComp component;

				cvFloodFill( image, cvPoint( ptr - startPtr, y), cvScalarAll( componentID++),
                cvScalarAll(0), cvScalarAll(0), &component, CV_FLOODFILL_FIXED_RANGE);

				if( component.area > _area_threshold){
					newRectangles.push_back( component.rect);
				}
			}
		}
	}

	vector< TrackedState> newObjects;

	for( unsigned i = 0; i < newRectangles.size(); i++){

		double bestOverlap = -1.0;
		int bestOverlapBlob = -1;

		for( unsigned k = 0; k < lastBlobs.size(); k++){

			// compute overlap
			const double x1 = max( (double) newRectangles[ i].x, lastBlobs[ k].x - lastBlobs[ k].rx );
			const double x2 = min( (double)newRectangles[ i].x + newRectangles[ i].width, lastBlobs[ k].x + lastBlobs[ k].rx);
			const double y1 = max( (double)newRectangles[ i].y, lastBlobs[ k].y - lastBlobs[ k].ry);
			const double y2 = min( (double)newRectangles[ i].y + newRectangles[ i].height, lastBlobs[ k].y + lastBlobs[ k].ry);
			
			const double overlapWidth = x2 - x1;
			const double overlapHeight = y2 - y1;

			if( overlapWidth > 0 && overlapHeight > 0){

				const double overlapArea = overlapWidth * overlapHeight;

				const double overlap = overlapArea / ( newRectangles[ i].width * newRectangles[ i].height + 2 * lastBlobs[ k].rx * 2 * lastBlobs[ k].ry);

				if( overlap > bestOverlap){
					bestOverlap = overlap;
					bestOverlapBlob = (int) k;
				}

			}

		}

		// compute derivation
		if( bestOverlap > 0.15){
			newObjects.push_back( TrackedState( newRectangles[i], &lastBlobs[ bestOverlapBlob]));
		} else {
			newObjects.push_back( TrackedState( newRectangles[i]));
		}

	}

	lastBlobs = newObjects;
	return lastBlobs;
	
}
