// VD_Core.cpp : Defines the initialization routines for the DLL.
//

#include "stdafx.h"
#include "VD_Core.h"

#ifdef _DEBUG
#define new DEBUG_NEW
#endif



// CVD_CoreApp

BEGIN_MESSAGE_MAP(CVD_CoreApp, CWinApp)
END_MESSAGE_MAP()

// CVD_CoreApp construction

CVD_CoreApp::CVD_CoreApp()
{
	// TODO: add construction code here,
	// Place all significant initialization in InitInstance
}


// The one and only CVD_CoreApp object

CVD_CoreApp theApp;


// CVD_CoreApp initialization

BOOL CVD_CoreApp::InitInstance()
{
	CWinApp::InitInstance();

	return TRUE;
}

//vypocet casove diference v ms mezi dvema casy
unsigned int CalcTimeDiff(FILETIME &t1, FILETIME &t2)
{
	unsigned int res;
	ULARGE_INTEGER a;
	memcpy(&a.QuadPart,&t1,sizeof(a));

	ULARGE_INTEGER b;
	memcpy(&b.QuadPart,&t2,sizeof(b));

	ULARGE_INTEGER c;
	c.QuadPart = a.QuadPart - b.QuadPart;

	c.QuadPart /= 10000;
	memcpy(&res, &c.LowPart,sizeof(res));
	return res;
}

//vzdalenost mezi dvema body na snimku
int CalcDistance(int y1, int y2)
{
	int length = (int) cameraSettings.height * (tan(cameraSettings.ver_angle_min + (IMAGE_HEIGHT - y1) /cameraSettings.pix_in_one_rad) 
			- tan(cameraSettings.ver_angle_min + (IMAGE_HEIGHT - y2) /cameraSettings.pix_in_one_rad));
	if(length < 0) length = - length;
	return length;
}

//delka vozidla
double CalcLength(int y1, int y2)
{
	double car_height = 150*(IMAGE_HEIGHT - y1)/IMAGE_HEIGHT;
	double length = (int)cameraSettings.height * (tan(cameraSettings.ver_angle_min + (IMAGE_HEIGHT - y1) /cameraSettings.pix_in_one_rad) 
			- tan(cameraSettings.ver_angle_min + (IMAGE_HEIGHT - y2) /cameraSettings.pix_in_one_rad));
	if(length < 0) length = - length;
	//and now subtract the gap behind car
	length -= car_height*tan(cameraSettings.ver_angle_min + (IMAGE_HEIGHT - y1) /cameraSettings.pix_in_one_rad);
	return length;
}

//detekce hran snimku
void EdgeDetection(IplImage *bg, IplImage *im, IplImage *shad,IplImage *th_im)
{
	//hrany popredi
	cvZero(temp);
	cvCopy(im,temp);
	cvLaplace(temp,temp_16S);
	cvConvertScale(temp_16S,ForeEdges);
	cvAnd(ForeEdges,th_im,ForeEdges);

	
	//hrany pozadi
	cvZero(temp);
	cvCopy(bg,temp);
	cvLaplace(temp,temp_16S2);
	cvConvertScale(temp_16S2,BackEdges);
	cvAnd(BackEdges,th_im,BackEdges);
	cvThreshold(BackEdges,BackEdgesThr,50,255,CV_THRESH_BINARY);

	//staticke hrany
	cvDiv(image_gs,bg_gs,image_ratio,100);
	cvZero(temp);
	cvDiv(ForeEdges,BackEdges,CEdges,100);
	cvSub(image_ratio,CEdges,temp,BackEdgesThr);
	cvThreshold(temp,staticEdges,40,255,CV_THRESH_BINARY_INV);
	cvAnd(staticEdges,BackEdgesThr,staticEdges);
	
	//platne hrany
	cvSub(ForeEdges,BackEdges,CEdges);
	cvThreshold(CEdges,movingEdges,30,255,CV_THRESH_BINARY);
	
	return;
}

//detekce mozneho stinu
void ShadowDetection(IplImage *bg, IplImage *im, IplImage *shad, IplImage *th_im = 0)
{	
	cvConvertScale(image_rgb,temp_rgb32F);
	
	cvSplit(temp_rgb32F,tB,tG,tR,0);
	
	cvDiv(tB,tG,temp_32F);
	cvDiv(tB,tR,temp_32F2);

	cvSplit(bg_Avg,temp_32F3,tG,tR,0);

	cvDiv(temp_32F3,tG,tG);
	cvDiv(temp_32F3,tR,tR);

	cvSub(temp_32F,tG,tG);
	cvSub(temp_32F2,tR,tR);
	cvSub(temp_32F3,tB,tB);

	cvThreshold(tG,temp,0,255,CV_THRESH_BINARY);
	cvThreshold(tR,temp2,0,255,CV_THRESH_BINARY);
	cvThreshold(tB,temp3,0,255,CV_THRESH_BINARY);
	
	cvZero(shad);
	cvAnd(temp,temp2,shad,th_im);
	cvAnd(shad,temp3,shad,th_im);	
}

//segmentace vozidel
void FindCarContours2(IplImage* m, IplImage* shad)
{
	
	if( mem_storage==NULL ) {
		mem_storage = cvCreateMemStorage(0);
	} else {
		cvClearMemStorage(mem_storage);
	}
		
	const CvScalar CVX_WHITE = CV_RGB(0xff,0xff,0xff);
	const CvScalar CVX_BLACK = CV_RGB(0x00,0x00,0x00);

	CvSeq* c;
	CvSeq* cIn;
	int numCars;
	CvRect carRect;
	
	cvCopy(m,maskT);
	cvZero(carMask);
	cvZero(rem);

	//eroze masky kvuli eliminaci hran stinu
	cvSetImageROI(maskT,cvRect(0,maskT->height/4,maskT->width,maskT->height/2));
	cvSetImageROI(temp2,cvRect(0,maskT->height/4,maskT->width,maskT->height/2));
	cvErode(maskT,temp2,0,2);
	cvSetImageROI(maskT,cvRect(0,(3*maskT->height)/4,maskT->width,maskT->height/4));
	cvSetImageROI(temp2,cvRect(0,(3*maskT->height)/4,maskT->width,maskT->height/4));
	cvErode(maskT,temp2,0,3);
	cvResetImageROI(maskT);
	cvResetImageROI(temp2);
	cvSub(shad,temp2,shadowEdges);
	cvAnd(shadowEdges,movingEdges,shadowEdges);
	cvSub(movingEdges,shadowEdges,movingEdges);

	//pouzite pro relaxaci vozidel
	cvAbsDiff(image_rgb,bg_rgb,temp_rgb);
	cvSplit(temp_rgb,cB,cG,cR,0);
	cvAdd(cB,cG,cG);
	cvAdd(cG,cR,cR);
	
	//vyhlazeni masky
	cvSmooth(staticEdges,maskSt,CV_BLUR,6,6);
	cvSmooth(movingEdges,maskIn,CV_BLUR,6,6);
	cvSub(maskIn,maskSt,smoothedEdges);
	
	cvThreshold(smoothedEdges,rem,30,255,CV_THRESH_TOZERO);
	cvThreshold(smoothedEdges,CEdges,50,255,CV_THRESH_BINARY);
	cvMorphologyEx( CEdges, CEdges, 0, 0, CV_MOP_CLOSE);
	cvAdd(rem,cR,rem);
	cvSub(rem,shad,rem);
	
	if( mem_storage==NULL ) {
		mem_storage = cvCreateMemStorage(0);
	} else {
		cvClearMemStorage(mem_storage);
	}

	//SEGMENTACE MASKY
	CvContourScanner scanner = cvStartFindContours(maskT,mem_storage,sizeof(CvContour),CV_RETR_EXTERNAL,CV_CHAIN_APPROX_SIMPLE);
	while( (c = cvFindNextContour( scanner )) != NULL ) 
	{
		double mask_len = cvContourPerimeter( c );
		double q = (maskT->height + maskT->width)/10;									//nastaveni
		//ELIMINACE MALYCH SEGMENTU
		numCars = 0;
		
		if( mask_len > q ) {			
			
			if( mem_storageIn==NULL ) {
				mem_storageIn = cvCreateMemStorage(0);
			} else {
				cvClearMemStorage(mem_storageIn);
			}	
			
			CvRect bb = cvBoundingRect(c);

			//SEGMENTACE HRANOVEHO SNIMKU UVNITR SEGMENTU MASKY
			cvZero(maskTemp);
			cvDrawContours(maskTemp,c,CVX_WHITE,CVX_WHITE,0,CV_FILLED,8);
			cvAnd(CEdges,maskTemp,maskIn);
			CvContourScanner scannerIn = cvStartFindContours(maskIn,mem_storageIn,sizeof(CvContour),CV_RETR_EXTERNAL,CV_CHAIN_APPROX_SIMPLE);
			while( (cIn = cvFindNextContour( scannerIn )) != NULL ) 
			{
				double car_len = cvContourPerimeter( cIn );
				
				double q = (maskTemp->height + maskTemp->width)/15;	

				if( car_len > q ) numCars++;		
				else cvSubstituteContour( scannerIn, NULL );	

			}
			
			contoursIn = cvEndFindContours( &scannerIn );
			
			if(numCars > 0) 
			{
				cvDrawContours(carMask,contoursIn,CVX_WHITE,CVX_WHITE,1,CV_FILLED,8);
				
				double Tr=80;
				cvAnd(rem,maskTemp,temp);
				cvSub(temp,carMask,temp);
				cvThreshold(temp,temp,Tr,255,CV_THRESH_BINARY);
				
				if( mem_storageIn==NULL ) {
					mem_storageIn = cvCreateMemStorage(0);
				} else {
					cvClearMemStorage(mem_storageIn);
				}	
				
				//RELAXACE OBJEKTU
				CvContourScanner scannerIn = cvStartFindContours(temp,mem_storageIn,sizeof(CvContour),CV_RETR_EXTERNAL,CV_CHAIN_APPROX_SIMPLE);
				while( (cIn = cvFindNextContour( scannerIn )) != NULL ) 
				{
					cvZero(maskIn);
					cvDrawContours(maskIn,cIn,CVX_WHITE,CVX_WHITE,1,CV_FILLED,8);
					cvDilate(maskIn,temp3);
					cvAnd(temp3,carMask,temp3);
					if(cvCountNonZero(temp3)>2) cvOr(carMask,maskIn,carMask);
				}
			
				cvEndFindContours( &scannerIn );
			}
		}
	}
	
	cvEndFindContours( &scanner );
		
}

