/*
* This file is part of robotreality
*
* Copyright(c) sschulz <AT> techfak.uni-bielefeld.de
* http://opensource.cit-ec.de/projects/robotreality
*
* This file may be licensed under the terms of the
* GNU General Public License Version 3 (the ``GPL''),
* or (at your option) any later version.
*
* Software distributed under the License is distributed
* on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either
* express or implied. See the GPL for the specific language
* governing rights and limitations.
*
* You should have received a copy of the GPL along with this
* program. If not, go to http://www.gnu.org/licenses/gpl.html
* or write to the Free Software Foundation, Inc.,
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*
* The development of this software was supported by the
* Excellence Cluster EXC 277 Cognitive Interaction Technology.
* The Excellence Cluster EXC 277 is a grant of the Deutsche
* Forschungsgemeinschaft (DFG) in the context of the German
* Excellence Initiative.
*
*/

#include "DotTracker.h"

///coriander settings:
///brightness 85
///auto exp MAN 7
///WHITEBAL OFF
///GAMMA 1
///SHUTTER MAN 440
///GAIN 49

#define DEBUG_TEST 0

DotTracker::DotTracker(char *filename){
	//init config file:
	cfg = new ConfigOptions();
	
	if (filename != NULL){
		cfg->load(filename);
	}
	
	//head tracking
	head_tracker = new HeadTracker(cfg);
	
	//init filters
	rgb_image_filter = new RGBImageFilter(cfg);
	
	blobfinder = new BlobFinder();
	
	//init eye finder
	eyefinder = new EyeFinder(cfg);
	
	//model
	marker_model = new MarkerModel(cfg);
	
	//init capture device
	init_camera(cfg->camera_id);
	
	//converter for angles
	unit_converter = new UnitConverter();
	
	//data output:
	#if CMAKE_XS2COMM_NOT_FOUND
	data_output = new DataOutputCSV(cfg);
	#else
	//allow output selection with config file:
	if (cfg->data_output_class == 1){
		data_output = new DataOutputLCM(cfg);
	}else{
		data_output = new DataOutputCSV(cfg);
	}
	#endif
	
	//visualizer
	output_vis = new OutputVisualizer(cfg, marker_model, eyefinder, head_tracker);
	
	//init main window
	namedWindow("MAIN", CV_WINDOW_AUTOSIZE); // CV_WINDOW_KEEPRATIO); //CV_WINDOW_AUTOSIZE);

	//add simple ui:
	createButton("re-fit model",&button_refit_model,this,CV_PUSH_BUTTON,1);
	createButton("set eye zero",&button_eye_zero,this,CV_PUSH_BUTTON,1);
	if(!cfg->grab_from_cam){
		createTrackbar("frame", "", &cfg->frame_number, cfg->frame_number_end);
		createButton("reset framecounter",&button_reset_framecounter,this,CV_PUSH_BUTTON,1);
	}

	createTrackbar("threshold 1 eyes", "", &cfg->eyefinder_brightness_threshold, 255, &trackbar_set_eyefinder_brightness, this);
	createTrackbar("threshold 2 eyes", "", &cfg->eyefinder_brightness_threshold2, 255, &trackbar_set_eyefinder_brightness, this);

	createButton("rotate view",&button_wrapper_bool,&cfg->rotate_resultview,CV_CHECKBOX,cfg->rotate_resultview);
	createButton("normalize view hist",&button_wrapper_bool,&cfg->normalize_resultview_histogram,CV_CHECKBOX,cfg->normalize_resultview_histogram);
	createButton("show normalized rgb",&button_wrapper_bool,&cfg->show_rgb_normalized,CV_CHECKBOX,cfg->show_rgb_normalized);
	createButton("show pixelfilter results",&button_wrapper_bool,&cfg->debug_show_pixelfilter_results,CV_CHECKBOX,cfg->debug_show_pixelfilter_results);
	createButton("show eyefinder results",&button_wrapper_bool,&cfg->debug_show_eyefinder_results,CV_CHECKBOX,cfg->debug_show_eyefinder_results);
	createButton("show model results",&button_wrapper_bool,&cfg->debug_show_model_results,CV_CHECKBOX,cfg->debug_show_model_results);
	createButton("show blobinfo text",&button_wrapper_bool,&cfg->debug_show_text,CV_CHECKBOX,cfg->debug_show_text);
	createButton("show results",&button_wrapper_bool,&cfg->debug_show_results,CV_CHECKBOX,cfg->debug_show_results);
	
	
	dummy_eye_radius_x = cfg->eye_radius_px.x * 10;
	createTrackbar("eye radius x (x10)", "", &dummy_eye_radius_x, 1000, &trackbar_set_double_10, &cfg->eye_radius_px.x);
	dummy_eye_radius_y = cfg->eye_radius_px.y * 10;
	createTrackbar("eye radius y (x10)", "", &dummy_eye_radius_y, 1000, &trackbar_set_double_10, &cfg->eye_radius_px.y);
	dummy_orientation_scale_factor_x = cfg->orientation_scale_factor.x * 100;
	createTrackbar("orientation scale factor X (x100)", "", &dummy_orientation_scale_factor_x, 200, &trackbar_set_double_100, &cfg->orientation_scale_factor.x);
	dummy_orientation_scale_factor_y = cfg->orientation_scale_factor.y * 100;
	createTrackbar("orientation scale factor Y (x100)", "", &dummy_orientation_scale_factor_y, 200, &trackbar_set_double_100, &cfg->orientation_scale_factor.y);
	
	
	createButton("2d fixation",&button_wrapper_bool,&cfg->fixation_2d_correction,CV_CHECKBOX,cfg->fixation_2d_correction);
	createButton("eyefinder hough",&button_wrapper_bool,&cfg->eyedetection_by_hough_circles,CV_CHECKBOX,cfg->eyedetection_by_hough_circles);	
	createButton("eyelid fake mode",&button_wrapper_bool,&cfg->eyelid_fake_mode,CV_CHECKBOX,cfg->eyelid_fake_mode);	
	
	
	dummy_eyelid_scale_factor = cfg->eyelid_scale_factor * 100;
	createTrackbar("eyelid scale factor (x100)", "", &dummy_eyelid_scale_factor, 200, &trackbar_set_double_100, &cfg->eyelid_scale_factor);
	
	
	createButton("update markermodel",&button_wrapper_bool,&cfg->update_markermodel,CV_CHECKBOX,cfg->update_markermodel);	
	createButton("visualize output",&button_wrapper_bool,&cfg->show_output,CV_CHECKBOX,cfg->show_output);
	createButton("live",&button_wrapper_bool,&cfg->free_running,CV_CHECKBOX,cfg->free_running);
	createButton("data output enabled",&button_wrapper_output,this,CV_CHECKBOX,cfg->data_output_active);

	
	
	createButton("calc head zero rot",&button_calc_head_zero_rot,this,CV_PUSH_BUTTON,1);
	
	dummy_mouth_scale_factor = cfg->mouth_scale_factor*1000;
	createTrackbar("mouth scale", "", &dummy_mouth_scale_factor, 1000, &trackbar_set_double_1000, &cfg->mouth_scale_factor);
	dummy_mouth_offset = cfg->mouth_offset*10;
	createTrackbar("mouth offset", "", &dummy_mouth_offset, 1000, &trackbar_set_double_10, &cfg->mouth_offset);
	
	
	
	data_output->add_input_gui();
	
// 	createTrackbar("TEST ANGLE", "", &angle_tmp_100, 90*100, &trackbar_set_double_100);
	
	//defaults
	reset_framecounter();
	set_eyefinder_brightness_threshold(cfg->eyefinder_brightness_threshold);
}

