// FaceFindLib.cpp : Defines the entry point for the DLL application.
//

#include "stdafx.h"
#include "OpenCV.h"
#include "stdafx.h"
#include <cv.h>
#include <highgui.h>
//#define CRTDBG_MAP_ALLOC
#include <stdlib.h>
//#include <crtdbg.h>
#include <stdio.h>
#include <cvcam.h>
#include <jni.h>

#include "PrunedTemplateMatch.h"

#define CAMERA_TO_USE 0
#define INPUT_WINDOW_WIDTH 320
#define INPUT_WINDOW_HEIGHT 240
#define NUM_FACES 1 
#define PROCESSING_FRAMERATE 3
#define null 0
//size of square template that is sent to Java/WEKA for training/testing
#define TEMPLATE_SIZE 32 

#define WINDOW_AVERAGE_TEMPLATE "Average Template"
#define WINDOW_TEMPLATES "Templates" 
#define TRACKBAR_TEMPLATES "#"
#define DRAW_CROSSHAIRS 1

//useful macros
#define ABS(A)		((A) >= 0 ? (A) : -(A))
#define SGN(A)		((A) > 0 ? 1 : ((A) < 0 ? -1 : 0 ))
#define ROUND(A)	((A) >= 0 ? (int)((A)+.5) : -(int)(.5-(A)))
#define FLOOR(A)	((A) >= 0 ? (int) (A) : (int) (A)-1)
#define CEILING(A)	((A)==FLOOR(A) ? FLOOR(A) : SGN(A)+FLOOR(A))
#define MAX(A,B)	((A) > (B) ? (A) : (B))
#define MIN(A,B)	((A) < (B) ? (A) : (B))
#define SWAP(A,B)	{ double temp=(A); (A)  = (B);	(B)  = temp; }
#define SWAP_INT(A,B)	{ (A) ^= (B);	   (B) ^= (A);	(A) ^= (B);  }
#define CLAMP(A,L,H)	((A)<=(L) ? (L) : (A)<=(H) ? (A) : (H))
#define I(img,x,y) ((uchar*)(img->imageData + img->widthStep*y))[x]
//

CvSeq * templatesSeq; //dynamic data structure (doubly linked list essentially) which will hold the templates
CvSeq * matchesSeq; //holds the best matches of the templates 

IplImage * faceTemplates[NUM_FACES]; 
IplImage * currentImage; 
IplImage *averageTemplate = null;
IplImage *templateBuf; //buffer to be used everytime a new template is sent to WEKA

int leftButtonIsDown; 
CvPoint mouseDownLocation, currentMouseLocation; 

int curFrame;
int frameRateTrackbarVal; 
int templateTrackbarVal; //the current template to be display in the trackbar
int maxScaleTrackbarVal = 8; 
int maxErrorTrackbarVal = 1000; 

int curTemplateDisplayed; //current template being displayed in WINDOW_TEMPLATES

CvPoint minLocations[NUM_FACES], maxLocations[NUM_FACES]; //holds locations extrema found within template search result (min==>best match of template)
double minVal, maxVal;			  //holds values of extrema

//jni vals
int testVal; 
HANDLE hCurrentImageMutex; 
#define CURRENT_IMAGE_MUTEX "currentimagemutex" 
jobject globalCalledObject; 
JNIEnv *globalEnv; 
JavaVM *jvm; //see http://java.sun.com/docs/books/jni/html/other.html for details on threading issues assocaited with JNIEnv
//end of JNI

//structure which holds details of a template being matched to a location in the current frame
typedef struct TemplateMatch { 
	CvPoint minLocation;
	double minValue;
	IplImage *matchTemplate; //template which this match corresponds to
	int personNum; 
} TemplateMatch;

typedef struct FaceTemplate { 
	IplImage * originalTemplate;
	IplImage * currentSizedTemplate; 
	int numTimesProcessed; 
	int personNum; 
} FaceTemplate; 
//end of structures


//function redefinitions; 
void drawTemplateWindow(); 


//macro taken from http://groups.yahoo.com/group/OpenCV/message/34592
//xorigen,yorigen are coordinates of top left corner
IplImage *CopySubImage(IplImage *imatge,
					   int xorigen, 
					   int yorigen, 
					   int width, int height)
{
	CvRect roi;
	IplImage *resultat;

	roi.x = xorigen;
	roi.y = yorigen;
	roi.width = MIN(width, imatge->width-xorigen-1);    //ensure width is not greater than images width
	roi.height = MIN(height, imatge->height-yorigen-1); //ensure height is not greather than images height

	// fixa la ROI a la imatge
	cvSetImageROI(imatge,roi);
	// crea una nova imatge de la mida del ROI
	resultat = cvCreateImage( cvSize(roi.width, roi.height),
		imatge->depth, imatge->nChannels );
	cvCopy(imatge,resultat);
	cvResetImageROI(imatge);
	return resultat;
}
//end of cited macro

void updateAverageTemplate() { 
	if (averageTemplate!=null) { 
		cvReleaseImage(&averageTemplate); 
	}
	if (templatesSeq->total==0) { 
		return; 
	}
		
	IplImage *curTemplate = *((IplImage **) cvGetSeqElem(templatesSeq, 0));
	averageTemplate = cvCreateImage(cvSize(curTemplate->width, curTemplate->height), curTemplate->depth, curTemplate->nChannels); 
	for (int x=0; x<averageTemplate->width; x++) { 
		for (int y=0; y<averageTemplate->height; y++) { 
			int acc=0; //accumlator
			for (int i=0; i<templatesSeq->total; i++) { 
				curTemplate = *((IplImage **) cvGetSeqElem(templatesSeq, i)); 
				acc+= I(curTemplate,x,y); 
			}
			I(averageTemplate,x,y) = CLAMP((acc/templatesSeq->total), 0, 255); 
		}
	}
	cvShowImage(WINDOW_AVERAGE_TEMPLATE, averageTemplate); 
}

