#include <stdint.h>
#include <mmintrin.h>
#include <unistd.h>
#include <math.h>
#include "config.h"
#include "gui.h"
#include "detect.h"

extern Detect detect;

//
// C / C++ Wrapper for the thread function
//
gpointer _DetectThread (gpointer data) {
	detect.Thread ();
	return NULL;
};



Detect::Detect() { // @suppress("Class members should be properly initialized")
	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);

	g_mutex_init (&mutexin);
	g_mutex_init (&muteximage);
	g_mutex_init (&mutex);
	running = 1;
	inFrame.SetSize(64,  64);
	oldFrame.SetSize(64,  64);
	image.SetSize(64,  64);
	imagegtk.SetSize(64, 64);
	thread = NULL;
	thread = g_thread_new("Detect", _DetectThread, NULL);
	objectW = 300;
	objectH = 300;
	objectX = -1;
	objectY = -1;
	posmaxx = 0;
	posmaxy = 0;
	maxx = NULL;
	maxy = NULL;
	detmatrix = NULL;
	detmatrixv2 = NULL;
	autodetecttype = 0;
	autofollowtype = 0;
};



Detect::~Detect() {
	running = 0;
	if (thread) {
		g_thread_join (thread);
		thread = NULL;
	}
};



int Detect::NewFrame(VideoFrame *newframe) {
	if (newframe == NULL) return -1;
	LockInMutex();
	inFrame.CopyFrom(newframe);
	inFrameNew = 1;
	UnLockInMutex();
	return 0;
};


//
// NewFrame: will set new frame
// Thread: newFrame |------> Find Object --- not found  ---> send gui information
void Detect::Thread() {
	DetectOutput output;
	int errorinputtype = -1, x, y;

	objectX = -1;
	objectY = -1;

	while (running) {
		// check for new frame
		LockInMutex();
		if (inFrameNew == 1) {
			inFrameNew = 0;

			LockMutex();					// lock Config

			//
			// 1. if no position of the object is known, search brightest
			//    object on the inFrame image.
			if (objectX < objectW/2 || objectX > (inFrame.w-objectW/2) ||
				objectY < objectH/2 || objectY > (inFrame.h-objectH/2))
			{
				switch (autodetecttype) {
				case AUTODETECT_BRIGHT:
					InputDetect(&objectX, &objectY);
					break;
				default:
					break;
				}
			}
			//
			// 2. check near the last known position.
			//
			else {
				switch (autofollowtype) {
				case AUTOFOLLOW_BRIGHT:
					InputDetect(&objectX, &objectY);
					break;
				case AUTOFOLLOW_CROSSC:
					InputDetectCrossC(&objectX, &objectY);
					break;
				default:
					break;
				}
			}

			oldFrame.CopyFrom(&inFrame);
			UnLockInMutex();

			// copy output image for gtk
			LockImageMutex();
			imagegtk.CopyFrom(&image);
			UnLockImageMutex();

			output.posx = objectX;
			output.posy = objectY;
			output.image = &imagegtk;

			UnLockMutex();					// unlock Config

			gdk_threads_add_idle(cb_thread_detect , &output);
		}
		else
			UnLockInMutex();

		usleep (10000);
	}
}


void Detect::SetInputSize (int nw, int nh) {
	if (posmaxx < nw) {
		if (maxx != NULL) free (maxx);
		maxx = (float*) malloc (nw * sizeof (float));
	}
	if (posmaxy < nh) {
		if (maxy != NULL) free (maxy);
		maxy = (float*) malloc (nh * sizeof (float));
	}

	if ((posmaxx * posmaxy) < (nw * nh)) {
		printf ("%s:%d %s reset Object Position (%d,%d) < (%d,%d) detmatrix:%p\n", __FILE__, __LINE__, __FUNCTION__,
				posmaxx, posmaxy, nw, nh, detmatrix);

		if (detmatrix != NULL) {
			free (detmatrix);
			free (detmatrixv2);
		}
		detmatrix = (float*) malloc (sizeof(float) * nw * nh);
		detmatrixv2 = (uint16_t*) malloc (sizeof(uint16_t) * nw * nh);

		objectX = nw/2;
		objectY = nw/2;
		posmaxx = nw;
		posmaxy = nh;
	}
}


