#include "tracker.h"
#include "cvtypes.h"
#include "mytypes.h"
#include "motionflow.h"
#include "stdio.h"
#include "highgui.h"
#include <math.h>

/* tracks stuff the image, and modifies that IplImage with the new
 * tracked silhouette.
 * this code extracted and ported from sample windows application on
 * implementing condensation in opencv.  (original source code:
 * visl.technion.ac.il/projects/2001w30/docs/OpenCV) */

// Constants
#define SHIFT (12)
#define mmax(a, b) ((b) + (((a) - (b))&((a)>(b)?-1:0)))
#define mmin(a, b) ((a) - (((a) - (b))&((a)>(b)?-1:0)))

int div_table[] = 
{
       0, 1044480,  522240,  348160,  261120,  208896,  174080,  149211,
  130560,  116053,  104448,   94953,   87040,   80345,   74606,   69632,
   65280,   61440,   58027,   54973,   52224,   49737,   47476,   45412,
   43520,   41779,   40172,   38684,   37303,   36017,   34816,   33693,
   32640,   31651,   30720,   29842,   29013,   28229,   27486,   26782,
   26112,   25475,   24869,   24290,   23738,   23211,   22706,   22223,
   21760,   21316,   20890,   20480,   20086,   19707,   19342,   18991,
   18651,   18324,   18008,   17703,   17408,   17123,   16846,   16579,
   16320,   16069,   15825,   15589,   15360,   15137,   14921,   14711,
   14507,   14308,   14115,   13926,   13743,   13565,   13391,   13221,
   13056,   12895,   12738,   12584,   12434,   12288,   12145,   12006,
   11869,   11736,   11605,   11478,   11353,   11231,   11111,   10995,
   10880,   10768,   10658,   10550,   10445,   10341,   10240,   10141,
   10043,    9947,    9854,    9761,    9671,    9582,    9495,    9410,
    9326,    9243,    9162,    9082,    9004,    8927,    8852,    8777,
    8704,    8632,    8561,    8492,    8423,    8356,    8290,    8224,
    8160,    8097,    8034,    7973,    7913,    7853,    7795,    7737,
    7680,    7624,    7569,    7514,    7461,    7408,    7355,    7304,
    7253,    7203,    7154,    7105,    7057,    7010,    6963,    6917,
    6872,    6827,    6782,    6739,    6695,    6653,    6611,    6569,
    6528,    6487,    6447,    6408,    6369,    6330,    6292,    6254,
    6217,    6180,    6144,    6108,    6073,    6037,    6003,    5968,
    5935,    5901,    5868,    5835,    5803,    5771,    5739,    5708,
    5677,    5646,    5615,    5585,    5556,    5526,    5497,    5468,
    5440,    5412,    5384,    5356,    5329,    5302,    5275,    5249,
    5222,    5196,    5171,    5145,    5120,    5095,    5070,    5046,
    5022,    4998,    4974,    4950,    4927,    4904,    4881,    4858,
    4836,    4813,    4791,    4769,    4748,    4726,    4705,    4684,
    4663,    4642,    4622,    4601,    4581,    4561,    4541,    4522,
    4502,    4483,    4464,    4445,    4426,    4407,    4389,    4370,
    4352,    4334,    4316,    4298,    4281,    4263,    4246,    4229,
    4212,    4195,    4178,    4161,    4145,    4128,    4112,    4096
};
    
// Data Structures
typedef CvHistogram hist_type;

typedef struct {

	float x, y, width, height; // relative position of object window
	int Smin, Vmin;
	int nSamples;
	int view;

} CvCondensParams;

typedef struct {
	hist_type *hist;
	
	float orientation;
	float width;
	float length;
	CvRect window;
	
	int min_ch_val[6];
	int max_ch_val[6];
	
	int shift;
	
	float *thresh[MAX_COUNT];
	float thresh_buf[MAX_COUNT * 2];
	int threshold;
	
} CvCamShiftTracker;