jintArray iplImageToJintArray(IplImage *img) { 
	jintArray jIA;
	int size = img->width * img->height; 
	jIA = globalEnv->NewIntArray(size); 
	
	//uchar * curPix = newTemplate->imageData; 
	
	int *buffer = (int *) malloc(sizeof(int) * img->width); 
	for (int y=0; y<img->height; y++) { 
		for (int x=0; x<img->width; x++) { 
			buffer[x] = I(img,x,y);
		}
		globalEnv->SetIntArrayRegion(jIA, y * img->width, img->width, (const jint*) buffer); 
	}
	free(buffer); 

	return jIA; 
}
void addNewTemplate(IplImage *temp) { 
	//create the face template 
	FaceTemplate * pFaceTemplate = (FaceTemplate *) malloc(sizeof(FaceTemplate)); 
	pFaceTemplate->originalTemplate = pFaceTemplate->currentSizedTemplate = temp; 
	pFaceTemplate->numTimesProcessed = 1; 
	pFaceTemplate->personNum = templatesSeq->total; 
	cvSeqPush(templatesSeq, &pFaceTemplate); 
	

	drawTemplateWindow(); 

	//save extracted template
	char templateFilename[128];
	sprintf(templateFilename, "%d.jpg", templatesSeq->total);
	cvSaveImage(templateFilename, temp); 

	/****************/
	//send image to Java to display in window
	jvm->AttachCurrentThread((void **)&globalEnv, NULL);

	jclass cls=globalEnv->GetObjectClass(globalCalledObject);
	jmethodID mid=globalEnv->GetMethodID(cls, 
		"newTemplate", 
		"(I[I)V");
	if (mid == 0) {
		globalEnv->ExceptionDescribe(); 
		printf("Can't find method sendArrayResults\n");
		return;
	}
	jintArray jIA = iplImageToJintArray(temp); 
	/*
	int size = newTemplate->width * newTemplate->height; 
	jIA = globalEnv->NewIntArray(size); 
	
	//uchar * curPix = newTemplate->imageData; 
	
	int *buffer = (int *) malloc(sizeof(int) * newTemplate->width); 
	for (int y=0; y<newTemplate->height; y++) { 
		for (int x=0; x<newTemplate->width; x++) { 
			buffer[x] = I(newTemplate,x,y);
		}
		globalEnv->SetIntArrayRegion(jIA, y * newTemplate->width, newTemplate->width, (const jint*) buffer); 
	}
	*/
	//jfloat test = f; 
	globalEnv->ExceptionClear();
	globalEnv->CallVoidMethod(globalCalledObject, mid, templatesSeq->total, jIA);
	/*********************/
}
//workhouse function that actually gets called by the other two overloaded functions
IplImage * extractTemplate(int xOrigin, int yOrigin, int width, int height) { 
	IplImage *newTemplate, *greyscaleTemplate; 
	newTemplate = CopySubImage(currentImage, xOrigin, yOrigin, width, height); 
	//if the template (because of the video stream) has its origin in the bottom left instead of top left, flip the image (otherwise the template will be upsidedown)
	
	//if (currentImage->origin ) {
		cvFlip(newTemplate, newTemplate);
	//}
	
	//convert new template to greyscale if it has more than 1 channel (i.e. color) 
	if (newTemplate->nChannels>1) { 
		greyscaleTemplate = cvCreateImage(cvSize(newTemplate->width, newTemplate->height), 8, 1); //8bit greyscale image of same size
		cvCvtColor(newTemplate, greyscaleTemplate, CV_RGB2GRAY); 
		cvReleaseImage(&newTemplate);
		newTemplate = greyscaleTemplate; 
	}
	 
	return newTemplate; 
}

//called by mouseCallback, once the user has selected a region (by clicking down, moving the mouse and then clicking back up)
//extracts out the rectangular region from the current image that the user selected
IplImage * extractTemplate() { 
	printf("Extracting based on (%d,%d)->(%d,%d)\n", mouseDownLocation.x, mouseDownLocation.y, currentMouseLocation.x, currentMouseLocation.y); 
	int xOrigin, yOrigin, width, height; //values for the template (don't assume that the user always clicks top left and drags down and to the right)
	xOrigin = MIN(mouseDownLocation.x, currentMouseLocation.x); 
	//yOrigin = MIN((currentImage->height-mouseDownLocation.y), (currentImage->height-currentMouseLocation.y)); 
	//make template a square (with side-length based on x-values)
	width = height = ABS(mouseDownLocation.x - currentMouseLocation.x);
	yOrigin = ABS(mouseDownLocation.y - currentImage->height); //MIN(mouseDownLocation.y, currentMouseLocation.y);
	yOrigin -= height; 
	/*width = ABS(mouseDownLocation.x - currentMouseLocation.x);
	height = ABS(mouseDownLocation.y - currentMouseLocation.y); 
	*/
	/*
	xOrigin = MIN(mouseDownLocation.x, currentMouseLocation.x); 
	yOrigin = MIN(currentImage->height-mouseDownLocation.y, currentImage->height-currentMouseLocation.y); 
	width = ABS(mouseDownLocation.x - currentMouseLocation.x);
	height = ABS(mouseDownLocation.y - currentMouseLocation.y); 
	*/
	printf("extracting template @ (%d, %d) of dimensions (%d,%d)\n", xOrigin, yOrigin, width, height); 
	return extractTemplate(xOrigin, yOrigin, width, height); 
}

//extracts a new template from a previous match (called by processFrame before it offsets the minLocation to be centered on the object, rather than in the corner)
IplImage * extractTemplate(IplImage *curImg, IplImage *templateImg, CvPoint minLocation) { 
	int xOrigin, yOrigin, width, height; 
	xOrigin = minLocation.x- (templateImg->width/2); 
	yOrigin = ABS((minLocation.y- (templateImg->height/2)) - curImg->height);
	yOrigin -= templateImg->height; 
	
	//yOrigin = curImg->height - yOrigin; 
	width = templateImg->width;
	height = templateImg->height; 
	//make sure we aren't trying to extract a template outside of the range of the image 
	if (yOrigin>0 && 
		xOrigin>0 && 
		(xOrigin+width)<curImg->width && 
		( yOrigin+height)<curImg->height) { 
		return extractTemplate(xOrigin, yOrigin, width, height); 
	}
	else {
		return null;
	}
}

