int DataReader::initBackground(){
	// make initial snapshot of background
	
	
	for (int i=0;i < BN; i++)
		 background[i]= dataLaserR[i];
		 
}

int DataReader::detectMotion(int threshold) {

int thresholdDETECTION = 5;

for (int i=0;i<BN;i++)
{
	if((-dataLaserR[i]+background[i]) > threshold) 
	detection[i] =1;
	else  detection[i] = 0;
	
	
}



	// diff all beams to locate the object(s)


}

int DataReader::printMotion() {

	std::cout << "printMotion" << std::endl;
	for(int i=0; i < BN; i++)
		if (detection[i])
			std::cout << "beam " << i << ", r="
			<< dataLaserR[i] << "cms, theta=" 
			<< dataLaser[i] << "degres, X=" 
			<< dataLaserX[i] << "cms, Y=" 
  			<< dataLaserY[i] << "cms" << std::endl;
	std::cout << std::endl;

	return NOERROR;

}
int DataReader::displayMotion() {

	for(int i=0; i < BN; i++)
		if (detection[i]) {
		   float x1,y1,x2,y2;
    
    		transformCoordinates(0,0,&x1,&y1);
		   transformCoordinates(dataLaserX[i],dataLaserY[i],&x2,&y2);
		   cvLine(image, cvPoint(x1,y1), cvPoint(x2,y2), CV_RGB(217,217,217),1,4);
		}
 
	cvShowImage("myWindow", image);
	return NOERROR;

}
int DataReader::formObject() {
	int i,p;
	int conseq;
	bool isconseq =0;
	for (i=0;i<BN;i++) {
		if(!isconseq){
		  if((detection[i]==1) && ((detection[i-1]==0) || i==0)){
                    xmin=dataLaserX[i];
                    xmax=dataLaserX[i];
                    ymin=dataLaserY[i];
                    ymax=dataLaserY[i];
                    p=i;
                    conseq++;

		  }
		  else if ((detection[i]==1) && (detection[i-1]==1))
		  {
			  xmin=dataLaserX[i];
			  xmax=dataLaserX[i];
			  ymin=dataLaserY[i];
			  ymax=dataLaserY[i];
			  p=i;
			  conseq++;
			  if (conseq>30) isconseq =1;
			
		  }
		}else break;
	}
	
	for (i=p;i<BN;i++) {
	
	if ((detection[i]==1) && ((dataLaserX[i]!=0)||(dataLaserY[i]!=0)))
	{
		if (dataLaserX[i] < xmin) xmin = dataLaserX[i];
		if (dataLaserY[i] < ymin) ymin = dataLaserY[i];
		
		if (dataLaserX[i] > xmax) xmax = dataLaserX[i];
		if (dataLaserY[i] > ymax) ymax = dataLaserY[i];
	}
	
	
	
	}
	float x1,x2,y1,y2;
	
	transformCoordinates(xmin,ymin,&x1,&y1);
	transformCoordinates(xmax,ymax,&x2,&y2);
	drawRectangle(x1,y1,x2,y2,CV_RGB(0,217,0));
}