DotTracker::~DotTracker(){
	delete data_output;
	delete rgb_image_filter;
	delete eyefinder;
	delete unit_converter;
	delete output_vis;
	delete head_tracker;
	delete cfg;
}

void DotTracker::init_mousehandler(){
	cv::setMouseCallback("MAIN", &mouse_wrapper, (void*)this);
}

void DotTracker::refit_model(){
	marker_model->initialise_face_model();
}

void DotTracker::set_eye_zero(){
	//set eye zero position to current value (while looking straight)
	unit_converter->set_eye_zero(eyefinder->get_eye_pos_unfixated(0),eyefinder->get_eye_pos_unfixated(1));
	//hack
// 	printf("HACK: start calib with this button\nFIXME: extra button!\n");
// 	eyefinder->start_calibration();
}

void DotTracker::calc_head_zero_rot(){
	//try to estimate head zero rotation:
	//idea: eyes should be on a straight line.
	//1) measure eye angle:
	double eye_angle = get_angle(eyefinder->get_eye_pos_unfixated(0), eyefinder->get_eye_pos_unfixated(1));
	//2) measure current nose marker angle
	double nose_angle = -(get_angle(marker_model->get_nose_marker_vector()[0], marker_model->get_nose_marker_vector()[1]));
	//3) total rotation offset is sum of both:
	cfg->head_zero_rotation = eye_angle + nose_angle;
	//4) store location of marker:
	cfg->head_zero_pos = marker_model->get_nose_marker_vector()[1];
	//5) set imu zero rot
	head_tracker->set_zero_rot();
	
	//6) set eye straight position:
	eyefinder->set_eye_straight_position(marker_model->get_nose_marker_vector()[1]);
	
	//add mouth zero
	cfg->mouth_center_pos[MOUTH_LEFT]  = marker_model->get_mouth_marker_vector()[MarkerModel::MOUTH_LEFT_UPPER].to_point2d();
	cfg->mouth_center_pos[MOUTH_LEFT] += marker_model->get_mouth_marker_vector()[MarkerModel::MOUTH_LEFT_LOWER].to_point2d();
	cfg->mouth_center_pos[MOUTH_LEFT] *= 0.5;
	cfg->mouth_center_pos[MOUTH_CENTER]  = marker_model->get_mouth_marker_vector()[MarkerModel::MOUTH_CENTER_UPPER].to_point2d();
	cfg->mouth_center_pos[MOUTH_CENTER] += marker_model->get_mouth_marker_vector()[MarkerModel::MOUTH_CENTER_LOWER].to_point2d();
	cfg->mouth_center_pos[MOUTH_CENTER] *= 0.5;
	cfg->mouth_center_pos[MOUTH_RIGHT]  = marker_model->get_mouth_marker_vector()[MarkerModel::MOUTH_RIGHT_UPPER].to_point2d();
	cfg->mouth_center_pos[MOUTH_RIGHT] += marker_model->get_mouth_marker_vector()[MarkerModel::MOUTH_RIGHT_LOWER].to_point2d();
	cfg->mouth_center_pos[MOUTH_RIGHT] *= 0.5;
}