#define POWERVAL 10
// #define DEBUGTIMES 1
void Detect::InputDetect(int *posx, int *posy) {
	int x, y, i, dx, dy, sy;
	unsigned char *pxs, *pxi;
	int idx, didx;
	struct timeval t1, t2;
	float f;

	f = get_cycletime(&t1);
	t2 = t1;

	image.SetSize (objectW, objectH);
	SetInputSize(inFrame.w, inFrame.h);
	pxs = inFrame.data;
	pxi = image.data;
	*posx = 0;
	*posy = 0;

#ifdef DEBUGTIMES
	f = get_cycletime(&t1);
	printf ("%s:%d setup memory time needed:%f\n", __FILE__, __LINE__, f);
#endif
	//
	// maximum brightness
	for (x = 0; x < posmaxx; x++) maxx[x] = 0.0;
	for (y = 0; y < posmaxy; y++) maxy[y] = 0.0;
	for (x = 0; x < inFrame.w; x++)
		for (y = 0; y < inFrame.h; y++) {
			idx = 3*(inFrame.w * y + x);
			maxx[x] += pow(pxs[idx+0] + pxs[idx+1] + pxs[idx+2], POWERVAL);
			maxy[y] += pow(pxs[idx+0] + pxs[idx+1] + pxs[idx+2], POWERVAL);
		}
#ifdef DEBUGTIMES
	f = get_cycletime(&t1);
	printf ("%s:%d calculated maximum:%f\n", __FILE__, __LINE__, f);
	// select maximum
#endif
	for (x = 1; x < inFrame.w; x++) if (maxx[x] > maxx[*posx]) *posx = x;
	for (y = 1; y < inFrame.h; y++) if (maxy[y] > maxy[*posy]) *posy = y;
#ifdef DEBUGTIMES
	f = get_cycletime(&t1);
	printf ("%s:%d selected maximum:%f\n", __FILE__, __LINE__, f);
#endif

	// select start corner
	if (*posx < (objectW / 2)) x = 0;
	else if (*posx + (objectW /2) > inFrame.w) x = inFrame.w - objectW;
	else x = ((*posx) - (objectW / 2));

	if (*posy < (objectH / 2)) sy = 0;
	else if (*posy + (objectH / 2) > inFrame.h) sy = inFrame.h - objectH;
	else sy = ((*posy) - (objectH / 2));

	for (dx = 0; dx < image.w && x < inFrame.w; x++, dx++)
		for (dy = 0, y = sy; dy < image.h && y < inFrame.h; y++, dy++) {
			idx = 3* (inFrame.w * y + x);
			didx = 3* (image.w * dy + dx);
			// printf ("[%d , %d] --> [%d, %d]    idx: %d --> %d    Value:%d\n", x, y, dx, dy, idx, didx, pxs[idx+i]);
			for (i = 0; i < 3; i++) pxi[didx+i] = pxs[idx+i];
		}

//	printf ("%s:%d %s pos: %d,%d  \n", __FILE__, __LINE__, __FUNCTION__, *posx, *posy);

#ifdef DEBUGTIMES
	f = get_cycletime(&t1);
	printf ("%s:%d copy output:%f\n", __FILE__, __LINE__, f);
	f = get_cycletime(&t2);
	printf ("%s:%d time needed:%f\n", __FILE__, __LINE__, f);
#endif
}

inline float calc_vector(uint8_t a1, uint8_t a2, uint8_t b1, uint8_t b2, uint8_t c1, uint8_t c2, uint8_t d1, uint8_t d2) {
  return a1 * a2 + b1 * b2 + c1 * c2 + d1 * d2;
  // __m64 m1 = _mm_set_pi16 (a1, b1, c1, d1);
  // __m64 m2 = _mm_set_pi16 (a2, b2, c2, d2);
  // __m64 m3 = _mm_madd_pi16 (m1, m2);
  // _mm_empty ();
  // uint32_t *data = (uint32_t *) &m3;
  // return data[0] + data[1];
}

