#include "motionDifference.h"
#include <Math.h>
//#include "glfw.h"

//===================================================
motionDifference::motionDifference(int width, int height, int threshold){

	thresh = threshold;

	prevImage = new unsigned char[width * height];
	memset(prevImage, 0, width * height);

	mDif = new unsigned char[width * height];
	memset(mDif, 0, width * height);

	w = width;
	h = height;
	centroid_x = 0.0f;
	centroid_y = 0.0f;

	//hacky centroid stuff
	min_size_thresh = 25;
	last_min_x = 0;
	last_min_y = 0;



}



//===================================================
void motionDifference::difference(unsigned char * imageSrc){

	int totalBytes = w * h;
	int change = 0;
	int p;

	//check for change
	for (int i = 1; i < h-1; i++){
    	for (int j = 1; j < w-1; j++){
       		p 	= i * w + j;
        	if(imageSrc[p] != prevImage[p]){
				change = 1;
				break;
			}
     	}
     }

	//printf("change = %d\n", change);

	//if change has occured recored the changed pixels
	int x_tot, y_tot, count;
	count = 1;
	x_tot = 0;
	y_tot = 0;

	if (change == 1){
		for (int i = 1; i < h-1; i++){
            for (int j = 1; j < w-1; j++){

                p 	= i * w + j;

				int diff = abs(imageSrc[p] - prevImage[p]);

				prevImage[p] = imageSrc[p];

				if (diff > thresh){
					mDif[p] = 140;

					//record centroid
					x_tot += j;
					y_tot += i;
					count++;

				}else{
					mDif[p] = 255;
				}
			} //end j loop
		} //end i loop

		total_pix = count;
		//printf("total pixels changes between frames = %d\n", total_pix);

		centroid_x = (x_tot / count) / float(w);
		centroid_y = (y_tot / count) / float(h);
		//printf("x= %f,  y= %f\n", centroid_x, centroid_y);

		//hack to keep centroid from not drifting back to 0,0
		if(total_pix > min_size_thresh){
			last_min_x = centroid_x;
			last_min_y = centroid_y;
		}

		centroid_x = last_min_x;
		centroid_y = last_min_y;
		//printf("x= %f,  y= %f\n", centroid_x, centroid_y);







	} //end if
} //end method

//===================================================
void motionDifference::draw(int x, int y, int xsize, int ysize){

    glPushMatrix();
	//draws red square at averaged motiondif (eg. centroid)
	glTranslatef(x,y,0);
	glLineWidth(2.0f);
	glBegin(GL_LINE_LOOP);

    	glColor3f(1.0, 0.0, .0);

		glVertex2f((centroid_x * xsize)-3, (centroid_y * ysize)-3);
		glVertex2f((centroid_x * xsize)-3, (centroid_y * ysize)+3);
		glVertex2f((centroid_x * xsize)+3, (centroid_y * ysize)+3);
		glVertex2f((centroid_x * xsize)+3, (centroid_y * ysize)-3);
	glEnd();

	glLineWidth(1.0f);

    glPopMatrix();
}