void DotTracker::set_eyefinder_brightness_threshold(int v){
	eyefinder->build_lookuptable(cfg->eyefinder_brightness_threshold, cfg->eyefinder_brightness_threshold2);
}

double DotTracker::get_angle(Point2d a, Point2d b){
	double angle = -atan( (a.x-b.x)/(a.y-b.y))* 180.0 / M_PI;
	return angle;
}

Mat DotTracker::get_rotation_translation_matrix(MarkerModel *m, double offset=0.0){
	marker_vector_t nose_marker =  m->get_nose_marker_vector();
	
	double angle = get_angle(nose_marker[0], nose_marker[1]);
// 	if (angle > 0.0){
// 		angle = angle-180.0;
// 	}
	
	angle = cfg->head_zero_rotation + angle + offset;
	
// 	angle = angle_tmp_100 / 100.0;
	
	if (angle < -90.0){
		angle = angle+ 180.0;
	}
	
	//rotate around marker 1
	Point2d src_center = nose_marker[1];
	
	Mat rot_matrix = getRotationMatrix2D(src_center, angle, 1.0);
	
	//resultview_rotated = Mat(Size(frame_rgb_filtered.size().height, frame_rgb_filtered.size().width), frame_rgb_filtered.type());
	
	if (cfg->head_zero_pos.x != 0.0){
		if (offset == 90.0){
			//90 deg rot -> calc shifting
// 			rot_matrix.at<double>(1,2) += frame_rgb_filtered.size().height/2.0 - nose_marker[1].center_y ;
			//keep vertical position (if initialized)
			rot_matrix.at<double>(0,2) += frame_rgb_filtered.size().width/2.0 -nose_marker[1].center_x;
		}else{
			//center horizontally:
			rot_matrix.at<double>(1,2) += cfg->head_zero_pos.y-nose_marker[1].center_y;
			//keep vertical position (if initialized)
			rot_matrix.at<double>(0,2) += cfg->head_zero_pos.x - nose_marker[1].center_x;
		}
	}
	
	return rot_matrix;
}