void mouseCallback(int event, int x, int y, int flags, void *param) { 
	//printf("mouse callback: %d, %d\n", x, y); 
	IplImage *flippedImage;
	switch (event) { 
		case CV_EVENT_RBUTTONDOWN:
			flippedImage = cvCreateImage(
				cvSize(currentImage->width, currentImage->height),
				currentImage->depth,
				currentImage->nChannels);
			cvCopy(currentImage, flippedImage); 
			cvFlip(flippedImage); 

			printf("Saving video snapshot as %s\n", "VideoSnapshot.jpeg");
			cvSaveImage("VideoSnapshot.jpeg", flippedImage); 
			cvReleaseImage(&flippedImage); 
			break;

		case CV_EVENT_MOUSEMOVE:
			currentMouseLocation.x = x;
			currentMouseLocation.y = y; 
			break; 

		case CV_EVENT_LBUTTONDOWN:
			leftButtonIsDown = 1; 
			mouseDownLocation.x = x;
			mouseDownLocation.y = y; 
			//printf("Left button down: %d,%d\n", mouseDownLocation.x, mouseDownLocation.y);
			//printf("Left button down: %d,%d\n", x, y);
			
			break;

		case CV_EVENT_LBUTTONUP:
			printf("left button up\n");
			leftButtonIsDown = 0; 
			IplImage *newTemplate = extractTemplate(); 
			addNewTemplate(newTemplate); 
			break;
	}
}

void findOriginal(IplImage *image, FaceTemplate *pFaceTemplate, TemplateMatch *pMatch) { 
	//now check to see the last time we tried using the original template, and if its been "awhile", try matching it again
	double minMatchVal, maxMatchVal;
	CvPoint minMatchPoint, maxMatchPoint;
	IplImage *resultImg; 


	resultImg = cvCreateImage(
		cvSize(image->width - pFaceTemplate->originalTemplate->width+1, image->height - pFaceTemplate->originalTemplate->height + 1), //w x h dimensions
		IPL_DEPTH_32F, //depth
		1				//# of channels
		); 

	cvMatchTemplate(
		image,
		pFaceTemplate->originalTemplate,
		resultImg, 
		CV_TM_SQDIFF
		); 

	cvMinMaxLoc(resultImg, 
		&minMatchVal, 
		&maxMatchVal,
		&minMatchPoint,
		&maxMatchPoint, 
		NULL); 

	int normalizedMinVal = minMatchVal/(pFaceTemplate->originalTemplate->width * pFaceTemplate->originalTemplate->height); 

	//printf("Scale: %f, matched with %d (vs. %d)\n", scale, normalizedMinVal, bestMinVal); 
	//see if this template matches better
	int normalizedCurrent = pMatch->minValue / (pFaceTemplate->currentSizedTemplate->width * pFaceTemplate->currentSizedTemplate->height); 
	if (normalizedMinVal<normalizedCurrent) { 
		pMatch->matchTemplate   = pFaceTemplate->originalTemplate;
		pMatch->minLocation.x   = minMatchPoint.x + pFaceTemplate->originalTemplate->width/2;
		pMatch->minLocation.y	= minMatchPoint.y + pFaceTemplate->originalTemplate->height/2; 
		pMatch->minValue		= minMatchVal; 
		/*
		bestMinVal= normalizedMinVal; 
		bestMinPoint.x = minMatchPoint.x;
		bestMinPoint.y = minMatchPoint.y; 
		bestRoiX = 0;
		bestRoiY = 0; 
		bestScale = 1.0; 
		foundBetterTemplate = 1; //true!
		//if a previous resized template was already selected, free it first
		if (bestTemplateImg!=null) { 
		cvReleaseImage(&bestTemplateImg); 
		}
		bestTemplateImg = pFaceTemplate->originalTemplate; 
		*/
	}
	
	cvReleaseImage(&resultImg); 
}