//URCENI OBSAZENOSTI JIZDNICH PRUHU
void Occupancy(IplImage *carmask)
{
	
	CvPoint* points = (CvPoint*)malloc( 2 * sizeof(points[0]));
	CvMat pointMat = cvMat( 1, 2, CV_32SC2, points );
	float line[4];
	CvPoint pt1;
	double d,t;

	int i,index = -1;
	double dev;
	int change=0;
	list<Lane>::iterator Iter,Iter2;

	//FIXNI PRUHY
	if(algSettings & FIXEDLANES) {
		Iter = fixedLanes.begin();
		while ( Iter != fixedLanes.end() ) {
			points[0].x = Iter->x1;
			points[0].y = Iter->y1;
			points[1].x = Iter->x2;
			points[1].y = Iter->y2;
			
			cvFitLine( &pointMat, CV_DIST_L1, 1, 0.01, 0.01, line );
			

			if((line[1]!= 0) && (line[0]!=0)) {
				double k,q;
				k = line[1]/line[0];
				q = line[3] - k*line[2];
				pt1.y = Iter->counting_line;
				pt1.x = (pt1.y - q)/k;
						
				cvZero(temp);
				cvLine(temp,pt1,pt1,CV_RGB(255,255,255));
				cvAnd(temp,carmask,temp);
				if(cvCountNonZero(temp)>0) 
					Iter->occupancy = 0.998*Iter->occupancy + 0.002;
				else
					Iter->occupancy = 0.998*Iter->occupancy;
			}
			++Iter;
		}
	}
	//AUTOMATICKE PRUHY
	else {
		Iter = autoLanes.begin();
		while ( Iter != autoLanes.end() ) {
			points[0].x = Iter->x1;
			points[0].y = Iter->y1;
			points[1].x = Iter->x2;
			points[1].y = Iter->y2;
			
			cvFitLine( &pointMat, CV_DIST_L1, 1, 0.01, 0.01, line );
			

			if((line[1]!= 0) && (line[0]!=0)) {
				double k,q;
				k = line[1]/line[0];
				q = line[3] - k*line[2];
				pt1.y = Iter->counting_line;
				pt1.x = (pt1.y - q)/k;
						
				cvZero(temp);
				cvLine(temp,pt1,pt1,CV_RGB(255,255,255));
				cvAnd(temp,carmask,temp);
				if(cvCountNonZero(temp)>0) 
					Iter->occupancy = 0.998*Iter->occupancy + 0.002;
				else
					Iter->occupancy = 0.998*Iter->occupancy;
			}
			++Iter;
		}
	}
	free(points);
}

//VYPOCET HUSTOTY A DOPRAVNIHO TOKU
void TraficStats()
{
	list<Lane>::iterator Iter;
	int i;
	if((algSettings & USELANES) && (algSettings & FIXEDLANES)) {
		for ( Iter = fixedLanes.begin( ) ; Iter != fixedLanes.end( ) ; Iter++ )  
			if(Iter->length > 0)
				Iter->density = 1000*Iter->occupancy/Iter->length;
		for (i = 0,Iter = fixedLanes.begin( ) ; (Iter != fixedLanes.end( )) && (i<NUMBER_OF_LANES) ; i++, Iter++ ) {
			density[i] = Iter->density;
			if(Iter->speed > 0) flow[i] = density[i]*Iter->speed;
		}
	}


	if((algSettings & USELANES) && !(algSettings & FIXEDLANES)) {
		for ( Iter = autoLanes.begin( ) ; Iter != autoLanes.end( ) ; Iter++ ) 
			if(Iter->length > 0)
				Iter->density = 1000*Iter->occupancy/Iter->length;
		for (i = 0,Iter = autoLanes.begin( ) ; (Iter != autoLanes.end( )) && (i<NUMBER_OF_LANES) ; i++, Iter++ ) {
			density[i] = Iter->density;
			if(Iter->speed > 0) flow[i] = density[i]*Iter->speed;
		}
	}


}

//ALR - AUTOMATICKA DETEKCE PRUHU, ROVNEZ URCUJE PRISLUSNOST VOZIDLA DO PRUHU
void LaneDetection(Car &car)
{
	const double MAX_DEV = IMAGE_WIDTH*1.5/NUMBER_OF_LANES;
		
	CvPoint* points = (CvPoint*)malloc( car.path.size() * sizeof(points[0]));
	CvMat pointMat = cvMat( 1, car.path.size(), CV_32SC2, points );
	float line[4];
	CvPoint pt1, pt2;
	double d,t;
	double alfa;
	list<CvPoint>::iterator Iter2;
	int i = 0;
	for ( Iter2 = car.path.begin( ) ; Iter2 != car.path.end( ) ; Iter2++,i++ ) {
		points[i].x = Iter2->x;
		points[i].y = Iter2->y;
	}
	cvFitLine( &pointMat, CV_DIST_L1, 1, 0.01, 0.01, line );
	free(points);


	if((line[1]!= 0) && (line[0]!=0)) {
		double k,q;
		k = line[1]/line[0];
		q = line[3] - k*line[2];
		pt1.y = temp->height/4;
		pt1.x = (pt1.y - q)/k;
		pt2.y = temp->height;
		pt2.x = (pt2.y - q)/k;

		list<Lane>::iterator Iter,Iter2;
		int i,index = -1;
		double dev,dev2, mindev = MAX_DEV;
		int change=0;
		if(algSettings & FIXEDLANES) {
			Iter2 = fixedLanes.end();
			for ( Iter = fixedLanes.begin( ) ; Iter != fixedLanes.end( ) ; Iter++ ) { 
				pt1.x = (Iter->y1 - q)/k;
				pt2.x = (Iter->y2 - q)/k;
				
				dev = fabs((pt1.x - Iter->x1) + (pt2.x - Iter->x2));
				
				if(dev < mindev) {
					mindev = dev;
					Iter2 = Iter;
				}
			}
			if(Iter2 != fixedLanes.end()) {
				//car.lane = index;
				if((Iter2->speed == -1) && (car.speed < 100)) Iter2->speed = car.speed;
				else Iter2->speed = 0.98 * Iter2->speed + 0.02*car.speed;
				if(Iter2->length == -1) Iter2->length = car.length;
				else Iter2->length = 0.98 * Iter2->length + 0.02*car.length;
			}
		}
		else {
			Iter2 = autoLanes.end();
			for ( Iter = autoLanes.begin( ) ; Iter != autoLanes.end( ) ; Iter++ ) {
				dev = fabs((pt1.x - Iter->x1) + (pt2.x - Iter->x2));
				
				if(dev < MAX_DEV) {
					Iter->x1 = Iter->x1*0.95 + 0.05*pt1.x;
					Iter->x2 = Iter->x2*0.95 + 0.05*pt2.x;
					change = 1;
					Iter->conf = Iter->conf*0.9 + 0.1*(100-dev); 
					if(dev < mindev) {
						mindev = dev;
						Iter2 = Iter;
					}
				}
			}
			
			if(Iter2 != fixedLanes.end() ) {
				//car.lane = index;
				if((Iter2->speed == -1) && (car.speed < 100)) Iter2->speed = car.speed;
				else Iter2->speed = 0.98 * Iter2->speed + 0.02*car.speed;
				if(Iter2->length == -1) Iter2->length = car.length;
				else Iter2->length = 0.98 * Iter2->length + 0.02*car.length;
			}
			
			if(change == 0) {
				Lane l;
				l.x1 = pt1.x;
				l.y1 = pt1.y;
				l.x2 = pt2.x;
				l.y2 = pt2.y;
				l.count = 1;
				l.conf = 0;
				l.speed = car.speed;
				l.occupancy = 0;
				l.length = 5;
				l.density = 0;
				cvClipLine(cvSize(IMAGE_WIDTH,IMAGE_HEIGHT),&pt1,&pt2);
				l.counting_line = 2*(pt2.y-pt1.y)/3 + pt1.y;
				autoLanes.push_back(l);				
			}
		}
		
	}
}

//SERAZENI AUTOMATICKYCH PRUHU ZLEVA DOPRAVA
void LaneEval() {
	list<Lane>::iterator Iter,Iter2;
		
	int i;
	if(algSettings & USELANES) {
		if(algSettings & FIXEDLANES)
			for ( i = 0,Iter = fixedLanes.begin( ) ; Iter != fixedLanes.end( ) ; Iter++,i++ )
			
				avgSpeed[i] = Iter->speed;
			
		else {
			Lane temp;
			int ex;
			if(autoLanes.size() > NUMBER_OF_LANES) 
				do {					
					ex = 0;
					Iter2 = autoLanes.begin( );
					Iter = autoLanes.begin( );
					Iter++;
					for (; Iter != autoLanes.end( ) ; ++Iter,++Iter2 ) {
						if(Iter->conf > Iter2->conf) {
							temp = *Iter;
							*Iter = *Iter2;
							*Iter2 = temp;
							ex = 1;
						}
					}
				} while(ex!=0);
			do {					
				ex = 0;
				Iter2 = autoLanes.begin( );
				Iter = autoLanes.begin( );
				Iter++;
				for (i=0; (Iter != autoLanes.end( )) && (i < NUMBER_OF_LANES) ; ++Iter,++Iter2,i++ ) {
					if(Iter->x1 < Iter2->x1) {
						temp = *Iter;
						*Iter = *Iter2;
						*Iter2 = temp;
						ex = 1;
					}
				}
			} while(ex!=0);
			for ( i = 0,Iter = autoLanes.begin( ) ; (Iter != autoLanes.end( )) && (i < NUMBER_OF_LANES) ; Iter++,i++ )
				avgSpeed[i] = Iter->speed;
			
		}

	}
}