void DotTracker::remove_translation_and_rotation(MarkerModel *m, EyeFinder *e, HeadTracker *h){
	//fetch rot/translation matrix:
	Mat rot_trans = get_rotation_translation_matrix(m);
	
	//ok now we have to update all coordinates:
	imu_data_t head_euler = h->get_euler_angles();
	m->remove_translation_and_rotation(rot_trans);
	e->remove_translation_and_rotation(rot_trans, head_euler);
}

void DotTracker::do_tracking(){
	//try to grab first frame
	printf("> waiting for frame grabbing to begin\n");
	while(!grab_image()){}
	
	//init model
	refit_model();
	
	
	//initialise matrix like the input source
	frame_rgb_filtered = frame_rgb.clone();
// 	frame_rgb_filtered2= frame_rgb.clone();

	//initialise matrix as 8bit unsigned:
	frame_pixelfilter_result.create(frame_rgb.size(), CV_8UC1);

	
	double t_grab, t_frame, t_filter, t_frame60, t_blobfind, t_display, t_markermodel, t_eyefind;
	int framecount = 0;
	
	//tell the blobfinder where the color lookuptable is:
	blobfinder->set_lookuptable(rgb_image_filter->get_lookuptable());
	
	while(1){
		t_frame = (double)getTickCount();
		t_grab = (double)getTickCount();
		if (grab_image()){
			t_grab = ((double)getTickCount() - t_grab)/getTickFrequency();
			//frame_rgb_filtered = frame_rgb.clone();
			//imshow("RGBIN", frame_rgb); waitKey(100000);
			t_filter = (double)getTickCount();
			if ((cfg->debug_show_results) && (cfg->debug_show_pixelfilter_results)){
				if (cfg->show_rgb_normalized){
					rgb_image_filter->filter_image(&frame_rgb, &frame_rgb_filtered, &frame_pixelfilter_result);
				}else{
					frame_rgb.copyTo(frame_rgb_filtered);
				}
			}
			t_filter = ((double)getTickCount() - t_filter)/getTickFrequency();

			t_blobfind = (double)getTickCount();
			blobfinder->process_image(&frame_rgb);
			t_blobfind = ((double)getTickCount() - t_blobfind)/getTickFrequency();
			
			if (cfg->update_markermodel){
				t_markermodel = (double)getTickCount();
				marker_model->update(blobfinder->get_blob_vector());
				t_markermodel = ((double)getTickCount() - t_markermodel)/getTickFrequency();
			}
			
			t_eyefind = (double)getTickCount();
			eyefinder->process_image(&frame_rgb, *marker_model);
			t_eyefind = ((double)getTickCount() - t_eyefind)/getTickFrequency();
			
			
			//we have the marker data, now we need to apply a rotation & translation to correct any shifting images
			remove_translation_and_rotation(marker_model, eyefinder, head_tracker);
			
			
			//dump data
			if (cfg->free_running){
				//add angles to markers
				unit_converter->process_data(marker_model, eyefinder, cfg->frame_number);
				//output marker data
				data_output->process_data(marker_model, eyefinder, head_tracker, cfg->frame_number);
			}
			
			t_display = (double)getTickCount();
			
			if (cfg->debug_show_results){
				if (!cfg->debug_show_pixelfilter_results){
					//show black image:
					frame_rgb_filtered =  Mat::zeros(frame_rgb_filtered.size(), frame_rgb_filtered.type());
				}else if (cfg->rotate_resultview){
					Mat rot_matrix = get_rotation_translation_matrix(marker_model);
// 					resultview_rotated = Mat(Size(frame_rgb_filtered.size().height, frame_rgb_filtered.size().width), frame_rgb_filtered.type());
					frame_rgb_filtered.copyTo(frame_rgb_filtered_rot);
					warpAffine(frame_rgb_filtered_rot, frame_rgb_filtered, rot_matrix, frame_rgb_filtered.size());
					
// 					imshow("MAIN", resultview_rotated);
				}
				
				//show results
				blobfinder->overlay_blobs_on_image(&frame_rgb_filtered);
				eyefinder->overlay_blobs_on_image(&frame_rgb_filtered);
				if (cfg->debug_show_model_results){
					marker_model->overlay_blobs_on_image(&frame_rgb_filtered);
				}
				
				if (cfg->debug_show_text){
					blobfinder->overlay_text_on_image(&frame_rgb_filtered);
					eyefinder->overlay_text_on_image(&frame_rgb_filtered);
					marker_model->overlay_text_on_image(&frame_rgb_filtered);
				}
				
				if(cfg->normalize_resultview_histogram){
					normalize_histogram(&frame_rgb_filtered);
				}
				
				if (cfg->show_output){
					output_vis->overlay_blobs_on_image(&frame_rgb_filtered);
				}
					
				
				if (cfg->rotate_resultview){
					//rotate resultview:
					transpose(frame_rgb_filtered, resultview_rotated);
					flip(resultview_rotated, resultview_rotated, 0);
					
// 					Mat rot_matrix = get_rotation_translation_matrix(&marker_model, 90.0);
// 					warpAffine(frame_rgb_filtered, resultview_rotated, rot_matrix, resultview_rotated.size());
					
					
					imshow("MAIN", resultview_rotated);
				}else{
					//overlay output
					imshow("MAIN", frame_rgb_filtered);
				}
				
				
				
				
				if (cfg->debug_show_eyefinder_results) eyefinder->show_debug_image();
			}

			#if WRITE_FILES
			char filename[256];
			sprintf(filename, "/tmp/output_%06d.pnm",cfg->frame_number);
			if (cfg->rotate_resultview){
				imwrite(filename, resultview_rotated);
			}else{
				imwrite(filename, frame_rgb_filtered);
			}
			#endif


			int key_pressed = 0;
			if (cfg->grab_from_cam){
				key_pressed = waitKey(1);
			}else{
 				key_pressed = waitKey(3);
			}
			
			t_display = ((double)getTickCount() - t_display)/getTickFrequency();
			
			switch(key_pressed & 0xFFFF){
				case('0'):
				case('1'):
				case('2'):
				case('3'):
					colorclass_select(key_pressed - '0');
					break;
					
				case('e'):
				case('E'):
					colorclass_clear();
					break;
			
				case('X'):
				case('x'):
					printf("> X key pressed exiting...\n");
					delete this;
					exit(0);
					
				case('s'):
				case('S'):
					cfg->save("/tmp/cfg");
					break;
					
				case('c'):
				case('C'):
// 					eyefinder->store_calib_point();
					break;
					
				case('r'):
				case('R'):
					///eyefinder->start_calibration();
					break;
				default:
					break;
			}

		}else{
			waitKey(1);
		}
		t_frame = ((double)getTickCount() - t_frame)/getTickFrequency();
		
		framecount++;
		if (framecount == 60){
			t_frame60 = ((double)getTickCount() - t_frame60)/getTickFrequency();
			if(t_frame60*1000.0/60.0 > 1000.0/60.0){
				printf("> WARNING: framedrop! avg loop took %4.2fms (>%2.1fms!) !\n",t_frame60*1000.0/60.0,1000.0/60.0);
			}
			
			printf("> stats: %3.2f fps -> full loop=%4.2fms grab=%4.2fms filter=%4.2fms blobfind=%4.2fms eyes=%4.2f marker=%4.2fms display=%4.2fms\n",60.0*1.0/t_frame60,t_frame*1000.0,t_grab*1000.0,t_filter*1000.0,t_blobfind*1000.0,t_eyefind*1000.0,t_markermodel*1000.0,t_display*1000.0);
			t_frame60 = (double)getTickCount();
			framecount=0;
		}
		
		
	}
}