CvCamShiftTracker m_cCamShift;
CvImageGroup color_transform_image_group;
IplImage *calc_back_project_image;
IplImage *trackobj_pre_color_image;

CvConDensation *ConDens;
CvCondensParams m_params;
CvRect m_object, m_Indicat1, m_Indicat2, m_X, m_VX, m_Y, m_VY;
CvPoint m_Old;
float Measurement[4];
bool IsInit;
float LBound[4];
float UBound[4];
float XCor;
float VXCor;
float YCor;
float VYCor;

void tracker(IplImage* image) {

	initTracker();
	initFilter();
	transform(image);

}

// here's where the good stuff begins
void transform(IplImage *image)
{
	float norm;
	int vxnorm, vynorm;
	CvPoint pp1, pp2;
	int i, j;

	m_object.x = cvRound(image->width * m_params.x);
	m_object.y = cvRound(image->height * m_params.y);
	m_object.width = cvRound(image->width * m_params.width);
	m_object.height = cvRound(image->height * m_params.height);
	applyCamShift(image, 1);
	CheckBackProject(image);

	cvConDensUpdateByTime(ConDens);
	m_object.x = cvRound(ConDens->State[0] - m_object.width * 0.5);
	m_object.y = cvRound(ConDens->State[2] - m_object.height * 0.5);
	
	applyCamShift(image, 0);

	CheckBackProject(image);

	//instead of drawing a rectangle, zero out everything outside the rectangle

	/* cvRectangle(image, */
/* 							cvPoint(m_object.x, m_object.y), */
/* 							cvPoint(m_object.x + m_object.width, m_object.y + */
/* 											m_object.height), */
/* 							-1, 1); */
	
	for (i = 0; i < IMAGE_HEIGHT; i++) {
		for (j = 0; j < IMAGE_WIDTH; j++) {
			
			if ((j < m_object.x) || (j > m_object.x + m_object.width) 
					|| (i < m_object.y) || (i > m_object.y + m_object.height)) {
				image->imageData[(3 * i * IMAGE_WIDTH) + (3 * j)] = 0;
				image->imageData[(3 * i * IMAGE_WIDTH) + (3 * j) + 1] = 0;
				image->imageData[(3 * i * IMAGE_WIDTH) + (3 * j) + 2] = 0;
			}
		}
	}

/* 	rectang(image, m_Indicat1, -1); */

	/* m_X.x = 10; */
/* 	m_X.y = 10; */
/* 	m_X.width = 50 * m_Old.x / image->width; */
/* 	m_X.height = 10; */
/* 	rectang(image, m_X, CV_RGB(0, 0, 255)); */
/* 	m_Y.x = 10; */
/* 	m_Y.y = 10; */
/* 	m_Y.width = 10; */
/* 	m_Y.height = 50 * m_Old.y / image->height; */
/* 	rectang(image, m_Y, CV_RGB(255, 0, 0)); */
/* 	m_Indicat2.x = 0; */
/* 	m_Indicat2.y = image->height - 50; */
/* 	m_Indicat2.width = 50; */
/* 	m_Indicat2.height = 50; */
/* 	rectang(image, m_Indicat2, -1); */
/* 	norm = cvSqrt(Measurement[1] * Measurement[1] + Measurement[3] * */
/* 							Measurement[3]); */
/* 	vxnorm = (fabs(Measurement[1]) > 5) ? (int) (12 * Measurement[1] / norm) */
/* 		: 0; */
/* 	vynorm = (fabs(Measurement[3]) > 5) ? (int) (12 * Measurement[3] / norm) */
/* 		: 0; */
/* 	pp1 = cvPoint(25, image->height - 25); */
/* 	pp2 = cvPoint(25 + vxnorm, image->height - 25 + vynorm); */
/* 	cvLine(image, pp1, pp2, 3, 0); */
/* 	cvSetImageData(image, 0, 0); */

/* leslie, i commented this out so i can mass render! :) */

/* 	cvvNamedWindow("got rectangle?", 0); */
/* 	cvvShowImage("got rectangle?", image); */
	
}

