#include <stdlib.h>
#include <stdint.h>
#include <mmintrin.h>
#include <unistd.h>
#include <math.h>
#include <sys/time.h>
#include "config.h"
#include "gui.h"
#include "detect.h"
#include "convert.h"
#include "configuration.h"	// needed to get V4L formats

extern Detect detect;
extern PosCtl posctl;

//
// 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);
	imageRGBout.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;
	autodetecttype = 0;
	autofollowtype = 0;
};



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



/*
 * copy the framedata into the next thread. Source data must be already mutex locked.
 * raw data will only be copied if the source is not an compressed stream
 */
int Detect::CopyNewFrame(VideoFrame *newframe, VideoFrameRaw *rawframe) {
	if (newframe == NULL) return -1;
	LockInMutex();

	if (rawframe->pixfmt != V4L2_PIX_FMT_MJPEG)
		inRawFrame.CopyFrom(rawframe);
	else
		inRawFrame.Delete();

	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 doutput;
	struct timeval timestamp;
	double dt;
	int oldX, oldY;

	objectX = -1;
	objectY = -1;
	doutput.detmatrix = (uint32_t*)malloc (sizeof(uint32_t) * DET_MAXSHIFT * DET_MAXSHIFT);
	gettimeofday (&timestamp, NULL);

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

			oldX = objectX;
			oldY = objectY;

			LockMutex();					// lock Config

			//
			// 1. check near the last known position.
			//
			if (objectX >= objectW/2 && objectX <= (inFrame.w-objectW/2) &&
				objectY >= objectH/2 && objectY <= (inFrame.h-objectH/2))
			{
				switch (autofollowtype) {
				case AUTOFOLLOW_BRIGHT:
					InputDetect(&objectX, &objectY);
					break;
				case AUTOFOLLOW_CROSSC:
					InputDetectCrossC(&objectX, &objectY);
					break;
				default:
					break;
				}

				if (abs(oldX - objectX) >= DET_MAXSHIFT/5 || abs(oldY - objectY) >= DET_MAXSHIFT/5) {
					objectX = -1;
					objectY = -1;
				}
			}

			//
			// 2. 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;
				}
			}

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

			// copy output image for gtk
			LockImageMutex();
			imageRGBout.CopyFrom(&image);
			if (imageRawout.RectCopyFrom(&inRawFrame, (objectX-objectW/2), (objectY-objectH/2), objectW, objectH) == 0)
				imageRawout.CopyFrom(&imageRGBout);
			UnLockImageMutex();

			doutput.posx = objectX;
			doutput.posy = objectY;
			doutput.image = &imageRGBout;
			doutput.rawimage = &imageRawout;
			if (detmatrix != NULL)
				memcpy (doutput.detmatrix, detmatrix, sizeof(uint32_t) * DET_MAXSHIFT * DET_MAXSHIFT);

			posctl.Loop (objectX, objectY, dt);

			UnLockMutex();					// unlock detect dataset

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

		usleep (10000);	// FIXME:: why is it here?
	}
	free (doutput.detmatrix);
}


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);
		}
		detmatrix = (uint32_t*) malloc (sizeof(uint32_t) * DET_MAXSHIFT * DET_MAXSHIFT);

		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;

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

	//
	// 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);
		}
	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;

	// 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);
			for (i = 0; i < 3; i++) pxi[didx+i] = pxs[idx+i];
		}

	return;
}

/* returns approx. 8-bit gray value */
inline uint8_t calc_gray(uint8_t r, uint8_t g, uint8_t b) {
  /* Gray ~ 0.3*R + 0.59*G + 0.11*B -> approx. 0.28125*R + 0.625*G + 0.09375*B */
  return (r >> 2) + (r >> 5) + (g >> 1) + (g >> 4) + (b >> 4) + (b >> 5);
}

/* returns 4-vector product */
inline uint32_t calc_vector(uint16_t a1, uint16_t a2, uint16_t b1, uint16_t b2, uint16_t c1, uint16_t c2, uint16_t d1, uint16_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 52
void Detect::InputDetectCrossC(int *posx, int *posy) {
	unsigned char *pxi;	// input image
	unsigned char *pxo; // old image
	int inx, iny, oldx, oldy;

	int shiftx, shifty, x, y, ini, oldi, mxi, mxx, mxy;
	uint32_t sp;

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


	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;

	mxx = mxy = 0;
	for (shifty = 0; shifty < DET_MAXSHIFT; shifty++) {
		mxi = shifty * DET_MAXSHIFT;
		for (shiftx = 0; shiftx < DET_MAXSHIFT; shiftx++, mxi++) {
			sp = 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 - DET_MAXSHIFT/2;
				iny = oldy + shifty - DET_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) {
							uint16_t ogray[4];
							uint16_t igray[4];
							ogray[0] = calc_gray( pxo[oldi+0], pxo[oldi+1], pxo[oldi+2]);
							ogray[1] = calc_gray( pxo[oldi+3], pxo[oldi+4], pxo[oldi+5]);
							ogray[2] = calc_gray( pxo[oldi+6], pxo[oldi+7], pxo[oldi+8]);
							ogray[3] = calc_gray( pxo[oldi+9], pxo[oldi+10], pxo[oldi+11]);
							igray[0] = calc_gray( pxi[oldi+0], pxi[ini+1], pxi[ini+2]);
							igray[1] = calc_gray( pxi[oldi+3], pxi[ini+4], pxi[ini+5]);
							igray[2] = calc_gray( pxi[oldi+6], pxi[ini+7], pxi[ini+8]);
							igray[3] = calc_gray( pxi[oldi+9], pxi[ini+10], pxi[ini+11]);
							sp += calc_vector( ogray[0], igray[0],
									  ogray[1], igray[1],
									  ogray[2], igray[2],
									  ogray[3], igray[3]);
							x+=3; oldi += 9; ini += 9; oldx+=3; inx+=3;
					}
				}
			}
			detmatrix[mxi] = sp;
			if (detmatrix[mxx + (mxy * DET_MAXSHIFT)] < sp) {
				mxx = shiftx;
				mxy = shifty;
			}
		}
	}

	*posx += (mxx-DET_MAXSHIFT/2);
	*posy += (mxy-DET_MAXSHIFT/2);
	CopyObjectImage (*posx, *posy);
	return;
}


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();
}