//OVERENI ALR JIZDNICH PRUHU, DETEKCE KRIZENI A BLIZKOSTI
void LaneValidation(){
	int er = 0;
	const double LINK_DEV = IMAGE_WIDTH*1.2/NUMBER_OF_LANES;
	const double CROSSLINK_DEV = IMAGE_WIDTH/(NUMBER_OF_LANES*1.5);
	list<Lane>::iterator Iter,Iter2;
	double dev, dev2,alfa;

	Iter = autoLanes.begin();
	while ( Iter != autoLanes.end() ) {
		Iter2 = ++Iter;
		while ( Iter2 != autoLanes.end() ) {
			dev = (Iter2->x1 - Iter->x1);
			dev2 = (Iter2->x2 - Iter->x2);
			er = 0;
			if(((dev < 0) && (dev2 > 0)) || ((dev > 0) && (dev2 < 0))) { 
				if(fabs(dev+dev2) < CROSSLINK_DEV) {
					alfa = Iter->conf/(Iter->conf + Iter2->conf);
					Iter->x1 = Iter->x1*alfa + (1-alfa)*Iter2->x1;
					Iter->x2 = Iter->x2*alfa + (1-alfa)*Iter2->x2;
					Iter->conf = Iter->conf*alfa + (1-alfa)*Iter2->conf;
					Iter->speed = Iter->speed*alfa + (1-alfa)*Iter2->speed;
					er = 2;
					
				}
				else {
					if((Iter->conf - Iter2->conf) > 30) er = 2;							
					
					if((Iter2->conf - Iter->conf) > 30) er = 1;
				}

			}
			else {
				if(fabs(dev+dev2) < LINK_DEV) {
					alfa = Iter->conf/(Iter->conf + Iter2->conf);
					Iter->x1 = Iter->x1*alfa + (1-alfa)*Iter2->x1;
					Iter->x2 = Iter->x2*alfa + (1-alfa)*Iter2->x2;
					Iter->conf = Iter->conf*alfa + (1-alfa)*Iter2->conf; 
					Iter->speed = Iter->speed*alfa + (1-alfa)*Iter2->speed;
					er = 2;
				}
			}
			if(er == 2) Iter2 = autoLanes.erase(Iter2);
			else ++Iter2;
			if(er == 1) {
				Iter = autoLanes.erase(Iter);
				break;
			}
		}
		if(er!=1) ++Iter;
	}
}

//CITAC VOZIDEL, URCUJE PRISLUSNOST KE TRIDE
void CarCounter(Car &car)
{
	if(!(algSettings & USELANES)) {
		if(((car.path.begin()->y < COUNTING_LINE) && (car.path.back().y > COUNTING_LINE) && (!car.counted)) ||
			((car.path.begin()->y > COUNTING_LINE) && (car.path.back().y < COUNTING_LINE) && (!car.counted))) {
			car.counted = true;
			vehCounter++;
			if(car.length > 15) truckCounter++;
			else carCounter++;
		}
	}
	else {
		if((algSettings & FIXEDLANES) && (car.lane>=0)) {
			if(((car.path.begin()->y < fixedLanes[car.lane].counting_line) && (car.path.back().y > fixedLanes[car.lane].counting_line) && (!car.counted)) ||
				((car.path.begin()->y > fixedLanes[car.lane].counting_line) && (car.path.back().y < fixedLanes[car.lane].counting_line) && (!car.counted))) {
				car.counted = true;
				vehCounter++;
				if(car.length > 15) truckCounter++;
				else carCounter++;
			}
		}
		if(!(algSettings & FIXEDLANES) && (car.lane>=0)) {
			if(((car.path.begin()->y < autoLanes[car.lane].counting_line) && (car.path.back().y > autoLanes[car.lane].counting_line) && (!car.counted)) ||
				((car.path.begin()->y > autoLanes[car.lane].counting_line) && (car.path.back().y < autoLanes[car.lane].counting_line) && (!car.counted))) {
				car.counted = true;
				vehCounter++;
				if(car.length > 15) truckCounter++;
				else carCounter++;
			}
		}
	}
}