//called once the best match within the current frame is called
//The primary purpose of this function is to try resized copies of the template to see if a bigger/smaller one matches better
void adjustTemplate(IplImage *image, FaceTemplate *pFaceTemplate, TemplateMatch *match) { 
	
	double minMatchVal, maxMatchVal;
	CvPoint minMatchPoint, maxMatchPoint;

	//keep track of the best points found so far
	int bestMinVal=match->minValue/(pFaceTemplate->currentSizedTemplate->height * pFaceTemplate->currentSizedTemplate->width); //normalized (error per pixel)
	CvPoint bestMinPoint; 
	float bestScale; 

	int foundBetterTemplate = 0; //initially false

	IplImage *matchedImageRegion, //extract from the full image the area around the match
			 *resultImg, 
			 *resizedTemplateImg,
			 *bestTemplateImg=null;
	
	//extract area around where template best matched from the full frame image
	int roiX, roiY, roiSize, roiWidth, roiHeight; 
	int bestRoiX, bestRoiY; 
	
	roiSize = (int) (pFaceTemplate->currentSizedTemplate->width * 1.6);
	
	roiX = match->minLocation.x-(roiSize>>1);
	roiY = match->minLocation.y-(roiSize>>1); 
	
	roiWidth = roiHeight = roiSize; 

	if (roiX<0) { 
		roiWidth -= (0-roiX); 
		roiX = 0; 
	}
	if (roiY<0) { 
		roiHeight -= (0-roiY);
		roiY = 0; 
	}

	if (roiX<0 || roiY<0) { 
		printf("CLAMP FAILED %d,%d; returning\n", roiX, roiY); 
		return; 
	}

	if (roiWidth<=pFaceTemplate->currentSizedTemplate->width || roiHeight<=pFaceTemplate->currentSizedTemplate->height) { 
		printf("On border region, cannot resize"); 
		return; 
	}

	//printf("From (%d,%d) & w=%d, using 
	matchedImageRegion = CopySubImage(image, roiX, roiY, roiWidth, roiHeight);  
	float minScale, maxScale; 
	minScale = (float) maxScaleTrackbarVal / 10.0;
	maxScale = 1.0 + (1.0 - minScale); 
	float currentScale = (float)pFaceTemplate->currentSizedTemplate->width / (float) pFaceTemplate->originalTemplate->width; 
		
	for (float scale=.5; scale<=1.6; scale+=.10) { 
		//create resized template image, and then copy the 
		int resizedWidth, resizedHeight;
		resizedWidth  =  pFaceTemplate->currentSizedTemplate->width*scale;
		resizedHeight =  pFaceTemplate->currentSizedTemplate->height*scale;
		//ensure we don't resize the image too small, and that we don't waste time doing a false (scale=1.0) resize
		float absoluteScale = scale * currentScale; 
		if (resizedWidth>32 && resizedHeight > 32 && scale!=1.0 && absoluteScale>minScale && absoluteScale<maxScale) { 
			resizedTemplateImg = cvCreateImage(
				cvSize(resizedWidth, resizedHeight),
				pFaceTemplate->currentSizedTemplate->depth,
				pFaceTemplate->currentSizedTemplate->nChannels);
			//ok to downsize using currently resized
			if (scale<1.0) { 
				cvResize(pFaceTemplate->currentSizedTemplate, resizedTemplateImg, CV_INTER_LINEAR);
			}
			//if increasing size, use the original, which is likely to be bigger (thus image quality does not progressively degrade through a series of resizes)
			else if (scale>1.0) { 
				cvResize(pFaceTemplate->originalTemplate, resizedTemplateImg, CV_INTER_LINEAR);
			}
		}

		//set to null if too small so that on this iteration it will not compare
		else { 
			resizedTemplateImg = null; 
		}
		
		if (resizedTemplateImg!=null 
			&& (matchedImageRegion->width - resizedTemplateImg->width+1) > 0 
			&& (matchedImageRegion->height - resizedTemplateImg->height + 1)>0) 
		{ 
			//create image to hold the result of the correlation
			resultImg = cvCreateImage(
				cvSize(matchedImageRegion->width - resizedTemplateImg->width+1, matchedImageRegion->height - resizedTemplateImg->height + 1), //w x h dimensions
				IPL_DEPTH_32F, //depth
				1				//# of channels
				); 

			cvMatchTemplate(
				matchedImageRegion,
				resizedTemplateImg,
				resultImg, 
				CV_TM_SQDIFF
				); 

			cvMinMaxLoc(resultImg, 
				&minMatchVal, 
				&maxMatchVal,
				&minMatchPoint,
				&maxMatchPoint, 
				NULL); 

			int normalizedMinVal = minMatchVal/(resizedTemplateImg->width * resizedTemplateImg->height); 

			//printf("Scale: %f, matched with %d (vs. %d)\n", scale, normalizedMinVal, bestMinVal); 
			//see if this template matches better
			if (normalizedMinVal<bestMinVal) { 
				bestMinVal= normalizedMinVal; 
				bestMinPoint.x = minMatchPoint.x;
				bestMinPoint.y = minMatchPoint.y; 
				bestRoiX = roiX;
				bestRoiY = roiY; 
				bestScale = scale; 
				foundBetterTemplate = 1; //true!
				//if a previous resized template was already selected, free it first
				if (bestTemplateImg!=null) { 
					cvReleaseImage(&bestTemplateImg); 
				}
				bestTemplateImg = resizedTemplateImg; 
			}
			else { 
				cvReleaseImage(&resizedTemplateImg); 
			}
			cvReleaseImage(&resultImg); 
		}
		//else if extracted region is smaller than template, just release the resized template	
		else { 
			cvReleaseImage(&resizedTemplateImg); 
		}
	}

	//now check to see the last time we tried using the original template, and if its been "awhile", try matching it again
	if (false) { 
	//if (pFaceTemplate->numTimesProcessed%10==0) { 
		resultImg = cvCreateImage(
			cvSize(image->width - pFaceTemplate->originalTemplate->width+1, image->height - pFaceTemplate->originalTemplate->height + 1), //w x h dimensions
			IPL_DEPTH_32F, //depth
			1				//# of channels
			); 

		cvMatchTemplate(
			image,
			pFaceTemplate->originalTemplate,
			resultImg, 
			CV_TM_SQDIFF
			); 

		cvMinMaxLoc(resultImg, 
			&minMatchVal, 
			&maxMatchVal,
			&minMatchPoint,
			&maxMatchPoint, 
			NULL); 

		int normalizedMinVal = minMatchVal/(pFaceTemplate->originalTemplate->width * pFaceTemplate->originalTemplate->height); 

		//printf("Scale: %f, matched with %d (vs. %d)\n", scale, normalizedMinVal, bestMinVal); 
		//see if this template matches better
		if (normalizedMinVal<bestMinVal) { 
			bestMinVal= normalizedMinVal; 
			bestMinPoint.x = minMatchPoint.x;
			bestMinPoint.y = minMatchPoint.y; 
			bestRoiX = 0;
			bestRoiY = 0; 
			bestScale = 1.0; 
			foundBetterTemplate = 1; //true!
			//if a previous resized template was already selected, free it first
			if (bestTemplateImg!=null) { 
				cvReleaseImage(&bestTemplateImg); 
			}
			bestTemplateImg = pFaceTemplate->originalTemplate; 
		}
		cvReleaseImage(&resultImg); 
	}
	//increment it since we've processed it again
	pFaceTemplate->numTimesProcessed++; 
	
	//if a resized template was better than the current one, change the template to the resized one
	if (bestTemplateImg!=null) { 
		//printf("Resizing template by %f (new %f vs old %f)\n", bestScale, bestMinVal, match->minValue); 
		//cvReleaseImage(&(*templateImg)); //free the original template
		//(*templateImg) = bestTemplateImg; 
		//only release the previous currentSizedTemplate if it isn't the original (which we want to keep) 
		if (pFaceTemplate->currentSizedTemplate!=pFaceTemplate->originalTemplate) { 
			cvReleaseImage(&(pFaceTemplate->currentSizedTemplate)); 
		}
		pFaceTemplate->currentSizedTemplate = bestTemplateImg; 

		match->matchTemplate = bestTemplateImg; 
		match->minValue = bestMinVal * bestTemplateImg->width * bestTemplateImg->height; //return to non-normalized
		match->minLocation.x = bestMinPoint.x + roiX + (bestTemplateImg->width>>1);
		match->minLocation.y = bestMinPoint.y + roiY + (bestTemplateImg->height>>1);
		drawTemplateWindow(); 
	}
	if (bestMinVal>maxErrorTrackbarVal) { 
		//printf("Finding original\n"); 
		findOriginal(image, pFaceTemplate, match); 
	}
}