#define OBJSIZE 50
#define MAXSHIFT 20
// #define DEBUGTIMES 1
void Detect::InputDetectCrossC(int *posx, int *posy) {
	unsigned char *pxi;	// input image
	unsigned char *pxd; // destination image
	unsigned char *pxo; // old image
	int inx, iny, oldx, oldy;

	struct timeval t1, t2;
	int shiftx, shifty, x, y, ini, oldi, desti, mxi, mxx, mxy;
	float f;

	if (oldFrame.h != inFrame.h || oldFrame.w != inFrame.w || *posx == -1 || *posy == -1) {
		*posx = -1;
		*posy = -1;
		return;
	}

#ifdef DEBUGTIMES
	f = get_cycletime(&t1);
	t2 = t1;
#endif

	if (objectW > inFrame.w || objectH > inFrame.h) {
		printf ("%s:%d %s objectW,H (%d,%d) > inFrame.W,H (%d, %d)\n", __FILE__, __LINE__, __FUNCTION__,
			objectW, objectH, inFrame.w, inFrame.h);
		*posx = -1;
		*posy = -1;
		return;
	}

	image.SetSize (objectW, objectH);
	SetInputSize(inFrame.w, inFrame.h);
	pxi = inFrame.data;
	pxo = oldFrame.data;
	pxd = image.data;

#ifdef DEBUGTIMES
	f = get_cycletime(&t1);
	printf ("%s:%d setup memory time needed:%f\n", __FILE__, __LINE__, f);
#endif

	mxx = mxy = 0;
	for (shifty = 0; shifty < MAXSHIFT; shifty++) {
		// printf ("%s:%d %s shift (%d)\n", __FILE__, __LINE__, __FUNCTION__, shifty);
		mxi = shifty * objectW;
		for (shiftx = 0; shiftx < MAXSHIFT; shiftx++, mxi++) {
			// fixme: help help
			f = 0.0;

			for (y = 0; y < OBJSIZE; y++) {
				oldx = (*posx)- OBJSIZE/2;
				oldy = (*posy)- OBJSIZE/2 + y;
				oldi = 3 * (oldx + oldFrame.w * oldy);

				inx = oldx + shiftx - MAXSHIFT/2;
				iny = oldy + shifty - MAXSHIFT/2;
				ini = 3* (inx + inFrame.w * iny);

				for (x = 0; x < OBJSIZE; x++, oldi += 3, ini += 3, oldx++, inx++) {
					if (oldx >= 0 && oldy >= 0 && oldx < oldFrame.w && oldy <= oldFrame.h &&
						inx >= 0 && inx < inFrame.w && iny >= 0 && iny < inFrame.h) {
							//f += (float)(pxo[oldi+0])*(float)(pxi[ini+0]);
							f += calc_vector( pxo[oldi+0], pxi[ini+0],
									  pxo[oldi+3], pxi[ini+3],
									  pxo[oldi+6], pxi[ini+6],
									  pxo[oldi+9], pxi[ini+9]);
							x+=3; oldi += 9; ini += 9; oldx+=3; inx+=3;
					}
				}
			}
			detmatrix[mxi] = f;
			if (detmatrix[mxx + (mxy * objectW)] < f) {
				mxx = shiftx;
				mxy = shifty;
			}
		}
	}

//	printf ("%s:%d %s pos (%d,%d)   dpos (%d,%d)  newpos (%d,%d)\n", __FILE__, __LINE__, __FUNCTION__,
//			*posx, *posy, (mxx-MAXSHIFT/2), (mxy-MAXSHIFT/2), *posx - (mxx-MAXSHIFT/2), *posy - (mxy-MAXSHIFT/2));

	*posx += (mxx-MAXSHIFT/2);
	*posy += (mxy-MAXSHIFT/2);
	CopyObjectImage (*posx, *posy);

#ifdef DEBUGTIMES
	f = get_cycletime(&t1);
	printf ("%s:%d copy output:%f\n", __FILE__, __LINE__, f);
	f = get_cycletime(&t2);
	printf ("%s:%d time needed:%f\n", __FILE__, __LINE__, f);
#endif
}


void Detect::CopyObjectImage(int objx, int objy) {
	int x, y, desti, ini;
	unsigned char *pxi;	// input image
	unsigned char *pxd; // destination image

	pxi = inFrame.data;
	pxd = image.data;

	//
	// copy image data to destination
	for (x = 0; x < objectW; x++) for (y = 0; y < objectH; y++) {
		desti = 3*(x + y * objectW);
		ini = 3*((x + objx - objectW/2) + (y + objy - objectH/2) * inFrame.w);
		if (desti < 0 || desti > objectW*objectH*3) continue;
		if (ini < 0 || ini > inFrame.w*inFrame.h*3) continue;
		pxd[desti+0] = pxi[ini+0];
		pxd[desti+1] = pxi[ini+1];
		pxd[desti+2] = pxi[ini+2];
	}
}


//
// set/get the automatic follow mode
void Detect::SetFollowType(int intype) {
	LockMutex();
	autofollowtype = intype;
	UnLockMutex();
}


int Detect::GetFollowType() {
	int i;

	LockMutex();
	i = autofollowtype;
	UnLockMutex();

	return i;
}


// set/get the automatic detection mode
void Detect::SetDetectType(int intype) {
	LockMutex();
	autodetecttype = intype;
	UnLockMutex();
}


int Detect::GetDetectType() {
	int i;

	LockMutex();
	i = autodetecttype;
	UnLockMutex();

	return i;
}


//
// to set a new size we need to disable the detection for some moments.
// this will not setup any memory for te calculation buffers
void Detect::SetObjectSize(int neww, int newh) {
	int orgtype = autodetecttype;

	// save detection type
	LockMutex();
	autodetecttype = -1;
	UnLockMutex();

	usleep (500000); // wait 0.5 sec

	LockImageMutex();
	objectH = newh;
	objectW = neww;
	UnLockImageMutex();

	// restore orginal detection type
	LockMutex();
	autodetecttype = orgtype;
	UnLockMutex();
}


void Detect::GetObjectSize (int *ow, int *oh) {
	LockMutex();
	*ow = objectW;
	*oh = objectH;
	UnLockMutex();
}


void Detect::SetMinBrightness(int value) {
	LockMutex();
	minBright = value;
	UnLockMutex();
}


int Detect::GetMinBrightness() {
	int i;

	LockMutex();
	i = minBright;
	UnLockMutex();

	return i;
}



void Detect::GetObjectPos(int *x, int *y) {
	LockMutex();
	*x = objectX;
	*y = objectY;
	UnLockMutex();
}

void Detect::SetObjectPos(int x, int y) {
	printf ("%s:%d %s new position:%d,%d\n", __FILE__, __LINE__, __FUNCTION__, x, y);

	LockMutex();
	image.SetSize (objectW, objectH);
	SetInputSize(inFrame.w, inFrame.h);
	objectX = x;
	objectY = y;
	CopyObjectImage (objectX, objectY);
	UnLockMutex();
}