//VYHODNOCENI SEGMENTOVANYCH KONTUR A CASOVE HLEDANI KORESPONDENCE
void EvalCarContours()
{
	cvCopy(carMask,maskT);

	const CvScalar CVX_WHITE = CV_RGB(0xff,0xff,0xff);

	CvSeq* c;
	CArray<Car> cars;
	CvMoments moments;
	double M00, M01, M10;

	actualCars.RemoveAll();

	Car car;
	//
	////DETEKCE VOZIDEL
	CvContourScanner scanner = cvStartFindContours(maskT,mem_storageCars,sizeof(CvContour),CV_RETR_EXTERNAL,CV_CHAIN_APPROX_SIMPLE);
	while( (c = cvFindNextContour( scanner )) != NULL ) 
	{			
		CvSeq* c_new;			

		cvZero(maskTemp);
		cvDrawContours(maskTemp,c,CVX_WHITE,CVX_WHITE,-1,CV_FILLED,8);

		cvMoments(maskTemp,&moments,1);
		M00 = cvGetSpatialMoment(&moments,0,0);
		M10 = cvGetSpatialMoment(&moments,1,0);
		M01 = cvGetSpatialMoment(&moments,0,1);
		car.x = (int)(M10/M00);
		car.y = (int)(M01/M00);		
		car.color = cvAvg(image_rgb,maskTemp);
		car.area = abs((int)cvContourArea(c));
		car.predecessor = false;
		car.vx = 0;
		car.vy = 0;
		car.path.push_back(cvPoint(car.x,car.y));
		car.firstTime = currentTime;
		car.duration = 0;
		car.borderRect = cvBoundingRect(c);
		car.length = CalcLength(car.borderRect.y,(car.borderRect.y + car.borderRect.height))/100;
		car.counted = false;
		car.compound = false;
		
	
		car.abandoned = 0;
		car.lane = -1;
		car.contour = cvCloneSeq(c);
		//pridani vozidla do databaze
		actualCars.Add(car);
		
		car.path.clear();
		
	}
	carContours = cvEndFindContours(&scanner);

	int darea,dx,dy,deviation,move_limit;
	double dcolor;

	int i = 0;
	//HLEDANI KORESPONDENCE VOZIDEL
	while(i<=prevCars.GetUpperBound()) {
		int bestDev = 1000;
		int bestInd = -1;
		for(int j=0; j<=actualCars.GetUpperBound();j++) {
			if(!actualCars[j].predecessor) {	
								
				dcolor = 0;
				dcolor += fabs(actualCars[j].color.val[0] / actualCars[j].color.val[2] - prevCars[i].color.val[0] / prevCars[i].color.val[2]);
				dcolor += fabs(actualCars[j].color.val[1] / actualCars[j].color.val[2] - prevCars[i].color.val[1] / prevCars[i].color.val[2]);				
				//predikce pozice
				if(prevCars[i].predecessor) {
					dx = abs(actualCars[j].x - (prevCars[i].x + prevCars[i].vx*(1+prevCars[i].abandoned)));
					dy = abs(actualCars[j].y - (prevCars[i].y + prevCars[i].vy*(1+prevCars[i].abandoned)));
				
					deviation = dx + dy + (int)50*dcolor;
					move_limit = (25*actualCars[j].y)/IMAGE_HEIGHT;
					if((dx < move_limit) && (dy < move_limit) && (dcolor < 0.8) && (deviation < bestDev)){						
						bestDev = deviation;
						bestInd = j;
					}				
				}
				else {
					dx = abs(actualCars[j].x - prevCars[i].x);
					dy = abs(actualCars[j].y - prevCars[i].y);
					deviation = dx + dy + (int)50*dcolor;
					move_limit = (50*actualCars[j].y)/IMAGE_HEIGHT;
					if((dx < move_limit) && (dy < move_limit) && (dcolor < 0.5) && (deviation < bestDev)) {						
						bestDev = deviation;
						bestInd = j;
					}
				}
	
				
			}
		}
		if(bestInd != -1) {
			//VYPOCET DYNAMIKY VOZIDLA PRI NALEZENI KORESPONDENCE
			actualCars[bestInd].vx = actualCars[bestInd].x - prevCars[i].x;
			actualCars[bestInd].vy = actualCars[bestInd].y - prevCars[i].y;
			actualCars[bestInd].path = prevCars[i].path;
			actualCars[bestInd].path.push_back(cvPoint(actualCars[bestInd].x,actualCars[bestInd].y));
			actualCars[bestInd].predecessor = true;
			actualCars[bestInd].firstTime = prevCars[i].firstTime;
			actualCars[bestInd].counted = prevCars[i].counted;
			actualCars[bestInd].prevRect = prevCars[i].borderRect;
			if(actualCars[bestInd].y < IMAGE_HEIGHT/3)
				actualCars[bestInd].length = prevCars[i].length;
			else
				actualCars[bestInd].length = 0.7*prevCars[i].length + 0.3*actualCars[bestInd].length;
			//speed calculation
			int distance = CalcDistance(actualCars[bestInd].y,actualCars[bestInd].path[0].y) /100;	//  /100(to meters)
			actualCars[bestInd].duration = (int) CalcTimeDiff(currentTime,actualCars[bestInd].firstTime);
			if(actualCars[bestInd].duration <= 0) actualCars[bestInd].speed = 0;
			else {
				actualCars[bestInd].speed = (3600*distance) / actualCars[bestInd].duration;			//*1000(conv to seconds)
			}
			prevCars.RemoveAt(i);	
		}
		else {
			if(prevCars[i].predecessor) {
				prevCars[i].abandoned++;
				if(prevCars[i].abandoned > ABANDONED_CARS_LEVEL) {
					//VYHODNOCENI VLIVU VOZIDLA PRI JEHO OPUSTENI SCENY
					if(algSettings & USELANES)
						LaneDetection(prevCars[i]);
					else {
						if(prevCars[i].x < IMAGE_WIDTH/2) 
							for(int n=0;n<3;n++) avgSpeed[n] = 0.9 * avgSpeed[n] + 0.1 * prevCars[i].speed;
						else
							for(int n=3;n<6;n++) avgSpeed[n] = 0.9 * avgSpeed[n] + 0.1 * prevCars[i].speed;
					}
					CarCounter(prevCars[i]);
					prevCars.RemoveAt(i);
				}
				else i++;
			}
			else prevCars.RemoveAt(i);			
		}
	}

	i = 0;	

	//HLEDANI ROZSIRENE KORESPONDENCE NENALEZENYCH VOZIDEL V DATABAZI S AKTUALNI MASKOU
	while(i<=prevCars.GetUpperBound()) {
		if((prevCars[i].predecessor) && (prevCars[i].abandoned==1)) {
			double dist,bestdist;
			int ind;
			bestdist = 0;
			ind = -1;
			CvPoint2D32f p = cvPoint2D32f(prevCars[i].x + prevCars[i].vx,prevCars[i].y + prevCars[i].vy);
			for(int m=0; m<=actualCars.GetUpperBound();m++) {
				dist = cvPointPolygonTest(actualCars[m].contour,p,1);	
				if((dist>bestdist) && (prevCars[i].y  > IMAGE_HEIGHT/3)) {
					bestdist = dist;
					ind = m;
				}
			}
			if(ind > -1) {
				if(!actualCars[ind].predecessor) {
					actualCars[ind].vx = actualCars[ind].x - prevCars[i].x;
					actualCars[ind].vy = actualCars[ind].y - prevCars[i].y;
					actualCars[ind].path = prevCars[i].path;
					actualCars[ind].path.push_back(cvPoint(actualCars[ind].x,actualCars[ind].y));
					actualCars[ind].predecessor = true;
					actualCars[ind].firstTime = prevCars[i].firstTime;
					actualCars[ind].counted = prevCars[i].counted;
					actualCars[ind].prevRect = prevCars[i].borderRect;
					//speed calculation
					int distance = CalcDistance(actualCars[ind].y,actualCars[ind].path[0].y) /100;	//  /100(to meters)
					actualCars[ind].duration = (int) CalcTimeDiff(currentTime,actualCars[ind].firstTime);
					if(actualCars[ind].duration <= 0) actualCars[ind].speed = 0;
					else {
						actualCars[ind].speed = (3600*distance) / actualCars[ind].duration;			//*1000(conv to seconds)
					}
					prevCars.RemoveAt(i);
				}
				else {
					if(actualCars[ind].area > 1.5*prevCars[i].area) {
						actualCars[ind].compound = true;
						//create a new found car
						cvZero(temp);
						cvDrawContours(temp,prevCars[i].contour,CVX_WHITE,CVX_WHITE,1,CV_FILLED,8);
						cvSetImageROI(temp,prevCars[i].borderRect);
						
						//POSUN KONTURY VOZIDLA V DATABAZI O VX
						cvFindContours(temp,mem_storageCars,&car.contour,sizeof(CvContour),CV_RETR_EXTERNAL,CV_CHAIN_APPROX_SIMPLE,
							cvPoint(prevCars[i].borderRect.x +prevCars[i].vx,prevCars[i].borderRect.y+ prevCars[i].vy));
						cvResetImageROI(temp);
						
						cvZero(temp);
						cvDrawContours(temp,car.contour,CVX_WHITE,CVX_WHITE,1,CV_FILLED,8);
						cvDrawContours(temp2,actualCars[ind].contour,CVX_WHITE,CVX_WHITE,1,CV_FILLED,8);
						cvAnd(temp,temp2,temp3);	
																		
						cvMoments(temp3,&moments,1);
						M00 = cvGetSpatialMoment(&moments,0,0);
						M10 = cvGetSpatialMoment(&moments,1,0);
						M01 = cvGetSpatialMoment(&moments,0,1);
						car.x = (int)(M10/M00);
						car.y = (int)(M01/M00);	
						car.color = cvAvg(image_rgb,temp3);
						
						//DETEKCE PRUNIKU VOZIDLA Z DATABAZE S MASKOU
						double length=1;
						cvSetImageROI(temp3,actualCars[ind].borderRect);
						scanner = cvStartFindContours(temp3,mem_storageCars,sizeof(CvContour),CV_RETR_EXTERNAL,CV_CHAIN_APPROX_SIMPLE,
							cvPoint(actualCars[ind].borderRect.x,actualCars[ind].borderRect.y));
						while( (c = cvFindNextContour( scanner )) != NULL ) 
						{
							if(cvContourPerimeter(c) > length){
								length = cvContourPerimeter(c);
								car.contour = cvCloneSeq(c);
							}							
						}
					
						cvResetImageROI(temp3);						
						//EXTRAKCE ZNOVUOBJEVENEHO VOZIDLA Z MASKY
						car.area = abs((int)cvContourArea(car.contour));
						car.predecessor = true;
						car.vx = car.x - prevCars[i].x;
						car.vy = car.y - prevCars[i].y;
						car.path = prevCars[i].path;
						car.path.push_back(cvPoint(car.x,car.y));
						car.firstTime = prevCars[i].firstTime;
						int distance = CalcDistance(car.y,car.path[0].y) /100;	//  /100(to meters)
						car.duration = (int) CalcTimeDiff(currentTime,car.firstTime);
						if(car.duration <= 0) car.speed = 0;
						else {
							car.speed = (3600*distance) / car.duration;			//*1000(conv to seconds)
						}
						car.borderRect = cvBoundingRect(car.contour,1);
						car.length = CalcLength(car.borderRect.y,(car.borderRect.y + car.borderRect.height))/100;
						if(car.y < IMAGE_HEIGHT/3)
							car.length = prevCars[i].length;
						else
							car.length = 0.7*prevCars[i].length + 0.3*car.length;
						car.counted = false;
					
						car.abandoned = 0;
						car.lane = -1;
						car.compound = false;
			
						actualCars.Add(car);
						car.path.clear();

						//KOREKCE MASKY
						cvSub(temp2,temp,temp3);
						cvMoments(temp3,&moments,1);
						M00 = cvGetSpatialMoment(&moments,0,0);
						M10 = cvGetSpatialMoment(&moments,1,0);
						M01 = cvGetSpatialMoment(&moments,0,1);
						actualCars[ind].x = (int)(M10/M00);
						actualCars[ind].y = (int)(M01/M00);	
						actualCars[ind].color = cvAvg(image_rgb,temp3);

						cvSetImageROI(temp3,actualCars[ind].borderRect);
						length=1;
						scanner = cvStartFindContours(temp3,mem_storageCars,sizeof(CvContour),CV_RETR_EXTERNAL,CV_CHAIN_APPROX_SIMPLE,
							cvPoint(actualCars[ind].borderRect.x,actualCars[ind].borderRect.y));
		
						while( (c = cvFindNextContour( scanner )) != NULL ) 
						{
							if(cvContourPerimeter(c) > length){
								length = cvContourPerimeter(c);
								actualCars[ind].contour = cvCloneSeq(c);
							}
						}
						cvResetImageROI(temp3);
						actualCars[ind].area = abs((int)cvContourArea(actualCars[ind].contour));;
						actualCars[ind].predecessor = true;
						actualCars[ind].borderRect = actualCars[ind].prevRect;
						actualCars[ind].path.erase(actualCars[ind].path.end()-1);
						//vypocet novych vx vy na predchozi auto
						actualCars[ind].vx = actualCars[ind].x - actualCars[ind].path[actualCars[ind].path.size()-1].x;
						actualCars[ind].vy = actualCars[ind].y - actualCars[ind].path[actualCars[ind].path.size()-1].y;
						actualCars[ind].borderRect.x+=actualCars[ind].vx;
						actualCars[ind].borderRect.y+=actualCars[ind].vy;
						actualCars[ind].path.push_back(cvPoint(actualCars[ind].x,actualCars[ind].y));
						actualCars[ind].firstTime = prevCars[i].firstTime;
						distance = CalcDistance(actualCars[ind].y,actualCars[ind].path[0].y) /100;	//  /100(to meters)
						actualCars[ind].duration = (int) CalcTimeDiff(currentTime,actualCars[ind].firstTime);
						if(actualCars[ind].duration <= 0) actualCars[ind].speed = 0;
						else {
							actualCars[ind].speed = (3600*distance) / actualCars[ind].duration;			//*1000(conv to seconds)
						}
						actualCars[ind].length = CalcLength(actualCars[ind].borderRect.y,(actualCars[ind].borderRect.y + actualCars[ind].borderRect.height))/100;
						if(actualCars[ind].y < IMAGE_HEIGHT/3)
							actualCars[ind].length = prevCars[i].length;
						else
							actualCars[ind].length = 0.7*prevCars[i].length + 0.3*actualCars[ind].length;
						actualCars[ind].counted = false;
					
						actualCars[ind].abandoned = 0;
						actualCars[ind].lane = -1;
						actualCars[ind].compound = false;

						prevCars.RemoveAt(i);
					}
					else i++;
				}
			}
			else i++;
		}
		else i++;
	}
	cvZero(carMask);
	for(int j = 0;j<=actualCars.GetUpperBound();j++)
		cvDrawContours(carMask,actualCars[j].contour,CVX_WHITE,CVX_WHITE,1,CV_FILLED,8);

	if((actualCars.GetUpperBound() >=0) && (algSettings & USELANES)) Occupancy(carMask);

	possCars.RemoveAll();
	possCars.Copy(actualCars);
	possCars.Append(prevCars);
	prevCars.RemoveAll();
	prevCars.Copy(possCars);
	
}

//SEGMENTACE OBJEKTU SVETEL, NOCNI REZIM
void FindCarLights(IplImage* m)
{
	cvCopy(m,maskTemp);

	CvSeq* contours = NULL;
	int center_x,center_y;

	const CvScalar CVX_WHITE = CV_RGB(0xff,0xff,0xff);

	if( mem_storage==NULL ) {
		mem_storage = cvCreateMemStorage(0);
	} else {
		cvClearMemStorage(mem_storage);
	}
	 
	CvSeq* c;	
	Light newLight;
	double MAX_PERIM;
	double MED_PERIM;

	CvMoments moments;
	double M00, M01, M10;
	
	lights.RemoveAll();

	//SEGMENTACE OBJEKTU SVETEL
	CvContourScanner scanner = cvStartFindContours(m,mem_storage,sizeof(CvContour),CV_RETR_EXTERNAL,CV_CHAIN_APPROX_SIMPLE);

	while( (c = cvFindNextContour( scanner )) != NULL ) 
	{
		double len = cvContourPerimeter( c );
		int ca = (int) cvContourArea(c);
		if(ca<0) ca = -ca;
		
		if(len<2)
			cvSubstituteContour( scanner, NULL );
		else {
			cvZero(maskTemp);
				
			cvDrawContours(maskTemp,c,CVX_WHITE,CVX_WHITE,-1,CV_FILLED,8);		
			cvMoments(maskTemp,&moments,1);
			M00 = cvGetSpatialMoment(&moments,0,0);
			M10 = cvGetSpatialMoment(&moments,1,0);
			M01 = cvGetSpatialMoment(&moments,0,1);
			center_x = (int)(M10/M00);
			center_y = (int)(M01/M00);

			MED_PERIM = (center_y*20)/IMAGE_HEIGHT + 20;
			MAX_PERIM = (center_y*70)/IMAGE_HEIGHT + 20;
			if(len > MED_PERIM) {
				//DETEKCE VETSI NEZ JEDNO SVETLO, TEDY MASKA NEBO ODRAZ
				CvBox2D box;
				box = cvMinAreaRect2(c);
				if(len < MAX_PERIM) {
					//DVE RUZNA NATOCENI OHRANICUJICIHO CTVERCE, KLASIFIKACE OBJEKTU
					if((fabs(box.angle)<5) && (box.size.height>2.5*box.size.width) && (box.size.height<7*box.size.width) && 
						(abs(box.center.x-center_x) < (box.size.height/15+1)) && (abs(box.center.y-center_y) < (box.size.width/8+1))) {
						newLight.area = ca/2;
						newLight.perim = len/2;
						newLight.bestCand = -1;
						newLight.bestDev = 1000;
						newLight.x = box.center.x-box.size.height/3;
						newLight.y = box.center.y;
						newLight.borderRect = cvRect(box.center.x-box.size.height/2,box.center.y-box.size.width/2,box.size.height/2,box.size.width);
						lights.Add(newLight);
						
						newLight.x = box.center.x+box.size.height/3;
						newLight.y = box.center.y;
						newLight.borderRect = cvRect(box.center.x,box.center.y-box.size.width/2,box.size.height/2,box.size.width);
						lights.Add(newLight);
					
					}
					else if((fabs(90 - box.angle)<5) && (box.size.width>2.5*box.size.height) && (box.size.width<7*box.size.height) && 
							(abs(box.center.x-center_x) < box.size.width/15+1) && (abs(box.center.y-center_y) < box.size.height/8+1)) {
						newLight.area = ca/2;
						newLight.perim = len/2;
						newLight.bestCand = -1;
						newLight.bestDev = 1000;
						newLight.x = box.center.x-box.size.width/3;
						newLight.y = box.center.y;
						newLight.borderRect = cvRect(box.center.x-box.size.width/2,box.center.y-box.size.height/2,box.size.width/2,box.size.height);
						lights.Add(newLight);
					
						newLight.x = box.center.x+box.size.width/3;
						newLight.y = box.center.y;
						newLight.borderRect = cvRect(box.center.x,box.center.y-box.size.height/2,box.size.width/2,box.size.height);
						lights.Add(newLight);
					
					}
					else cvSubstituteContour( scanner, NULL );
				}
				else  cvSubstituteContour( scanner, NULL );
			}
			else {
				
				newLight.area = ca;
				newLight.x = center_x;
				newLight.y = center_y;
				newLight.perim = len;
				newLight.bestCand = -1;
				newLight.bestDev = 1000;
				newLight.borderRect = cvBoundingRect(c);
				lights.Add(newLight);
			
			}
		}
	}
	
	contours = cvEndFindContours( &scanner );	
}