void sendCorrelation(float f) { 
	/*
	jvm->AttachCurrentThread((void **)&globalEnv, NULL);

	jclass cls=globalEnv->GetObjectClass(globalCalledObject);
	jmethodID mid=globalEnv->GetMethodID(cls, 
		"cCalled", 
		"(F)V");
	if (mid == 0) {
		printf("Can't find method sendArrayResults\n");
		return;
	}
	jfloat test = f; 
	globalEnv->ExceptionClear();
	globalEnv->CallVoidMethod(globalCalledObject, mid, test);
	*/
}
void sendLastTemplate(int personNum, IplImage *lastTemplate, int correlation) { 
	//happens with border issues!
	if (lastTemplate==null) { 
		return;
	}
	
	//resize template to the specified size
	cvResize(lastTemplate, templateBuf); 

	jvm->AttachCurrentThread((void **)&globalEnv, NULL);

	jclass cls=globalEnv->GetObjectClass(globalCalledObject);
	jmethodID mid=globalEnv->GetMethodID(cls, 
		"updateLastTemplate", 
		"(I[II)V");
	if (mid == 0) {
		printf("Can't find method sendArrayResults\n");
		globalEnv->ExceptionDescribe(); 
		return;
	}
	jintArray jIA = iplImageToJintArray(templateBuf); 

	globalEnv->ExceptionClear();
	globalEnv->CallVoidMethod(globalCalledObject, mid, personNum, jIA, correlation);
	cvReleaseImage(&lastTemplate); 
}

//comparison function used to sort the elements
int matchCmpFnc( const void* _a, const void* _b, void* userdata) {
	TemplateMatch *mA, *mB;
	mA = *((TemplateMatch**) _a); 
	mB = *((TemplateMatch**) _b); 
	if (mA->minValue<mB->minValue) { 
		return -1;
	}
	else if (mA->minValue>mB->minValue) { 
		return 1; 
	}
	return 0;
}

void eliminateLargeErrorMatches() { 
	TemplateMatch * match; //m is the match being examined, c are all the other matches being compared against m
	int minValNormalized; //normalized per pixel 
	for (int i=0; i<matchesSeq->total; i++) { 
		match = *((TemplateMatch**) cvGetSeqElem(matchesSeq, i)); 

		//if the error of this match is greater than the trackbar value for max error than remove it 
		if (match) { 
			minValNormalized = match->minValue / (match->matchTemplate->width * match->matchTemplate->height); 
			if (minValNormalized>maxErrorTrackbarVal) { 
				//printf("Removing %d (with %d greater than %d)\n", i, match->minValue, maxErrorTrackbarVal); 
				free(match);
				cvSeqRemove(matchesSeq, i); 
				i--; 
			}
		}
	}
}

void eliminateOverlappingMatches() { 
	TemplateMatch * m, *c; //m is the match being examined, c are all the other matches being compared against m

	//draw a circle over each template matched
	int templateSize;
	int mMinX, mMaxX, mMinY, mMaxY, cx, cy;
	int cMinX, cMaxX, cMinY, cMaxY;
	int mRemoved = 0; //keeps track of whether m has been removed based on c being a better match
	double dist, maxDist; 

	//first sort template matches
	cvSeqSort(matchesSeq, matchCmpFnc, 0); 

	for (int i=0; i<matchesSeq->total; i++) { 
		m = *((TemplateMatch**) cvGetSeqElem(matchesSeq, i)); 
		if (m) { 
			mRemoved = 0; 
			mMinX = m->minLocation.x - m->matchTemplate->width/2;
			mMaxX = m->minLocation.x + m->matchTemplate->width/2;
			mMinY = m->minLocation.y - m->matchTemplate->height/2;
			mMaxY = m->minLocation.y + m->matchTemplate->height/2;

			for (int j=0; j<matchesSeq->total && !mRemoved; j++) { 
				if (i!=j) { 
					c = *((TemplateMatch**) cvGetSeqElem(matchesSeq, j)); 
					if (c) { 
						dist = sqrt(pow(m->minLocation.x - c->minLocation.x, 2) + pow(m->minLocation.y - c->minLocation.y, 2)); 
						//maxDist = sqrt(2.0 * pow(MAX(m->matchTemplate->width, c->matchTemplate->width),2)); 
						maxDist = (m->matchTemplate->width/2) + (c->matchTemplate->width/2); 
						cx = c->minLocation.x;
						cy = c->minLocation.y;
						//if match for c is located within the bounds of m
						//if (cx>mMinX && cx<mMaxX && cy>mMinY && cy<mMaxY) { 
						if (dist<maxDist) { 						
						//remove m if c is better
							if (c->minValue < m->minValue) { 
								//printf("Removing %d @ (%d,%d) in favor of %d\n", i, m->minLocation.x, m->minLocation.y, j); 
								free(m);
								cvSeqRemove(matchesSeq, i); 
								mRemoved = 1; 
								i--; 
							}
							//remove c if m is better
							else  { 
								//printf("Removing %d @ (%d,%d) in favor of %d\n", j, m->minLocation.x, m->minLocation.y, i); 
								free(c);
								cvSeqRemove(matchesSeq, j); 
								j--; 
							}
						}
					}
				}
			}
		}
	}
}