void CheckBackProject(IplImage *image)
{
	IplImage *src;

	if (m_params.view != 0) {
		src = get_back_project();
		if (src && src->imageData && image) {
			iplGrayToColor(src, image, 0, 0, 0);
		}
	}
}

IplImage *get_back_project()
{
	return (calc_back_project_image);
}

void applyCamShift(IplImage *image, bool initialize)
{
	CvSize size;
	int bins = 20;
	CvMat LB, UB;

	// tracker
	set_hist_dims(1, &bins);
	set_thresh(0, 1, 180);
	set_min_ch_val(1, m_params.Smin);
	set_max_ch_val(1, 255);
	set_min_ch_val(2, m_params.Vmin);
	set_max_ch_val(2, 255);

	cvGetImageRawData(image, 0, 0, &size);

	if (m_object.x < 0) m_object.x = 0;
	if (m_object.x > size.width - m_object.width - 1)
		m_object.x = size.width - m_object.width - 1;

	if (m_object.y < 0) m_object.y = 0;
	if (m_object.y > size.height - m_object.height - 1)
		m_object.y = size.height - m_object.height - 1;

	set_window(m_object);

	if (initialize) {
		cvClearHist(m_cCamShift.hist); //reset_histogram();
		update_histogram(image);
	}

	track_object(image);
	m_object = m_cCamShift.window;
	
	LBound[0] = (float) m_object.x;
	LBound[1] = (float) -m_object.width * 0.5f;
	LBound[2] = (float) m_object.y;
	LBound[3] = (float) -m_object.height * 0.5f;
	UBound[0] = (float) m_object.x + m_object.width;
	UBound[1] = (float) m_object.width * 0.5f;
	UBound[2] = (float) m_object.y + m_object.height;
	UBound[3] = (float) m_object.height * 0.5f;

	Measurement[0] = (float) m_object.x + m_object.width * 0.5f;
	Measurement[1] = initialize ? 0 : (float) (Measurement[0] - m_Old.x);
	Measurement[2] = (float) m_object.y + m_object.height * 0.5f;
	Measurement[3] = initialize ? 0 : (float) (Measurement[2] - m_Old.y);
	m_Old.x = cvRound(Measurement[0]);
	m_Old.y = cvRound(Measurement[2]);
	if (initialize) {
		LB = cvMat(4, 1, CV_32F, LBound);
		UB = cvMat(4, 1, CV_32F, UBound);
		//LB = cvMat(4, 1, CV_MAT4x1_32F, LBound);
		//UB = cvMat(4, 1, CV_MAT4X1_32F, UBound);
		cvConDensInitSampleSet(ConDens, &LB, &UB);
	}
	XCor = 1.5f / m_object.width;
	VXCor = 3.0f / m_object.width;
	YCor = 1.5f / m_object.height;
	VYCor = 3.0f / m_object.height;
	CondProbDens(ConDens, Measurement);

	m_Old.x = cvRound(Measurement[0]);
	m_Old.y = cvRound(Measurement[2]);
	
}

void CondProbDens(CvConDensation *CD, float *Measurement)
{
	float Prob = 1;
	int i;

	for (i = 0; i < CD->SamplesNum; i++) {
		Prob = 1;
		Prob *= (float)exp(-XCor * XCor * 
											 (Measurement[0] - CD->flSamples[i][0]) * 
											 (Measurement[0] - CD->flSamples[i][0]));
		Prob *= (float)exp(-VXCor * VXCor *
											 (Measurement[1] - CD->flSamples[i][1]) *
											 (Measurement[1] - CD->flSamples[i][1]));
		Prob *= (float)exp(-YCor * YCor *
											 (Measurement[2] - CD->flSamples[i][2]) *
											 (Measurement[2] - CD->flSamples[i][2]));
		Prob *= (float)exp(-VYCor * VYCor *
											 (Measurement[3] - CD->flSamples[i][3]) *
											 (Measurement[3] - CD->flSamples[i][3]));
		CD->flConfidence[i] = Prob;
	}
}