//PAROVANI JEDNOTLIVYCH SVETEL, NOCNI REZIM
int AssociateLights()
{
	
	Light l1,l2;
	
	int deviation = 0;
	int dx,dxc,dy,dPerim;
	int bestDev,bestInd;
	for(int i=0; i<=lights.GetUpperBound(); i++) {
		l1 = lights[i];
		bestDev = 1000;	
		bestInd = -1;
		if(l1.bestCand == -2) continue;
		for(int j=i+1; j<=lights.GetUpperBound(); j++) {
			l2 = lights[j];

			if((i!=j) && (l2.bestCand != -2)) {
			
				dx = abs(l2.x - l1.x);
				dxc = abs(dx - ((l1.y*IMAGE_WIDTH/11)/IMAGE_HEIGHT + IMAGE_WIDTH/100));	
				dy = abs(l2.y - l1.y);
				dPerim = abs(l2.perim - l1.perim);
				
				deviation = dxc + 5*dy + dPerim;	
				int perim = (l1.perim + l2.perim) / 2;

				if((dxc < 10) && (dy < 10) && (dx > dy) && (dPerim <= perim) && (deviation < 20) && (deviation < bestDev)) {		
					bestDev = deviation;
					bestInd = j;
				}
			}
		}
		if(bestInd >= 0) {
			l2 = lights[bestInd];
			if((bestDev < l1.bestDev) && (bestDev < l2.bestDev)) {
				lights[i].bestCand = bestInd;
				lights[bestInd].bestCand = i;
				lights[i].bestDev = bestDev;
				lights[bestInd].bestDev = bestDev;
			}
		}
	}
	
	Car car;
	int newCars = 0;
	int alx,aly,expy;
	bool reflection = false;

	const double ALLOWED_Y_COEF = 0.15;

	//POTVRZENI KORESPONDENCE A VYTVORENI OBJEKTU VOZIDLA
	for(int i=0; i<=lights.GetUpperBound(); i++) {
		if(lights[i].bestCand >= 0) {
			int ind = lights[i].bestCand;
			if(lights[ind].bestCand == i) {
				newCars++;
			
				lights[i].bestCand = -2;		
				lights[ind].bestCand = -2;
				
				if(lights[i].x < lights[ind].x) {
					car.carLights.left = lights[i];			
					car.carLights.right = lights[ind];
				}
				else {
					car.carLights.left = lights[ind];			
					car.carLights.right = lights[i];
				}
				car.x = (car.carLights.right.x + car.carLights.left.x) / 2;
				car.y = (car.carLights.right.y + car.carLights.left.y) / 2;
				car.width = car.carLights.right.x - car.carLights.left.x;

			
				car.height = car.width / 4;

				car.predecessor = false;
				car.vx = 0;
				car.vy = 0;
				car.path.push_back(cvPoint(car.x,car.y));
				car.firstTime = currentTime;
				car.duration = 0;
				car.length = 5;
				car.counted = false;
				car.borderRect=cvMaxRect(&lights[i].borderRect,&lights[ind].borderRect);
				car.area = car.borderRect.width*car.borderRect.height;
			
				car.lane = -1;
				car.speed = -1;
				car.abandoned = 0;
				possCars.Add(car);
				car.path.clear();
			}
			else {
				lights[i].bestCand = -1;
				lights[i].bestDev = 1000;
			}
		}
	}
	//ZDA JE ZKOUMANY OBJEKT VOZIDLO NEBO ODRAZ
	for(int j=0; j<=possCars.GetUpperBound(); j++) {
		reflection = false;
		
		for(int k=0; k<=possCars.GetUpperBound(); k++) {
			alx = (possCars[j].width > possCars[k].width) ? possCars[j].width/2 : possCars[k].width/2;
			expy = (int)(ALLOWED_Y_COEF * possCars[j].y);	//0 - 20 podle y souradnice testovanyho odrazu...2/5 moznej drift
			aly = (int)((double)expy*0.7);
			dx = (possCars[j].carLights.left.x < possCars[k].carLights.left.x) ? (possCars[k].carLights.left.x - possCars[j].carLights.left.x) : 0;
			dx += (possCars[j].carLights.right.x > possCars[k].carLights.right.x) ? (possCars[j].carLights.right.x - possCars[k].carLights.right.x) : 0;
			if((j!=k) && (dx < alx) && (abs(possCars[j].y-possCars[k].y-expy)<=aly))
				reflection = true;
			
			
		}
		if(!reflection) actualCars.Add(possCars[j]);
		
	}
	return newCars;
}

//CASOVE MONITOROVANI VOZIDEL V NOCNIM REZIMU
void TrackCars()
{
	int dx,dxc,dy,dPerim;
	int darea,move_limit,allowed_area;
	double dcolor;
	int deviation = 0;

	int i = 0;
	while(i<=prevCars.GetUpperBound()) {
		int bestDev = 1000;
		int bestInd = -1;
		for(int j=0; j<=actualCars.GetUpperBound();j++) {
			if(!actualCars[j].predecessor) {	
				darea = abs(actualCars[j].area - prevCars[i].area);
			
				if(prevCars[i].predecessor) {
					dx = abs(actualCars[j].x - (prevCars[i].x + prevCars[i].vx*(1+prevCars[i].abandoned)));
					dy = abs(actualCars[j].y - (prevCars[i].y + prevCars[i].vy*(1+prevCars[i].abandoned)));
					allowed_area = (actualCars[j].area > prevCars[i].area) ? (actualCars[j].area) : (prevCars[i].area);
					deviation = dx + dy + darea;
					move_limit = (25*actualCars[j].y)/IMAGE_HEIGHT;
					if((dx < move_limit) && (dy < move_limit) && (darea <= allowed_area) && (deviation < bestDev)) {							//nastavit podle rozliseni
						bestDev = deviation;
						bestInd = j;
					}			
				}
				else {
					dx = abs(actualCars[j].x - prevCars[i].x);
					dy = abs(actualCars[j].y - prevCars[i].y);
					deviation = dx + dy + darea;
					allowed_area = (actualCars[j].area > prevCars[i].area) ? (0.7*actualCars[j].area) : (0.7*prevCars[i].area);
					
					move_limit = (50*actualCars[j].y)/IMAGE_HEIGHT;
					if((dx < move_limit) && (dy < move_limit) && (darea < allowed_area) && (deviation < bestDev)) {							//nastavit podle rozliseni
						bestDev = deviation;
						bestInd = j;
					}
				}
	
			}
		}
		//DYNAMICKY POPIS VOZIDLA PRI NALEZENI KORESPONDENCE
		if(bestInd != -1) {
			actualCars[bestInd].vx = actualCars[bestInd].x - prevCars[i].x;
			actualCars[bestInd].vy = actualCars[bestInd].y - prevCars[i].y;
			actualCars[bestInd].path = prevCars[i].path;
			actualCars[bestInd].path.push_back(cvPoint(actualCars[bestInd].x,actualCars[bestInd].y));
			actualCars[bestInd].predecessor = true;
			actualCars[bestInd].firstTime = prevCars[i].firstTime;
			actualCars[bestInd].counted = prevCars[i].counted;
			//speed calculation
			int distance = CalcDistance(actualCars[bestInd].y,actualCars[bestInd].path[0].y) /100;	//  /100(to meters)
			actualCars[bestInd].duration = (int) CalcTimeDiff(currentTime,actualCars[bestInd].firstTime);
			if(actualCars[bestInd].duration <= 0) actualCars[bestInd].speed = 0;
			else {
				actualCars[bestInd].speed = (3600*distance) / actualCars[bestInd].duration;			//*1000(conv to seconds)
			}
			
			prevCars.RemoveAt(i);	
		}
		else {
			if(prevCars[i].predecessor) {
				prevCars[i].abandoned++;
				
				if(prevCars[i].abandoned > ABANDONED_CARS_LEVEL) {		
					if(algSettings & USELANES)
						LaneDetection(prevCars[i]);
					else {
						if(prevCars[i].x < IMAGE_WIDTH/2) 
							for(int n=0;n<3;n++) avgSpeed[n] = 0.9 * avgSpeed[n] + 0.1 * prevCars[i].speed;
						else
							for(int n=3;n<6;n++) avgSpeed[n] = 0.9 * avgSpeed[n] + 0.1 * prevCars[i].speed;
					}
					CarCounter(prevCars[i]);
					prevCars.RemoveAt(i);
				}
				else i++;
			}
			else prevCars.RemoveAt(i);
			
		}
	}

	const int CAR_TO_LIGHT_COEF = 4;
	cvZero(carMask);
	for(int i = 0;i<= actualCars.GetUpperBound();i++)
		cvRectangle(carMask,cvPoint(actualCars[i].borderRect.x,actualCars[i].borderRect.y),
			cvPoint(actualCars[i].borderRect.x + actualCars[i].borderRect.width ,actualCars[i].borderRect.y + CAR_TO_LIGHT_COEF*actualCars[i].borderRect.height),
			CV_RGB(255,255,255),CV_FILLED);
	

	if((actualCars.GetUpperBound() >=0) && (algSettings & USELANES)) Occupancy(carMask);
	
	possCars.RemoveAll();
	possCars.Copy(actualCars);
	possCars.Append(prevCars);
	prevCars.RemoveAll();
	prevCars.Copy(possCars);
}