void DotTracker::colorclass_select(int color_class){
	if ((color_class < 0) && (color_class > 2)){
		printf("> WARNING: discarding select colorclass %d (invalid class)\n",color_class);
		return;
	}
	
	printf("> selected colorclass %d\n", color_class);
	
	char buf[128];
	sprintf(buf, "selected colorclass %d",color_class);
	displayOverlay("MAIN", buf, 1500);
	
	cfg->colorclass_selected = color_class;
}


void DotTracker::colorclass_clear(){
	rgb_image_filter->colorclass_clear();
	printf("> erased lookuptable\n");
	displayOverlay("MAIN", "colorclass lookuptable cleared", 1500);
}

void DotTracker::set_output(bool state){
	if (data_output != NULL){
		printf("> setting output %s\n",state?"active":"inactive");
		cfg->frame_number = 0;
		cfg->data_output_active = state;
		data_output->set_output_active(state);
	}
}

void DotTracker::init_camera(int idx){
	first = true;
	//init
	frame_rgb =  Mat::ones(Size(654,480), CV_8UC1);
	
	if ((!cfg->grab_from_cam) || (idx == -1)){
		printf("> grabbing from cam disabled, will use image files\n");
		return;
	}
	
	//open camera
	printf("> will open cv camera with index %d\n",idx);
	camera = new VideoCapture(idx);
	
	if(!camera->isOpened()){
		printf("> ERROR: failed to open cam %d\n",idx);
		exit(1);
	}
#if 1
	if (!camera->set(CV_CAP_PROP_MODE, DC1394_VIDEO_MODE_FORMAT7_0)){
		printf("> ERROR: setting mode 7:0 failed\n");
		exit(1);
	}
	
	if ((!camera->set(CV_CAP_PROP_FRAME_HEIGHT, 524)) || (!camera->set(CV_CAP_PROP_FRAME_WIDTH, 652)) ){
		printf("> ERROR: setting size (mode 7:0 failed\n");
		exit(1);
	}
	
	if (!camera->set(CV_CAP_PROP_FPS, 0.0)){
		printf("> ERROR: set 0fps (=auto for mode7) failed\n");
		exit(1);
	}

#else	
	if (!camera->set(CV_CAP_PROP_FPS, 60.0)){
		printf("> ERROR: set 60fps failed\n");
		exit(1);
	}
#endif
}