void track_object(IplImage *src_image)
{
	trackobj_find(calc_back_project(trackobj_post_color(
				trackobj_color_transform(trackobj_pre_color(src_image)))));

}

void trackobj_find(IplImage *image)
{
	CvConnectedComp out;
	CvBox2D box;

	assert(image);
	assert(image->imageData);

	CvRect _window = { m_cCamShift.window.x >> m_cCamShift.shift, 
										 m_cCamShift.window.y >> m_cCamShift.shift, 
										 m_cCamShift.window.width >> m_cCamShift.shift, 
										 m_cCamShift.window.height >> m_cCamShift.shift };
	cvCamShift(image,
						 _window,
						 cvTermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1),
						 &out, &box);
	m_cCamShift.orientation = box.angle;
	m_cCamShift.length = box.size.height;
	m_cCamShift.width = box.size.width;
	m_cCamShift.window.x = out.rect.x << m_cCamShift.shift;
	m_cCamShift.window.y = out.rect.y << m_cCamShift.shift;
	m_cCamShift.window.width = out.rect.width << m_cCamShift.shift;
	m_cCamShift.window.height = out.rect.height << m_cCamShift.shift;
	m_cCamShift.length *= 1 << m_cCamShift.shift;
	m_cCamShift.width *= 1 << m_cCamShift.shift;
	
}

IplImage *calc_back_project(CvImageGroup *image)
{
	calc_back_project_image = cvCloneImage(image->image[0]);
	cvCopyImage(image->image[0], calc_back_project_image);
	cvCalcBackProject(get_group(), calc_back_project_image,
										m_cCamShift.hist);
	return (calc_back_project_image);
}

CvImageGroup *trackobj_color_transform(IplImage *src_image)
{
	return (color_transform(src_image));
}

bool set_hist_dims(int c_dims, int *dims)
{
	int i;
	if (c_dims > 6 || c_dims <= 0) return (0);
	
	if (m_cCamShift.hist) {
		if (m_cCamShift.hist->c_dims == c_dims) {
			for (i = 0; i < c_dims; i++) {
				if (m_cCamShift.hist->dims[i] != dims[i]) break;
			}
			if (i == c_dims) return (1);
		}
		cvReleaseHist(&(m_cCamShift.hist));
	}

	m_cCamShift.hist = cvCreateHist(c_dims, dims, CV_HIST_ARRAY);
	for (i = 0; i < c_dims; i++) {
		m_cCamShift.thresh[i][0] = 0.f;
		m_cCamShift.thresh[i][1] = 255.f;
	}
	cvSetHistThresh(m_cCamShift.hist, &(m_cCamShift.thresh[0]), 1);
	return (m_cCamShift.hist != 0);

}

bool set_thresh(int channel, int min_val, int max_val)
{
	m_cCamShift.thresh[channel][0] = (float) min_val;
	m_cCamShift.thresh[channel][1] = (float) max_val;

	cvSetHistThresh(m_cCamShift.hist, &(m_cCamShift.thresh[0]), 1);
	return (1);
}

bool set_min_ch_val(int channel, int val)
{
	m_cCamShift.min_ch_val[channel] = val; return (1);
}

bool set_max_ch_val(int channel, int val)
{
	m_cCamShift.max_ch_val[channel] = val; return (1);
}

bool set_window(CvRect _window)
{
	m_cCamShift.window = _window; return (1);
}

void update_histogram(IplImage *src_image)
{
	calc_histogram(hist_post_color(
			 hist_color_transform(hist_pre_color(src_image))));
}

IplImage *hist_pre_color(IplImage *src_image)
{
	return (trackobj_pre_color(src_image));
}