//VYTVORENI MODELU POZADI
int BGCreate(CStringList &files)		
{
	CString fn;
	POSITION pos;

	for (pos = files.GetHeadPosition(); (pos != NULL) && (bg_count < BG_INIT);bg_count++) {
		fn = files.GetNext(pos);
		ImageLoad((char *)(LPCTSTR) fn);
		
		
		cvAcc(image_rgb,bg_Avg);
		cvSquareAcc(image_rgb,bg_SD);			
		cvAcc(image_rgb,bg_SDtemp);
	}

		
	//bg calculation
	cvConvertScale(bg_Avg,bg_Avg,(double)(1.0/(double)bg_count));
	//stand dev
	cvConvertScale(bg_SD, bg_SD,(double)(1.0/(double)bg_count));
	cvConvertScale(bg_SDtemp, bg_SDtemp,(double)(1.0/(double)bg_count));
	cvPow(bg_SDtemp,bg_SDtemp,2);
	cvSub(bg_SD,bg_SDtemp,bg_SD);
	cvPow(bg_SD,bg_SD,(double) 0.5);
	//POUZITI 2SIGMA JAKO VARIANCE BODU
	cvConvertScale(bg_SD, bg_SD, 2.0);							
	cvAddS( bg_SD, cvScalar( 1.0, 1.0, 1.0), bg_SD );
	cvConvertScale(bg_SD,bg_SD8U);
	cvSplit(bg_SD8U,bg_SDR,bg_SDG,bg_SDB,0);


	bg_rgb = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 3 );
	bg_gs = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 );

	cvConvertScale(bg_Avg,bg_rgb);
	cvCvtColor(bg_rgb,bg_gs,CV_BGR2GRAY);
	
	cvConvertScale(bg_rgb,shadow_stdf);
		
	return bg_count;
}

//AKTUALIZACE POZADI
void BGUpdate() 
{
	cvAbsDiff(image_rgb,bg_rgb,temp_rgb);
	cvSplit(temp_rgb,cR,cG,cB,0);
	cvConvertScale(bg_SD,bg_SD8U);
	cvSplit(bg_SD8U,bg_SDR,bg_SDG,bg_SDB,0);

	cvCmp(cR,bg_SDR,fg_mask,CV_CMP_GT);
	
	cvCmp(cG,bg_SDG,temp,CV_CMP_GT);
	cvOr(fg_mask,temp,fg_mask);

	cvCmp(cB,bg_SDB,temp,CV_CMP_GT);
	cvOr(fg_mask,temp,fg_mask);

	CvScalar a = cvAvg(bg_SD8U);

			
	cvSubRS(fg_mask,cvRealScalar(255.0),temp);

	//AKTUALIZACE ODCHYLKY	
	cvConvertScale(temp_rgb,temp_rgb2,2);
	cvRunningAvg(temp_rgb2,bg_SD,0.05);

	cvSub(temp,carMask,temp);
	cvRunningAvg(image_rgb,bg_Avg,0.05,temp);
	cvConvertScale(bg_Avg,bg_rgb);
	cvCvtColor(bg_rgb,bg_gs,CV_BGR2GRAY);	
}

//EXTRAKCE POPREDI SNIMKU
void FGExtract(IplImage *m)
{
	cvAbsDiff(image_rgb,bg_rgb,temp_rgb);
	cvSplit(temp_rgb,cB,cG,cR,0);
	//KOREKCE GLOBALNIHO JASU
	cvThreshold(cR,temp,30,255,CV_THRESH_BINARY);

	cvThreshold(cG,temp2,30,255,CV_THRESH_BINARY);
	cvOr(temp2,temp,temp);
	cvThreshold(cB,temp2,30,255,CV_THRESH_BINARY);
	cvOr(temp2,temp,temp3);
	
	cvSubRS(temp3,cvScalar(255.0),temp3);
	CvScalar bg_avg = cvAvg(bg_rgb,temp3);
	CvScalar im_avg = cvAvg(image_rgb,temp3);
	CvScalar diff;
	diff.val[0] = bg_avg.val[0] - im_avg.val[0];
	diff.val[1] = bg_avg.val[1] - im_avg.val[1];
	diff.val[2] = bg_avg.val[2] - im_avg.val[2];
	cvAddS(image_rgb,diff,temp_rgb);
	
	//PRAHOVANI DO MASKY
	cvAbsDiff(temp_rgb,bg_rgb,temp_rgb2);
	cvSplit(temp_rgb2,cB,cG,cR,0);

	cvThreshold(cR,temp,15,255,CV_THRESH_BINARY);
	cvThreshold(cG,temp2,15,255,CV_THRESH_BINARY);
	cvOr(temp2,temp,temp);
	cvThreshold(cB,temp2,15,255,CV_THRESH_BINARY);
	cvOr(temp2,temp,temp);
	cvSmooth(temp,m,CV_MEDIAN);
	
	cvCopy(temp_rgb,image_rgb);
}

//EXTRAKCE POPREDI V NOCNIM REZIMU
void FGExtractNight(IplImage *m)
{
		
	double minimum, maximum, threshold;

	cvSplit(image_rgb,cB,cG,cR,0);
	cvMax(image_gs,cR,image_gs);

	//*****LEFT side
	cvCopy(m,temp);
	cvSetImageROI(temp,cvRect(m->width/2,0,m->width/2,m->height));
	cvSetZero(temp);
	cvResetImageROI(temp);
	cvMinMaxLoc(image_gs, &minimum,&maximum,0,0,temp);
	
	threshold = (maximum-minimum < 25) ? minimum : (maximum - 25);

	cvThreshold(image_gs, temp2,threshold,255,CV_THRESH_BINARY);
	cvAnd(m,temp2,temp2);
	cvAnd(temp2,temp,temp2);

	//*****RIGHT side
	cvCopy(m,temp);
	cvSetImageROI(temp,cvRect(0,0,m->width/2,m->height));
	cvSetZero(temp);
	cvResetImageROI(temp);
	cvMinMaxLoc(image_gs, &minimum,&maximum,0,0,temp);
	
	threshold = (maximum-minimum < 25) ? minimum : (maximum - 25);

	cvThreshold(image_gs, temp3,threshold,255,CV_THRESH_BINARY);
	cvAnd(m,temp3,temp3);
	cvAnd(temp3,temp,temp3);
	cvZero(m);
	cvOr(temp3,temp2,m);	
}

//INICIALIZACE POTREBNYCH VELICIN
void Init()
{
	dayMode = true;
	bg_count = 0;
	carCounter = 0;
	vehCounter = 0;
	carCounter = 0;
	truckCounter = 0;
	prevCars.RemoveAll();
	autoLanes.clear();
	fixedLanes.clear();

	shadowVector_x = 0;
	shadowVector_y = 0;

	if( mem_storageCars==NULL ) {
		mem_storageCars = cvCreateMemStorage(0);
	} else {
		cvClearMemStorage(mem_storageCars);
	}

	for(int i=0;i<NUMBER_OF_LANES;i++)
		avgSpeed[i] = -1;
	for(int i=0;i<NUMBER_OF_LANES;i++)
		density[i] = 0;
	for(int i=0;i<NUMBER_OF_LANES;i++)
		flow[i] = 0;

	bg_Avg = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_32F, 3 );
	bg_Avg8U = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 3 );

	bg_SD = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_32F, 3 );
	bg_SD8U = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 3 );

	bg_SDR = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 );
	bg_SDG = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 );
	bg_SDB = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 );
	bg_SDtemp = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_32F, 3 );

	cR = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 );
	cG = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 );
	cB = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 );
	
	image_proc = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 ); 
	image_ratio = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 ); 
	image_gs = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 );
	result_temp = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 3 );
	result_temp2 = cvCreateImage( cvSize(VIEW_WIDTH,VIEW_HEIGHT), IPL_DEPTH_8U, 3 );
	result_temp3 = cvCreateImage( cvSize(VIEW_WIDTH,VIEW_HEIGHT), IPL_DEPTH_8U, 3 );
	result_temp4 = cvCreateImage( cvSize(VIEW_WIDTH,VIEW_HEIGHT), IPL_DEPTH_8U, 1 );
	result = cvCreateImage( cvSize(VIEW_WIDTH,VIEW_HEIGHT), IPL_DEPTH_8U, 3 );

	image_rgb = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 3 );
	image_prev = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 );
	fg_mask = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 );
	temp_rgb = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 3 );
	temp_rgb2 = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 3 );
	temp_rgb3 = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 3 );
	temp_16S = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_16S, 1 );
	temp_16S2 = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_16S, 1 );
	temp = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 );
	temp2 = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 );
	temp3 = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 );
	temp_32F = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_32F, 1 );
	temp_32F2 = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_32F, 1 );
	temp_32F3 = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_32F, 1 );
	temp_32S = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_32S, 1 );
	temp_32S2 = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_32S, 1 );
	temp_rgb32F = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_32F, 3 );

	tR = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_32F, 1 );
	tG = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_32F, 1 );
	tB = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_32F, 1 );

	BackEdges = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 );
	ForeEdges = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 );
	BackEdgesThr = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 );
	ForeEdgesThr = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 );
	CEdges = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 );
	CEdgesThr = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 );
	staticEdges = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 );
	movingEdges = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 );
	smoothedEdges = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 );
	mask = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 );
	maskT = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 );
	maskTemp = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 );
	maskIn = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 );
	maskSt = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 );
	rem = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 );
	road = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 );
	roadSigns = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 );
	carMask = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 );		

	movementMap_x = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_32F, 1 );
	movementMap_y = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_32F, 1 );
	movementMap = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_32F, 1 );

	shadowMask = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 );
	shadowEdges = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 1 );
	shadow_stdf = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_32F, 3 );
	shadow_std = cvCreateImage( cvSize(IMAGE_WIDTH,IMAGE_HEIGHT), IPL_DEPTH_8U, 3 );


	initialized = true;
	return;
}

void Update()
{
	bg_count++;
	if(bg_count % 5 == 0)	BGUpdate();
	if((bg_count % 10 == 0) && (algSettings & USELANES) && !(algSettings & FIXEDLANES))  LaneValidation();
}