void DotTracker::handle_mouse_event(int event, int x, int y, int flags){
	//printf("> mouse event %6d [%3d:%3d] flags %d\n",event,x,y,flags);
	if (event == CV_EVENT_LBUTTONDOWN){
		printf("> mouse left click at [%3d:%3d]\n",x,y);
		
		//rotated view?
		if(cfg->rotate_resultview){
			int tmp = frame_rgb.size().height - x;
			x = y;
			y = tmp;
		}
		
		printf("> mouse left click at [%3d:%3d]\n",x,y);
		
		//get pixel & set colorclass
		const cv::Vec3b& bgr = frame_rgb.at<cv::Vec3b>(y,x); //NOTE! X/Y SWAP!
		rgb_image_filter->colorclass_add_pixel(cfg->colorclass_selected, bgr[2], bgr[1], bgr[0]);
	}
}

void DotTracker::single_cvtColor(Mat source, Mat target, int bayer_mode){
	cvtColor(source,target, CV_BayerRG2RGB);
// 	printf("i %d\n",i);
// 	usleep(1000000);
}

//do bayer conversion in two threads:
void DotTracker::threaded_cvtColor(Mat source, Mat target, int bayer_mode){
// 	target = Mat(source);
	Mat target_upper_half = target(Rect(0,0,				target.size().width,target.size().height/2));
	Mat target_lower_half = target(Rect(0,target.size().height/2,	target.size().width,target.size().height/2-1));
	
	Mat source_upper_half = source(Rect(0,0,				source.size().width,source.size().height/2));
	Mat source_lower_half = source(Rect(0,source.size().height/2,	source.size().width,source.size().height/2-1));
	//imshow("UPPER", source_upper_half);
	//imshow("LOWER", source_lower_half);
	
	//now start two conversion threads:
	boost::thread thread1(boost::bind(&DotTracker::single_cvtColor, this, source_upper_half, target_upper_half, bayer_mode));
	boost::thread thread2(boost::bind(&DotTracker::single_cvtColor, this, source_lower_half, target_lower_half, bayer_mode));
// 	imshow("S UPPER", source_upper_half);
	
	thread1.join();
	thread2.join();
// 	imshow("OUTPUT", target);
	//boost::thread *thread2 = new boost::thread(boost::bind( &DataOutputLCM::output_thread, this));

}