void processFrame(IplImage *image) { 
	IplImage* greyscaleImg = image;
	IplImage *resultImg; 
	int i,j;
	
	assert (image);

	//convert image to greyscale
	greyscaleImg = cvCreateImage(cvSize(image->width, image->height),  IPL_DEPTH_8U, 1); 
	cvCvtColor(image, greyscaleImg, CV_RGB2GRAY); 

	if (image->origin ) {
		cvFlip(greyscaleImg, greyscaleImg);
	}
	
	//remove the old matches from the last matching session
	TemplateMatch ** match; 
	//keep removing the first item, until all old matches are deleted
	while (matchesSeq->total>0) { 
	//for (int i=0; i<matchesSeq->total; i++) { 
		match = ((TemplateMatch**) cvGetSeqElem(matchesSeq,0)); 
		if (match) { 
			free(*match);
			cvSeqRemove(matchesSeq, 0); 
		}
	}

	FaceTemplate *pFaceTemplate; 	
	IplImage **curTemplateImgDblPtr; 
	IplImage *curTemplateImg; 
	TemplateMatch *newMatch; 
	double minMatchVal, maxMatchVal;
	CvPoint minMatchPoint, maxMatchPoint; 
	
	for (int i=0; i<templatesSeq->total; i++) { 
	//for (int i=0; i<MIN(5,templatesSeq->total); i++) { 
		pFaceTemplate = *((FaceTemplate **) cvGetSeqElem(templatesSeq, i));
		//curTemplateImgDblPtr = (IplImage **) cvGetSeqElem(templatesSeq, i); //CV_GET_SEQ_ELEM(CvPoint, seq, i); 
		if (pFaceTemplate) { 
			curTemplateImg = pFaceTemplate->currentSizedTemplate; 
			
			newMatch = (TemplateMatch *) malloc(sizeof(TemplateMatch)); 
			newMatch->personNum = pFaceTemplate->personNum; 
			resultImg = cvCreateImage(
				cvSize(image->width - curTemplateImg->width+1, image->height - curTemplateImg->height + 1), //w x h dimensions
				IPL_DEPTH_32F, //depth
				1				//# of channels
				); 

			cvMatchTemplate(
				greyscaleImg,
				curTemplateImg,
				resultImg, 
				CV_TM_SQDIFF
				); 

			cvMinMaxLoc(resultImg, 
				&minMatchVal, 
				&maxMatchVal,
				&minMatchPoint,
				&maxMatchPoint, 
				NULL); 

			newMatch->minValue = minMatchVal;
			newMatch->minLocation = minMatchPoint; 
			newMatch->matchTemplate = curTemplateImg; 
			//extractTemplate(currentImage, curTemplateImg, newMatch->minLocation); 
			
			newMatch->minLocation.x += (curTemplateImg->width>>1);
			newMatch->minLocation.y += (curTemplateImg->height>>1); 

			adjustTemplate(greyscaleImg, pFaceTemplate, newMatch);
			
			//sendLastTemplate(i, extractTemplate(currentImage, newMatch->matchTemplate, newMatch->minLocation), correlation); 
			
			cvSeqPush(matchesSeq, &newMatch); 
			//sendCorrelation((float)(minMatchVal/(curTemplateImg->width*curTemplateImg->height))); 
			//printf("Face %d located @ (%d,%d) with SSD of %f\n", i, minLocations[i].x, minLocations[i].y, (minMatchVal/(curTemplateImg->width*curTemplateImg->height))); 
			cvReleaseImage(&resultImg);
		}
	}

	cvReleaseImage(&greyscaleImg);
	
	//continue filtering the results
	eliminateLargeErrorMatches();
	eliminateOverlappingMatches();
	
	TemplateMatch *m; //m is the match being examined, c are all the other matches being compared against m
	int minValNormalized; //normalized per pixel 
	
	for (int i=0; i<matchesSeq->total; i++) { 
		m = *((TemplateMatch**) cvGetSeqElem(matchesSeq, i)); 

		//if the error of this match is greater than the trackbar value for max error than remove it 
		if (m) { 
			int correlation = m->minValue / (m->matchTemplate->width * m->matchTemplate->height); 			
			
			sendLastTemplate(m->personNum, extractTemplate(currentImage, m->matchTemplate, m->minLocation), correlation); 
		}
	}
}

//callback is called on every new frame from video camera
void callback(IplImage* image)
{
	curFrame++; //increment the current frame counter
//	currentImage = image; //keep track of the current image (required for extractTemplate() which is called by mouseCallback, driven by GUI thread) 
	if (currentImage!=null) { 
		cvReleaseImage(&currentImage); 
	}
	
	currentImage  = cvCreateImage(cvSize(image->width, image->height), image->depth, image->nChannels); 
	cvCopyImage(image, currentImage); 
	
	if (curFrame%frameRateTrackbarVal == 0) { 
		processFrame(image); 
		//cvFlip(image, image);
		for (int i=0; i<NUM_FACES; i++) { 
			minLocations[i].y = image->height-minLocations[i].y;
		}
	}
	//cvFlip(image, image);

	//draw template extracting box on the video stream so the user can see the template being chosen
	if (leftButtonIsDown) { 
		//ensure template is a square, so utilize largest dimensions
		int templateSize = ABS(currentMouseLocation.x-mouseDownLocation.x); 
		//printf("Template Size: %d; ", templateSize); 
		cvRectangle(image, cvPoint(mouseDownLocation.x,image->height-mouseDownLocation.y),  cvPoint(mouseDownLocation.x+templateSize, image->height-(mouseDownLocation.y+templateSize)), cvScalarAll(128), 2, 8, 0); 
		
		//printf("\n"); 
		//cvRectangle(image, cvPoint(mouseDownLocation.x,image->height-mouseDownLocation.y),  cvPoint(currentMouseLocation.x, image->height-currentMouseLocation.y), cvScalarAll(128), 2, 8, 0); 
		//printf("Drawing rectangle from %d,%d to %d,%d\n", mouseDownLocation.x, image->height-mouseDownLocation.y, currentMouseLocation.x, image->height-currentMouseLocation.y); 
	}

	//draw circle showing last known location everytime
	/*
	for (int i=0; i<NUM_FACES; i++) { 
		cvCircle(image, minLocations[i], 10, cvScalarAll(i*100), 2, 8, 0); 
	}
	*/
	TemplateMatch * match;
	
	//draw a circle over each template matched
	int templateSize; 
	for (int i=0; i<matchesSeq->total; i++) { 
		match = *((TemplateMatch**) cvGetSeqElem(matchesSeq, i)); 
		if (match) { 
			templateSize = match->matchTemplate->width; //assume template is square
			
			if (DRAW_CROSSHAIRS) {
				//draw circle
				cvCircle(image, cvPoint(match->minLocation.x, image->height-match->minLocation.y), templateSize/2, cvScalarAll(0), 2, 8, 0);
				//draw vertical bar
				cvLine(image, cvPoint(match->minLocation.x, image->height-(match->minLocation.y - templateSize/2)), 
					cvPoint(match->minLocation.x, image->height-(match->minLocation.y + templateSize/2)),
					cvScalarAll(0), 1, 8, 0); 
				//draw horizontal bar
				cvLine(image, cvPoint(match->minLocation.x - templateSize/2, image->height-(match->minLocation.y)), 
					cvPoint(match->minLocation.x + templateSize/2, image->height-(match->minLocation.y)),
					cvScalarAll(0), 1, 8, 0);
			}

			else { 
				cvRectangle(image, cvPoint(match->minLocation.x-templateSize/2,image->height-(match->minLocation.y-templateSize/2)),  
				cvPoint(match->minLocation.x+templateSize/2,image->height-(match->minLocation.y+templateSize/2)),
				cvScalarAll(128), 2, 8, 0);
			}
			/*
			cvCircle(image, cvPoint(match->minLocation.x, image->height-match->minLocation.y), 10, cvScalarAll(i*100), 2, 8, 0);
			*/
		}
	}
}