//NACENI NOVEHO SNIMKU
void ImageLoad(char *FileName)
{
	if(image_rgb_hr) cvReleaseImage(&image_rgb_hr);
	
	image_rgb_hr = cvLoadImage(FileName);
	resCoef_x = image_rgb_hr->width/IMAGE_WIDTH;
	resCoef_y = image_rgb_hr->height/IMAGE_HEIGHT;
	if(image_gs) cvCopy(image_gs,image_prev);
	
	cvResize(image_rgb_hr, image_rgb);
		
	cvCvtColor(image_rgb,image_gs,CV_BGR2GRAY);
	
	return;
}

//NACTENI CASOVEHO RAZITKA
void TimeLoad(char *FileName, SYSTEMTIME &time)
{
	SYSTEMTIME st;
		
	CString str(FileName);
	CString str2;
	
	str2 = str.Right(22);
	str = str2.Left(4);
	time.wYear = atoi((char*)(LPCTSTR)str);
	str = str2.Mid(4,2);
	time.wMonth = atoi((char*)(LPCTSTR)str);
	str = str2.Mid(6,2);
	time.wDay = atoi((char*)(LPCTSTR)str);
	str = str2.Mid(9,2);
	time.wHour = atoi((char*)(LPCTSTR)str);
	str = str2.Mid(11,2);
	time.wMinute = atoi((char*)(LPCTSTR)str);
	str = str2.Mid(13,2);
	time.wSecond = atoi((char*)(LPCTSTR)str);
	str = str2.Mid(15,3);
	time.wMilliseconds = atoi((char*)(LPCTSTR)str); 
	
	SystemTimeToFileTime(&time, &currentTime);

}

//NACTENI INI SOUBORU
void LoadIniFile()
{
	CIniReader iniReader((char*)(LPCTSTR)iniFile);
	cameraSettings.height = iniReader.ReadFloat("CAMERA", "height", 1000);
	cameraSettings.ver_angle = iniReader.ReadFloat("CAMERA", "ver_angle", 1.3);
	cameraSettings.angle_range_x = iniReader.ReadFloat("CAMERA", "angle_range_x", 1);
	cameraSettings.angle_range_y = iniReader.ReadFloat("CAMERA", "angle_range_y", 0.5);
	
	cameraSettings.road_dist = cameraSettings.height * tan(cameraSettings.ver_angle);
	cameraSettings.air_dist = cameraSettings.height / cos(cameraSettings.ver_angle);
	cameraSettings.pix_in_one_rad = IMAGE_HEIGHT / cameraSettings.angle_range_y;
	cameraSettings.ver_angle_min = cameraSettings.ver_angle - cameraSettings.angle_range_y/2;
		
	
	CvSize s = cvGetSize(image_rgb_hr);
	int hdiv = s.height/IMAGE_HEIGHT;
	int wdiv = s.width/IMAGE_WIDTH;

	char track[50];
	CString str;
	int i = 0;
	Lane la;
	int a,b,c,d,e;
	str.Format("tr%d",i);
	while((iniReader.ReadString(track,"LANES",(char*)(LPCTSTR)str,"") > 0) && (i<NUMBER_OF_LANES)) {
		sscanf_s(track,"%d;%d;%d;%d;%d;",&a,&b,&c,&d,&e);
		la.x1 = a/resCoef_x;
		la.y1 = b/resCoef_y;
		la.x2 = c/resCoef_x;
		la.y2 = d/resCoef_y;
		la.counting_line = e/resCoef_y;
		la.index = i;
		la.speed = -1;
		la.occupancy = 0;
		la.length = 5;
		la.density = 0;
		fixedLanes.push_back(la);
		i++;
		str.Format("tr%d",i);
	}	
}

//NASTAVENI VYSTUPNIHO SNIMKU PRO GRAFICKE ROZHRANNI
int ArrangeResult(int day)
{
	cvZero(result_temp);
	if(day) {	

		if(algSettings & CARCONTOURS) cvDrawContours(result_temp,carContours,CV_RGB(0,255,0),CV_RGB(0,255,0),1,1,8);
		
		if(algSettings & ROADCONTOURS)	cvDrawContours(result_temp,roadContours,CV_RGB(222,0,0),CV_RGB(222,0,0),1,1,8);
			
		if(algSettings & ROADSIGNS)	cvSet(result_temp,CV_RGB(222,0,0),roadSigns);

		if((algSettings & LANES) && (algSettings & USELANES)) {
			list<Lane>::iterator Iter;
			if(algSettings & FIXEDLANES)
				for ( Iter = fixedLanes.begin( ) ; Iter != fixedLanes.end( ) ; Iter++ ) 
					cvLine( result_temp, cvPoint((int)Iter->x1,(int)Iter->y1), cvPoint((int)Iter->x2,(int)Iter->y2), CV_RGB(0,255,0), 1, 8, 0 );
			else {
				int k = 0;			
				for ( Iter = autoLanes.begin( ) ; (Iter != autoLanes.end( )) && (k<NUMBER_OF_LANES) ; Iter++,k++ ) 
					cvLine( result_temp, cvPoint((int)Iter->x1,(int)Iter->y1), cvPoint((int)Iter->x2,(int)Iter->y2), CV_RGB(0,255,0), 1, 8, 0 );
				while(Iter != autoLanes.end( )) {
					cvLine( result_temp, cvPoint((int)Iter->x1,(int)Iter->y1), cvPoint((int)Iter->x2,(int)Iter->y2), CV_RGB(128,255,128), 1, 8, 0 );
					Iter++; 
				}
			}
		}

		cvResize(result_temp,result_temp2,CV_INTER_AREA);
		cvResize(image_rgb_hr,result_temp3,CV_INTER_AREA);
		cvOr(result_temp3,result_temp2,result);
		//*************************************************
		//UPRAVA ROZLISENI PRO VYSTUPNI SNIMEK
		int resMult = VIEW_WIDTH / IMAGE_WIDTH;
		
		if(algSettings & SHADOWMASK) {
			cvResize(shadowMask,result_temp4,CV_INTER_AREA);
			cvSet(result,CV_RGB(50,50,50),result_temp4);
		}

		CvFont *font = new CvFont();
		CString str;
		cvInitFont(font,CV_FONT_HERSHEY_SIMPLEX,0.35,0.35);
		
		if(algSettings & (PATH | SPEED | LENGTH | CARRECTS)) {
			for(int i = 0; i<=actualCars.GetUpperBound();i++) {
				if((actualCars[i].speed < 10) && (actualCars[i].predecessor))
					cvRectangle(result,cvPoint(actualCars[i].borderRect.x*resMult,actualCars[i].borderRect.y*resMult),
						cvPoint((actualCars[i].borderRect.x + actualCars[i].borderRect.width)*resMult,(actualCars[i].borderRect.y + actualCars[i].borderRect.height)*resMult),
						CV_RGB(220,0,0),1,8);
				else
					cvRectangle(result,cvPoint(actualCars[i].borderRect.x*resMult,actualCars[i].borderRect.y*resMult),
						cvPoint((actualCars[i].borderRect.x + actualCars[i].borderRect.width)*resMult,(actualCars[i].borderRect.y + actualCars[i].borderRect.height)*resMult),
						CV_RGB(0,220,220),1,8);
				cvRectangle(result,cvPoint(actualCars[i].x*resMult-2,actualCars[i].y*resMult-2),cvPoint(actualCars[i].x*resMult+2,actualCars[i].y*resMult+2),CV_RGB(255,0,0),1);
				//speed
				if((algSettings & SPEED) && (actualCars[i].predecessor)) {	//only for cars spotted for more than 1 period
					cvRectangle(result,cvPoint(actualCars[i].borderRect.x*resMult,actualCars[i].borderRect.y*resMult),
						cvPoint(actualCars[i].borderRect.x*resMult + 30,actualCars[i].borderRect.y*resMult + 12),
						CV_RGB(0,220,220),CV_FILLED,8);
					str.Format("S:%d",(int)actualCars[i].speed);
					cvPutText(result,(LPCTSTR)str,cvPoint(actualCars[i].borderRect.x*resMult,actualCars[i].borderRect.y*resMult + 11),font,CV_RGB(0,0,0));
				}
				//path
				if((algSettings & PATH) && (actualCars[i].predecessor)) {
					for(int j=1;j<actualCars[i].path.size();j++)
						cvLine(result,cvPoint(actualCars[i].path[j-1].x*resMult,actualCars[i].path[j-1].y*resMult),cvPoint(actualCars[i].path[j].x*resMult,actualCars[i].path[j].y*resMult),CV_RGB(255,0,0),1);
					cvRectangle(result,cvPoint((actualCars[i].path[0].x)*resMult-2,actualCars[i].path[0].y*resMult-2),
						cvPoint(actualCars[i].path[0].x*resMult+2,actualCars[i].path[0].y*resMult+2),
						CV_RGB(255,0,0),1,8);
				}
				//length
				if(algSettings & LENGTH) {
					cvRectangle(result,cvPoint(actualCars[i].borderRect.x*resMult,actualCars[i].borderRect.y*resMult+12),
						cvPoint(actualCars[i].borderRect.x*resMult + 30,actualCars[i].borderRect.y*resMult + 24),
						CV_RGB(0,220,220),CV_FILLED,8);
					str.Format("L:%2.0f",actualCars[i].length);
					cvPutText(result,(LPCTSTR)str,cvPoint(actualCars[i].borderRect.x*resMult,actualCars[i].borderRect.y*resMult + 23),font,CV_RGB(0,0,0));			
				}
			}			
		}
		delete font;
	}
	else {

		if((algSettings & LANES) && (algSettings & USELANES)) {
			list<Lane>::iterator Iter;
			if(algSettings & FIXEDLANES)
				for ( Iter = fixedLanes.begin( ) ; Iter != fixedLanes.end( ) ; Iter++ ) 
					cvLine( result_temp, cvPoint((int)Iter->x1,(int)Iter->y1), cvPoint((int)Iter->x2,(int)Iter->y2), CV_RGB(0,255,0), 1, 8, 0 );
			else{
				int k = 0;			
				for ( Iter = autoLanes.begin( ) ; (Iter != autoLanes.end( )) && (k<NUMBER_OF_LANES) ; Iter++,k++ ) 
					cvLine( result_temp, cvPoint((int)Iter->x1,(int)Iter->y1), cvPoint((int)Iter->x2,(int)Iter->y2), CV_RGB(0,255,0), 1, 8, 0 );
				while(Iter != autoLanes.end( )) {
					cvLine( result_temp, cvPoint((int)Iter->x1,(int)Iter->y1), cvPoint((int)Iter->x2,(int)Iter->y2), CV_RGB(128,255,128), 1, 8, 0 );
					Iter++; 
				}
			}
		}
			
		cvResize(result_temp,result_temp2,CV_INTER_AREA);
		cvResize(image_rgb_hr,result_temp3,CV_INTER_AREA);
		cvOr(result_temp3,result_temp2,result);

		CvFont *font = new CvFont();
		CString str;
		cvInitFont(font,CV_FONT_HERSHEY_SIMPLEX,0.35,0.35);

		int resMult = VIEW_WIDTH / IMAGE_WIDTH;
		if(algSettings & (PATH | SPEED | CARRECTS)) {
			for(int i = 0; i<=actualCars.GetUpperBound();i++) {
				if((actualCars[i].speed < 10) && (actualCars[i].predecessor))
					cvRectangle(result,cvPoint(actualCars[i].borderRect.x*resMult,actualCars[i].borderRect.y*resMult),
						cvPoint((actualCars[i].borderRect.x+actualCars[i].borderRect.width)*resMult,(actualCars[i].borderRect.y + actualCars[i].borderRect.height)*resMult),
						CV_RGB(220,0,0),1,8);
				else
					cvRectangle(result,cvPoint(actualCars[i].borderRect.x*resMult,actualCars[i].borderRect.y*resMult),
						cvPoint((actualCars[i].borderRect.x+actualCars[i].borderRect.width)*resMult,(actualCars[i].borderRect.y + actualCars[i].borderRect.height)*resMult),
						CV_RGB(0,220,220),1,8);
				cvRectangle(result,cvPoint(actualCars[i].x*resMult-2,actualCars[i].y*resMult-2),cvPoint(actualCars[i].x*resMult+2,actualCars[i].y*resMult+2),CV_RGB(255,0,0),1);
				//speed
				if((algSettings & SPEED) && (actualCars[i].predecessor)) {	//only for cars spotted for more than 1 period
					cvRectangle(result,cvPoint((actualCars[i].x - actualCars[i].width/2)*resMult,(actualCars[i].y - actualCars[i].height/2)*resMult),
						cvPoint((actualCars[i].x - actualCars[i].width/2)*resMult + 30,(actualCars[i].y - actualCars[i].height/2)*resMult-12),
						CV_RGB(0,220,220),CV_FILLED,8);
					str.Format("S:%d",(int)actualCars[i].speed);
					cvPutText(result,(LPCTSTR)str,cvPoint((actualCars[i].x - actualCars[i].width/2)*resMult,(actualCars[i].y - actualCars[i].height/2)*resMult-1),font,CV_RGB(0,0,0));
				}
				//path
				if((algSettings & PATH) && (actualCars[i].predecessor)) {
					for(int j=1;j<actualCars[i].path.size();j++)
						cvLine(result,cvPoint(actualCars[i].path[j-1].x*resMult,actualCars[i].path[j-1].y*resMult),cvPoint(actualCars[i].path[j].x*resMult,actualCars[i].path[j].y*resMult),CV_RGB(255,0,0),1);
					cvRectangle(result,cvPoint((actualCars[i].path[0].x)*resMult-2,actualCars[i].path[0].y*resMult-2),
						cvPoint(actualCars[i].path[0].x*resMult+2,actualCars[i].path[0].y*resMult+2),
						CV_RGB(255,0,0),1,8);
				}
			}			
		}
		delete font;
	}
	
	return 1;
}

