#include "trackingManager.h"

CTrackingManager::CTrackingManager()
{

}
//--------------------------------------------------------------
CTrackingManager::~CTrackingManager()
{

}
//--------------------------------------------------------------
void CTrackingManager::setup( int _w, int _h, int _screenW, int _screenH )
{	 
        
        w = _w;
        h = _h;
        
        screenW = _screenW;
        screenH = _screenH;
        
		giBgImage.allocate(w,h);
		giDiff.allocate(w,h);
		giScreenArea.allocate(w,h);
		giTouchArea.allocate(w,h);
		giAllTouchArea.allocate(w,h);
		giWarpTouchArea.allocate(w,h);
		
		fiLearn.allocate(w,h);
		
		contourToImage.setup(w,h);
		
		nThreshold = 80;
		
		fLearnRate = 0.00001f;
		
		bSetScreenArea	= false;
		
		loadImage(giBgImage,"bg.bmp");
		bgCapture( giBgImage );
}
//--------------------------------------------------------------
void CTrackingManager::update( ofCvGrayscaleImage _giLive )
{	 
          
          
		// learn background
		learnBackground( _giLive, giBgImage, fiLearn, fLearnRate);
          
		
		// get difference image
		giDiff.absDiff(giBgImage, _giLive);
		giDiff.threshold(nThreshold);

		
		// find contours 
		contourFinder.findContours(giDiff, 20, (340*240)/3, 10, true);
		
		
		
		// loop through each blob found and see if it intersects with the screen area
		
		giAllTouchArea.set(0);
		int nTouchAreaMin = 20;
		nTotalTouching = 0; 
		
		
		for( int i = 0; i < contourFinder.nBlobs; i++)
		{
			
			// use the contour points to convert the blob area back to an image	
			cvCopy(contourToImage.convertContourToImage( (ofPoint2f*)contourFinder.blobs[i].pts,contourFinder.blobs[i].nPts), giTouchArea.getCvImage(), 0);
			
			
			// AND the images so we get only the area where both the blob and screen overlap
			giTouchArea &= giScreenArea;
			
			
			// count pixels to see how many are in the screen area 
            int nCountPix = giTouchArea.countNonZeroInRegion(0,0,w,h);
            
            
            if( nCountPix > nTouchAreaMin )
            {
            	
            	// warp the image so it is a quad (makes it easy to scale up the screen coordinates)        	
            	// this might be inefficient and could probbl be done with a look up table
            	// should look into this, maybe with help from Bourke
            	// http://local.wasp.uwa.edu.au/~pbourke/geometry/distort/
            	
            	giWarpTouchArea.warpIntoMe(giTouchArea, scrnSrcPts, scrnDstPts );
            	
            	
            	
            	// use the contour finder to find the blob inside the screen
            	
            	contourFinderHand.findContours(giWarpTouchArea, 2, (340*240)/3, 1, false);
            	
            	// record the coordinates
            	
            	touchSpots[ nTotalTouching ].x = contourFinderHand.blobs[0].centroid.x;
            	touchSpots[ nTotalTouching ].y = contourFinderHand.blobs[0].boundingRect.height;// + contourFinderHand.blobs[0].boundingRect.height;
            	
            	touchSpots[ nTotalTouching ].x *= scrnScaleUp.x;
            	touchSpots[ nTotalTouching ].y *= scrnScaleUp.y;
            	
            	touchSpots[ nTotalTouching ].y = 768 - touchSpots[ nTotalTouching ].y;
            	
            	nTotalTouching++;
            	
            	
            	giAllTouchArea += giTouchArea;	// add it to an overall image of overlapping areas for debugging mostly
            }
            
		}
		
		
		
		
	
          
             
}
//--------------------------------------------------------------
void CTrackingManager::learnBackground( ofCvGrayscaleImage & _giLive, ofCvGrayscaleImage & _giBgImage, ofCvFloatImage & _fiLearn, float _fLearnRate )
{

	_fiLearn.addWeighted( _giLive, _fLearnRate);
    
    _giBgImage = _fiLearn;
}
//--------------------------------------------------------------
void CTrackingManager::bgCapture( ofCvGrayscaleImage _giLive )
{
	learnBackground( _giLive, giBgImage, fiLearn, 1.0f);       
}
//--------------------------------------------------------------
void CTrackingManager::setScreenArea( ofPoint2f srcPts[4] )
{
		cvCopy(contourToImage.convertQuadToImage(srcPts), giScreenArea.getCvImage(), 0);
		
		
		for( int i = 0; i < 4; i++) scrnSrcPts[i] = srcPts[i];
		
		// find the maximum x and y offset by the minimum x and y 		
        float maxW = MAX( abs(srcPts[0].x-srcPts[1].x),abs(srcPts[3].x-srcPts[2].x));
        float maxH = MAX( abs(srcPts[0].y-srcPts[3].y),abs(srcPts[1].y-srcPts[2].y));
        
        
        // make it proportional to the screen size;
        
        if(maxW > 0 && maxH > 0)
        {
        
        	if( maxW >= maxH )
        	{
        		maxH = maxW * (screenH / screenW);
        	}else{
        		maxW = maxH * (screenW / screenH);
        	}
        	
        }else{
        	printf("Error: Screen area bad values. w: %f h: %f\n",maxW,maxH);
        }
        
         // save values for warping later
        scrnDstPts[0].x = 0;
		scrnDstPts[0].y = maxH;
		
		scrnDstPts[1].x = maxW;
		scrnDstPts[1].y = maxH;
		
		scrnDstPts[2].x = maxW;
		scrnDstPts[2].y = 0;
		
		scrnDstPts[3].x = 0;
		scrnDstPts[3].y = 0;
		
		
		giWarpTouchArea.clear();
		giWarpTouchArea.allocate(maxW,maxH);
		
		scrnScaleUp.x = screenW / maxW;
		scrnScaleUp.y = screenH / maxH;
		
		for(int i = 0; i < 4; i++)
		{
		
			printf("%i: x %f y %f\n",i,srcPts[i].x,srcPts[i].y);
			
		}
		/*
		
		*/
		
		
}
//--------------------------------------------------------------
void CTrackingManager::drawTouchingPoints()
{
 	
 	
 	
 	
 	glPushMatrix();
 		
 		ofNoFill();
 		
 		
 		
 		for( int i = 0; i < nTotalTouching; i++)
 		{
 		
 		if(i%2 == 0) ofSetColor(0xff0000);
 		else ofSetColor(0xff00ff);
 		
 		ofLine(0,touchSpots[ i ].y,screenW,touchSpots[ i ].y);
 		ofLine(touchSpots[ i ].x,0,touchSpots[ i ].x,screenH);
 		ofCircle(touchSpots[ i ].x,touchSpots[ i ].y,10);
 		
 		
 		}
 		
 		
 	glPopMatrix();
 	
 	ofSetColor(0xffffff);
}

//--------------------------------------------------------------
/*void CTrackingManager::saveImage( ofCvGrayscaleImage & giSaveMe, char * filename)
{
	cvSaveImage( filename, giSaveMe.getCvImage() ); 
}*/
//--------------------------------------------------------------
void CTrackingManager::loadImage( ofCvGrayscaleImage & giLoadIntoMe, char * filename)
{
	 ofImage  * imageTemp = new ofImage();
    
    imageTemp->loadImage(filename);
    giLoadIntoMe.setFromPixels(imageTemp->getPixels(), w, h);
    
    
    delete imageTemp;
  
}
//--------------------------------------------------------------