IplImage *trackobj_pre_color(IplImage *image)
{
	int j;

	if (!m_cCamShift.shift) return (image);

	trackobj_pre_color_image = cvCloneImage(image);
	cvCopyImage(image, trackobj_pre_color_image);
	cvPyrDown((uchar *) image->imageData, (uchar *) trackobj_pre_color_image->imageData);
	//p_cvPyrDown8uC3R((uchar *) image->imageData, image->widthStep,
	//							 (uchar *) trackobj_pre_color_image->imageData,
	//							 trackobj_pre_color_image->widthStep,
	//							 cvSize(image->width, image->height), (CvFilter) 7);
	(trackobj_pre_color_image->width) >>= 1;
	(trackobj_pre_color_image->height) >>= 1;

	for (j = 1; j < m_cCamShift.shift; j++) {
		cvPyrDown((uchar *) trackobj_pre_color_image->imageData, 
							(uchar *) trackobj_pre_color_image->imageData);
		//p_cvPyrDown8uC3R((uchar *) trackobj_pre_color_image->imageData, 
		//							 trackobj_pre_color_image->widthStep,
		//							 (uchar *) trackobj_pre_color_image->imageData,
		//							 trackobj_pre_color_image->widthStep,
		//							 cvSize(trackobj_pre_color_image->width,
		//											trackobj_pre_color_image->height), 
		//							 (CvFilter) 7);
		(trackobj_pre_color_image->width) >>= 1;
		(trackobj_pre_color_image->height) >>= 1;
	}
	return (trackobj_pre_color_image);
}

CvImageGroup *hist_color_transform(IplImage *src_image)
{
	return (color_transform(src_image));
}

CvImageGroup *color_transform(IplImage *image)
{
	int planes = 3;
	int x, y, x1, dst_pitch, b, g, r, h, s, v, vmin, diff, vr, vg;
	IplImage *im;
	uchar *dst;

	uchar *src = (uchar *) image->imageData;
	int src_pitch = image->widthStep;

	// color_transform_image_group
	operatorEqualsInt(1);
	copyImage(image);
	
	im = color_transform_image_group.image[0];
	dst = (uchar *) im->imageData;
	dst_pitch = im->widthStep;

	assert(src);
	assert(dst);
	
	for (y = 0; y < image->height; y++, src += src_pitch, dst += dst_pitch)
		for (x = 0, x1 = 0; x < image->width * planes; x += planes, x1++) {

			b = src[x]; g = src[x+1]; r = src[x+2];
			v = mmax(r, g);
			v = mmax(v, b);
			vmin = mmin(r, g);
			vmin = mmin(vmin, b);
			
			diff = v - vmin;
			vr = v == r ? -1 : 0;
			vg = v == g ? -1 : 0;
			s = diff * div_table[v] >> SHIFT;
			h = (vr& (g - b)) + (~vr&((vg&(b-r+2*diff)) +
																((~vg)&(r-g+4*diff))));
			h = ((h * div_table[diff]*15 + (1 << (SHIFT+6))) >> (7+SHIFT)) 
				+ (h < 0 ? 30 * 6 : 0);

		}
	
	if (s >= m_cCamShift.min_ch_val[1] 
			&& v >= m_cCamShift.min_ch_val[2] 
			&& v <= m_cCamShift.max_ch_val[2])
		dst[x1] = (uchar) h;
	else dst[x1] = 0;
	
	return (&color_transform_image_group);
}

CvImageGroup *hist_post_color(CvImageGroup *src_image)
{
	return (trackobj_post_color(src_image));
}

CvImageGroup *trackobj_post_color(CvImageGroup *image)
{
	return (image);
}

void calc_histogram(CvImageGroup *image)
{
	IplROI roi = { 0, m_cCamShift.window.x >> m_cCamShift.shift, 
								 m_cCamShift.window.y >> m_cCamShift.shift,
								 m_cCamShift.window.width >> m_cCamShift.shift, 
								 m_cCamShift.window.height >> m_cCamShift.shift };
	int i, count = image->count;
	int hist_size;
	float scale, max_val;
	
	assert(image);
	for (i = 0; i < count; i++) {
		image->image[i]->roi = &roi;
	}
	cvCalcHist(get_group(), m_cCamShift.hist, 0);
	
	for (i = 0; i < count; i++) {
		image->image[i]->roi = 0;
	}
	max_val = 0;
	cvGetMinMaxHistValue(m_cCamShift.hist, 0, &max_val, 0, 0);
	hist_size = m_cCamShift.hist->dims[0] * m_cCamShift.hist->mdims[0];
	scale = 255 / max_val;
	for (i = 0; i < hist_size; i++) {
		m_cCamShift.hist->array[i] *= scale;
	}
}