int ShowCamVideo(HWND hwnd, int width, int height)
{
	//FRAMES_TO_PROCESS };
	VidFormat vidFmt={ width, height, 30.0};
	int numCams, camera, *camerasSelected, numCamerasSelected;

    numCams = cvcamGetCamerasCount( );
	printf("%d cameras detected\n", numCams);
	
	//get camera(s) to use; function returns # cameras selected, camerasSelected holds array of ints of cameras chosen
#ifndef CAMERA_TO_USE
	numCamerasSelected = cvcamSelectCamera(&camerasSelected); 
	if (numCamerasSelected!=1) { //for now only handle one camera being selected, return otherwise
		return 0; 
	}
	camera = camerasSelected[0]; 
#else
	camera = CAMERA_TO_USE; 
#endif
	printf("Using Camera %d\n", camera); 
    cvcamSetProperty(camera, CVCAM_PROP_ENABLE, CVCAMTRUE);
    cvcamSetProperty(camera, CVCAM_PROP_CALLBACK,callback);
	
	cvcamSetProperty(camera, CVCAM_RNDWIDTH, (void *) &width);
	cvcamSetProperty(camera, CVCAM_RNDHEIGHT, (void *) &height);

    cvcamSetProperty(camera, CVCAM_PROP_WINDOW, &hwnd);   

    //Set Video Format Property
    cvcamSetProperty(camera, CVCAM_PROP_SETFORMAT, &vidFmt);

	//displays camera's properties dialog
	//cvcamGetProperty(camera, CVCAM_CAMERAPROPS, NULL);

	//displays camera's vidoe format properties dialog
	//cvcamGetProperty(camera, CVCAM_VIDEOFORMAT, NULL);
    
	if( !cvcamInit() )
        return 0;
    cvcamStart();
    return 1;
}
void templateTrackbarCallback(int pos) { 
	IplImage *tempToDraw;
	FaceTemplate *pFaceTemplate = *((FaceTemplate **) cvGetSeqElem(templatesSeq, pos)); 
	tempToDraw = pFaceTemplate->currentSizedTemplate; 

	cvShowImage(WINDOW_TEMPLATES, tempToDraw);
	if (SetWindowPos((HWND)cvGetWindowHandle(WINDOW_TEMPLATES), HWND_TOP, 0, 0, 320, 240, SWP_NOMOVE)==0) { 
		printf("Error resizing window\n"); 
	}
}
void drawTemplateWindow() { 
	cvCreateTrackbar("Template#", WINDOW_TEMPLATES, &templateTrackbarVal, MAX(templatesSeq->total-1,1), templateTrackbarCallback); 
	//if at least one template has already been taken, display the last template taken
	if (templatesSeq->total>0) { 
		FaceTemplate *pFaceTemplate = *((FaceTemplate **) cvGetSeqElem(templatesSeq, templatesSeq->total-1)); 
		cvShowImage(WINDOW_TEMPLATES, pFaceTemplate->currentSizedTemplate); 
		if (SetWindowPos((HWND)cvGetWindowHandle(WINDOW_TEMPLATES), HWND_TOP, 0, 0, 240, 240, SWP_NOMOVE)==0) { 
			printf("Error resizing window\n"); 
		}
	}
}
void maxErrorTrackbarCallback(int pos) { 
	
}
void maxScaleTrackbarCallback(int pos) { 
	
}
DWORD WINAPI LaunchCameraThread( LPVOID lpParam ) 
{ 
	leftButtonIsDown = 0; 
	curFrame = 0; 
	frameRateTrackbarVal = PROCESSING_FRAMERATE; 
	currentImage = null; //so that callback does not deallocate on first call

	CvMemStorage* storage = cvCreateMemStorage(0); //memory to hold actual values

	templatesSeq = cvCreateSeq( 
		CV_SEQ_ELTYPE_PTR, // sequence of generic types (will be pointers)
		sizeof(CvSeq), //header size - no extra fields 
		sizeof(FaceTemplate *), //element size
		storage // the container storage
		);

	CvMemStorage *matchesStorage = cvCreateMemStorage(0); 
	matchesSeq = cvCreateSeq(
		CV_SEQ_ELTYPE_PTR,
		sizeof(CvSeq),
		sizeof(TemplateMatch *),
		matchesStorage); 

	//allocate greyscale template buffer to be filled in with extracted templates and sent to WEKA
	templateBuf = cvCreateImage(cvSize(TEMPLATE_SIZE, TEMPLATE_SIZE), IPL_DEPTH_8U, 1); 

	//allocate mutex for synchronizing access to currentImage
	/*
	hCurrentImageMutex = CreateMutex(
		NULL,
		false,
		CURRENT_IMAGE_MUTEX); 
*/

	cvNamedWindow("TestWindow", CV_WINDOW_AUTOSIZE); 
	cvNamedWindow(WINDOW_TEMPLATES, 0); 
	//cvNamedWindow("window", 1);
	//cvNamedWindow(WINDOW_AVERAGE_TEMPLATE, CV_WINDOW_AUTOSIZE); 

	IplImage *blankImage; //create a blank image just for the purpose of initializing the video input stream window, without doing this the mouse callbacks do not contain the (x,y) locations of the mouse
	blankImage = cvCreateImage(cvSize(INPUT_WINDOW_WIDTH, INPUT_WINDOW_HEIGHT), IPL_DEPTH_32S, 3); 
	cvShowImage("TestWindow", blankImage); 
	cvSetMouseCallback("TestWindow", mouseCallback, (void *) 0);
	//cvSetMouseCallback("window",     mouseCallback, (void *) 0);
	
	cvCreateTrackbar("MaxError", "TestWindow", &maxErrorTrackbarVal, 2000, maxErrorTrackbarCallback); 
	cvCreateTrackbar("MaxScale", "TestWindow", &maxScaleTrackbarVal, 10, maxScaleTrackbarCallback); 
	drawTemplateWindow(); 

	if( ShowCamVideo((HWND)cvGetWindowHandle("TestWindow"), INPUT_WINDOW_WIDTH, INPUT_WINDOW_HEIGHT) )
	{
		cvReleaseImage(&blankImage); //now that the video stream should be displaying we can free this image
		cvWaitKey(0);
		//cvcamStop();
	}

    cvcamExit();
	return 0;
}