//EXTERNI FUNKCE INICIALIZACE Z GRAFICKEHO PROSTREDI
extern "C" __declspec(dllexport) int VD_Initialize(CStringList &files, CString ini)
{
	if(initialized) Clean();
	Init();
	
	if(BGCreate(files) == 0) return 0;

	iniFile = ini;
	LoadIniFile();

	return 1;
}

//EXTERNI FUNKCE PRO VOLANI ALGORITMU
extern "C" __declspec(dllexport) IplImage* VD_Core(char *FileName, Results &results, int settings)
{
	if(!initialized) Init();
	ImageLoad(FileName);
	TimeLoad(FileName,results.time);
	
	algSettings = settings;
	if(algSettings & RELOADINI) LoadIniFile();

	CvScalar avg = cvAvg(bg_rgb);
	double avgBg = 0.33*avg.val[0] + 0.33*avg.val[1] + 0.33*avg.val[2];
	if((dayMode) && (avgBg < DAY_NIGHT_SEP - 2)) dayMode = false;
	if(!(dayMode) && (avgBg > DAY_NIGHT_SEP + 2)) dayMode = true;

	if(dayMode) {
		FGExtract(mask);
	
		//ELIMINACE HORNI CASTI MASKY
		cvSetImageROI(mask,cvRect(0,0,IMAGE_WIDTH,IMAGE_HEIGHT/4));
		cvSetZero(mask);
		cvResetImageROI(mask);

		ShadowDetection(bg_gs,image_gs,shadowMask,mask);
		
		EdgeDetection(bg_gs,image_gs,shadowMask,mask);

		FindCarContours2(mask,shadowMask);
	
		EvalCarContours();
				
		ArrangeResult(1);
	}
	else {
		
		cvSub(image_gs,bg_gs,temp);
		cvThreshold(temp,mask,50,255,CV_THRESH_BINARY);

		cvSetImageROI(mask,cvRect(0,0,IMAGE_WIDTH,IMAGE_HEIGHT/4));
		cvSetZero(mask);
		cvResetImageROI(mask);
		
		FGExtractNight(mask);
				
		possCars.RemoveAll();
		FindCarLights(mask);

		actualCars.RemoveAll();
		
		while(AssociateLights() > 0) {
			possCars.RemoveAll();
		};
		
		TrackCars();
	
		ArrangeResult(0);
	}
	
	Update();
	TraficStats();
	if(algSettings & USELANES) LaneEval();
		
	//ULOZENI VYSLEDKU PRO GRAFICKE ROZHRANNI
	results.carCount = carCounter;
	results.vehCount = vehCounter;
	results.trCount = truckCounter;
	results.actCount = actualCars.GetSize();
	
	for(int i=0;i<NUMBER_OF_LANES;i++)
		results.avgSpeed[i] = avgSpeed[i];
	for(int i=0;i<NUMBER_OF_LANES;i++) {
		results.density[i] = density[i];
		if(density[i] <= 7) results.LOS[i] = 'A';
		if((density[i] > 7) && (density[i] <= 11)) results.LOS[i] = 'B';
		if((density[i] > 11) && (density[i] <= 16)) results.LOS[i] = 'C';
		if((density[i] > 16) && (density[i] <= 22)) results.LOS[i] = 'D';
		if((density[i] > 22) && (density[i] <= 28)) results.LOS[i] = 'E';
		if(density[i] > 28) results.LOS[i] = 'F';
	}
	for(int i=0;i<NUMBER_OF_LANES;i++)
		results.flow[i] = flow[i];

	if(result) return result;
	else return image_rgb_hr;

}

//VYCISTENI PAMETI PO UKONCENI
void Clean() 
{
	initialized = false;
	cvReleaseImage(&bg_Avg);
	cvReleaseImage(&bg_Avg8U);

	cvReleaseImage(&bg_SD);
	cvReleaseImage(&bg_SD8U);

	cvReleaseImage(&bg_SDR);
	cvReleaseImage(&bg_SDG);
	cvReleaseImage(&bg_SDB);
	cvReleaseImage(&bg_SDtemp);

	cvReleaseImage(&cR);
	cvReleaseImage(&cG);
	cvReleaseImage(&cB);
	
	cvReleaseImage(&image_proc); 
	cvReleaseImage(&image_gs);
	cvReleaseImage(&image_ratio);
	cvReleaseImage(&result);
	cvReleaseImage(&result_temp);
	cvReleaseImage(&result_temp2);
	cvReleaseImage(&result_temp3);
	cvReleaseImage(&result_temp4);

	cvReleaseImage(&image_rgb);
	cvReleaseImage(&image_prev);
	cvReleaseImage(&fg_mask);
	cvReleaseImage(&temp_rgb);
	cvReleaseImage(&temp_rgb2);
	cvReleaseImage(&temp_rgb3);
	cvReleaseImage(&temp_16S);
	cvReleaseImage(&temp_16S2);
	cvReleaseImage(&temp);
	cvReleaseImage(&temp2);
	cvReleaseImage(&temp3);
	cvReleaseImage(&temp_32F);
	cvReleaseImage(&temp_32F2);
	cvReleaseImage(&temp_32F3);
	cvReleaseImage(&temp_32S);
	cvReleaseImage(&tR);
	cvReleaseImage(&tG);
	cvReleaseImage(&tB);
	cvReleaseImage(&movementMap_x);
	cvReleaseImage(&movementMap_y);
	cvReleaseImage(&movementMap);

	cvReleaseImage(&BackEdges);
	cvReleaseImage(&ForeEdges);
	cvReleaseImage(&BackEdgesThr);
	cvReleaseImage(&ForeEdgesThr);
	cvReleaseImage(&staticEdges);
	cvReleaseImage(&movingEdges);
	cvReleaseImage(&smoothedEdges);
	cvReleaseImage(&CEdges);
	cvReleaseImage(&CEdgesThr);
	cvReleaseImage(&mask);
	cvReleaseImage(&maskT);
	cvReleaseImage(&maskTemp);
	cvReleaseImage(&maskIn);
	cvReleaseImage(&maskSt);
	cvReleaseImage(&rem);
	cvReleaseImage(&road);
	cvReleaseImage(&roadSigns);
	cvReleaseImage(&carMask);	
	cvReleaseImage(&shadowMask);
	cvReleaseImage(&shadowEdges);
	cvReleaseImage(&shadow_std);
	cvReleaseImage(&shadow_stdf);

	//ULOZENI AUTOMATICKY LINII DO INIFILU
	CIniWriter iniWriter((char*)(LPCTSTR)iniFile);
	
	CString str,s;
	list<Lane>::iterator it;
	int i=0;
	for(it=autoLanes.begin();it!=autoLanes.end();it++) {
		str.Format("%.0f;%.0f;%.0f;%.0f;%.0f;%.0f;",it->x1*resCoef_x,it->y1*resCoef_y,it->x2*resCoef_x,it->y2*resCoef_y,it->counting_line*resCoef_y,
			it->conf);
		s.Format("tr%d",i);
		i++;
		iniWriter.WriteString("ALR",(char*)(LPCTSTR)s,(char*)(LPCTSTR)str);
	}	
}

int CVD_CoreApp::ExitInstance()
{	
	Clean();
	return CWinApp::ExitInstance();
}