void operatorEqualsInt(int _count)
{
	int i;

	assert(_count < MAX_COUNT);
	color_transform_image_group.count = _count;
	for (i = 0; i < _count; i++) {
		if (!color_transform_image_group.image[i]) 
			color_transform_image_group.image[i] = cvCreateImage(cvSize(1, 1), 8, 1);
	}
}

void copyImage(IplImage *another)
{
	IplImage *src;
	IplROI roi;
	CvSize src_size, dst_size;
	bool single = another->nChannels == 1 || cvGetImageCOI(another) != 0;
	int i;

	src = cvCloneImage(another);
	cvCopyImage(another, src);
	cvGetImageRawData(another, 0, 0, &src_size);
	src->roi = &roi;
	
	if (another->roi) {
		roi = *(another->roi);
	} else {
		roi.coi = 0;
		roi.xOffset = roi.yOffset = 0;
		roi.width = src->width;
		roi.height = src->height;
	}

	for (i = 0; i < color_transform_image_group.count; i++) {
		cvGetImageRawData(color_transform_image_group.image[i], 0, 0, &dst_size);
		if ((src_size.width != dst_size.width && src_size.height != dst_size.height) ||
				color_transform_image_group.image[i]->depth != another->depth) {
			cvReleaseImage(&(color_transform_image_group.image[i]));
			color_transform_image_group.image[i] = cvCreateImage(src_size, another->depth, 1);
		}
		if (!single) {
			if (i >= another->nChannels) continue;
			src->roi->coi = i + 1;
		}
		iplCopy(src, color_transform_image_group.image[i]);
	}

}

IplImage **get_group()
{
	return (&(color_transform_image_group.image[0]));
}

void rectang(IplImage *img, CvRect Rect, int color)
{
	int j;
	CvPoint p1, p2;
	
	for (j = 0; j < Rect.height; j++) {
		p1 = cvPoint(Rect.x, Rect.y + j);
		p2 = cvPoint(Rect.x + Rect.width, Rect.y + j);
		cvLine(img, p1, p2, color);
	}
}

void initTracker() {

	int i;
	
	m_cCamShift.length = m_cCamShift.width = m_cCamShift.orientation = 0;
	m_cCamShift.window = cvRect(0, 0, 0, 0);
	
	for (i = 0; i < MAX_COUNT; i++) m_cCamShift.thresh[i] = NULL;
	m_cCamShift.threshold = 0;
	m_cCamShift.shift = 0;

	for (i = 0; i < 6; i++) {
		m_cCamShift.min_ch_val[i] = 0;
		m_cCamShift.max_ch_val[i] = 255;
		m_cCamShift.thresh[i] = m_cCamShift.thresh_buf + i * 2;;
	}

	m_cCamShift.hist = NULL;

}

void initFilter() {
	m_params.nSamples = 64;
	m_params.x = 0.4f;
	m_params.y = 0.3f;
	m_params.width = 0.2f;
	m_params.height = 0.3f;

	m_params.Smin = 20;
	m_params.Vmin = 40;
	m_params.view = 0;

	m_Indicat1.x = 0;
	m_Indicat1.y = 0;
	m_Indicat1.width = 100;
	m_Indicat1.height = 60;
	IsInit = false;

	ConDens = cvCreateConDensation(4, 4, m_params.nSamples);
	CvMat Dyn = cvMat(4, 4, CV_MAT4x4_32F, ConDens->DynamMatr);
	cvmSetIdentity(&Dyn);
	cvmSet(&Dyn, 0, 1, 0.9);
	cvmSet(&Dyn, 2, 3, 0.9);
	
}