bool DotTracker::grab_image(){
	if (cfg->grab_from_cam){
		if (cfg->camera_id == -1){
			//camera disabled hack:
			waitKey(1);
			return true;
		}
		
		if (!cfg->free_running){
			return true;
		}
		
		if (!camera->grab()){
			printf("> ERROR: grab failed\n");
			return false;
		}else{
			if (!camera->retrieve(frame_raw, 0)){
				printf("> ERROR: retrieve failed\n");
				return false;
			}else{
				if((frame_raw.size().height <= 0) ||(frame_raw.size().width <= 0)){
					printf("> ERROR: grab input image size = %d x %d?!\n",frame_raw.size().width,frame_raw.size().width);
					return false;
				}
				
				Mat *raw_ptr = &frame_raw;
				if (frame_raw.channels() == 3){
					//strange, the pointgrey cam gives us RAW values with channels=3
					//seems like every channel is equal to the other -> extract only one:
					split(frame_raw, frame_raw_planes);
					raw_ptr = &frame_raw_planes[0];
				}

				
				if (1){
					//cut out ROI from image;
					//set ROI 
					frame_raw_roi_upper = frame_raw(Rect(raw_ptr->size().width*1/3-1,0,raw_ptr->size().width*2/3,raw_ptr->size().height));
					if (first){
						cvtColor(*raw_ptr,frame_rgb, CV_BayerRG2RGB);
						frame_rgb =  Mat::zeros(raw_ptr->size(), frame_rgb.type());
						frame_rgb_roi = frame_rgb(Rect(frame_rgb.size().width*1/3-1,0,frame_rgb.size().width*2/3,frame_rgb.size().height));
						first = false;
					}else{
						///cvtColor(frame_raw_roi_upper,frame_rgb_roi, CV_BayerRG2RGB);
						threaded_cvtColor(*raw_ptr, frame_rgb, CV_BayerRG2RGB);
					}
				}else{
					//full frame
					cvtColor(*raw_ptr,frame_rgb, CV_BayerRG2RGB);
				}
				cfg->frame_number++;
				return true;
			}
		}
	}else{
		//grab image from file
		char filename[1024];
		
		if ((!cfg->free_running) && (cfg->frame_number == last_frame)){
			waitKey(1000/60);
	// 		return false;
		}
		
// 		printf("> FRAME %6d\n",cfg->frame_number);
		last_frame = cfg->frame_number;
		
		sprintf(filename,cfg->image_source_filename.c_str(),cfg->frame_number);
		frame_rgb = imread(filename);
		
		if(frame_rgb.data == NULL){
			printf("> cant read file '%s'\n",filename);
			if (cfg->free_running){
				exit(1);
			}else{
				reset_framecounter();
				return false;
			}
		}
		
		if (cfg->frame_number >= cfg->frame_number_end){
			//reset_framecounter();
			cfg->free_running = false;
		}
		
	// 	usleep(1000*1000); //simulate grabbing
	}
	if (cfg->free_running){
		cfg->frame_number++;
		setTrackbarPos("frame","MAIN", cfg->frame_number);
	}
	return true;
}

void DotTracker::reset_framecounter(){
	cfg->frame_number = 0; 
}

void DotTracker::normalize_histogram(Mat *img){
	Mat hsv;
	vector <Mat> hsv_vector;
	
	//convert to hsv
	cvtColor(*img, hsv, CV_BGR2HSV);
	//split to single channels
	split(hsv, hsv_vector);
	//equalize V channel
	equalizeHist(hsv_vector[2],hsv_vector[2]);
	//merge back
	merge(hsv_vector, hsv);
	//back to rgb
	cvtColor(hsv, *img, CV_HSV2BGR);
	
	//cleanup
	hsv_vector[0].release();
	hsv_vector[1].release();
	hsv_vector[2].release();
	hsv.release();
}