//main function used solely for testing new template matching code without need of having camera attached
void debugTemplateMatching() { 
	/***BEGIN NEW CONVOLUTION DEBUG CODE****/
	IplImage *snapshotImg = cvLoadImage("VideoSnapshot.jpeg", 1); 
	IplImage *firstTemplate = cvLoadImage("1.jpg", 0); 
	//cvSeqPush(templatesSeq, &firstTemplate); 
	cvNamedWindow("Video", 0);
	//cvNamedWindow("Template", 0); 
	cvShowImage("Video", snapshotImg);
	//cvShowImage("Template", firstTemplate); 

	IplImage *greyscaleImg, *resultImg, *curTemplateImg; 

	curTemplateImg = firstTemplate; 

	//convert image to greyscale
	greyscaleImg = cvCreateImage(cvSize(snapshotImg->width, snapshotImg->height),  IPL_DEPTH_8U, 1); 
	cvCvtColor(snapshotImg, greyscaleImg, CV_RGB2GRAY); 
	double minMatchVal, maxMatchVal;
	CvPoint minMatchPoint, maxMatchPoint; 

	resultImg = cvCreateImage(
		cvSize(greyscaleImg->width - curTemplateImg->width+1, greyscaleImg->height - curTemplateImg->height + 1), //w x h dimensions
		IPL_DEPTH_32F, //depth
		1				//# of channels
		); 
	
	cvCustomMatchTemplate(
		greyscaleImg,
		curTemplateImg,
		resultImg, 
		CV_TM_CCORR_NORMED
		); 

	for (int i=0; i<resultImg->width; i++) { 
		for (int j=0; j<resultImg->height; j++) { 
			
			
			if (I(resultImg, i, j)!=-1. && I(resultImg, i, j)!=1. && I(resultImg, i, j)!=0.0) { 
				printf("(%d, %d): %f\n", i, j, I(resultImg, i, j)); 
			}
			
		}
	}

	cvMinMaxLoc(resultImg, 
		&minMatchVal, 
		&maxMatchVal,
		&minMatchPoint,
		&maxMatchPoint, 
		NULL);
	
	CvPoint bestMatchPoint;

	if (false) { 
		bestMatchPoint.x = minMatchPoint.x + (curTemplateImg->width>>1);
		bestMatchPoint.y = minMatchPoint.y + (curTemplateImg->height>>1); 
	}
	else { 
		bestMatchPoint.x = maxMatchPoint.x + (curTemplateImg->width>>1);
		bestMatchPoint.y = maxMatchPoint.y + (curTemplateImg->height>>1); 
	}
	cvCircle(snapshotImg, bestMatchPoint, 10, cvScalarAll(255), 2, 8, 0); 

	cvShowImage("Video", snapshotImg);
	//cvShowImage("Template", firstTemplate); 
	
	printf("Img: (%d,%d); Template: (%d,%d)\n", greyscaleImg->width, greyscaleImg->height, firstTemplate->width, firstTemplate->height); 
	/*
	CvPoint *bestMatch = findBestTemplateMatch(greyscaleImg, firstTemplate); 
	printf("Best match @ (%d,%d) versus (%d, %d)\n",bestMatch->x, bestMatch->y, minMatchPoint.x, minMatchPoint.y);  
	*/
	cvWaitKey(0); 

	/***END CONVOLUTION DEBUG CODE****/
}

JNIEXPORT void JNICALL Java_OpenCV_openCamera
(JNIEnv *env, jobject obj) {
	/*
	DWORD dwThreadID; //contains ID used to reference camera thread
	HANDLE hCameraThread; //handle to the camera thread
	hCameraThread = CreateThread( 
            NULL,              // default security attributes
            0,                 // use default stack size  
            LaunchCameraThread,// thread function 
            NULL,             // argument to thread function 
            0,                 // use default creation flags 
            &dwThreadID);   // returns the thread identifier 
*/
	//debugTemplateMatching(); 
	
	globalCalledObject = env->NewGlobalRef(obj); 
	/*
	jclass cls=env->GetObjectClass(globalCalledObject);
	jmethodID mid=env->GetMethodID(cls, 
		"cCalled", 
		"(I)V");
	if (mid == 0) {
		printf("Can't find method sendArrayResults\n");
		return;
	}
	jint test = 5; 
	env->ExceptionClear();
	//env->CallVoidMethod(globalCalledObject, mid, test);
	*/
	globalEnv = env; 
	
	if (env->GetJavaVM(&jvm)!=0) { 
		printf("Error getting JVM\n"); 
	}
	
	LaunchCameraThread(0); 
	testVal = 2343; 
}

JNIEXPORT jintArray JNICALL Java_OpenCV_getImage
(JNIEnv *env, jobject) {
	IplImage *greyscaleCurrentImg; 
	greyscaleCurrentImg = cvCreateImage(cvSize(currentImage->width, currentImage->height),  IPL_DEPTH_8U, 1); 
	cvCvtColor(currentImage, greyscaleCurrentImg, CV_RGB2GRAY); 
	//#define I(img,x,y) ((uchar*)(img->imageData + img->widthStep*y))[x]

	jintArray picArray = (*env).NewIntArray(greyscaleCurrentImg->width * greyscaleCurrentImg->height); 
	/*
	for (int h=0; h<currentImage->width; h++) { 
		(*env).SetIntArrayRegion(picArray, h*greyscaleCurrentImg->width, greyscaleCurrentImg->width,(const jint *) ((uchar*)(greyscaleCurrentImg->imageData + greyscaleCurrentImg->widthStep*h)));
	}
	*/

	return picArray; 
}

JNIEXPORT jint JNICALL Java_OpenCV_getImageWidth
(JNIEnv *, jobject) {
	//testVal = 240; 
	return testVal; 
	//return currentImage->width; 
	//return 320; 
}

JNIEXPORT jint JNICALL Java_OpenCV_getImageHeight
(JNIEnv *, jobject) {
	//return currentImage->height; 
	return 240; 
}

BOOL APIENTRY DllMain( HANDLE hModule, 
                       DWORD  ul_reason_for_call, 
                       LPVOID lpReserved
					 )
{
    return TRUE;
}

