/*
* 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 "EyeFinder.h"

EyeFinder::EyeFinder(ConfigOptions *c){
	cfg = c;
	
	blobfinder[0].check_and_allocate(Size(640,480)); //tell the blobfinder to allocate 640x480px storage (not needed her but safe to do so now -> no runtime alloc)
	blobfinder[1].check_and_allocate(Size(640,480)); //tell the blobfinder to allocate 640x480px storage (not needed her but safe to do so now -> no runtime alloc)
	
	uninitialised[0] = true;
	uninitialised[1] = true;
	
	eye_pos[0] = Point2d(200+20,200);
	eye_pos[1] = Point2d(200-20,200);
	
	int colorsample_lookup_size = 256*256*256;
	color_lookup = (int *) malloc((colorsample_lookup_size)*sizeof(int));
	
	//default
	build_lookuptable(30, 20);
	
	//set names
	eye_calib[0] = new EyeCalib(cfg, 0);
	eye_calib[1] = new EyeCalib(cfg, 1);
	
	//eyelid offset for dummy mode:
	eyelid_offset = 10.0;
	
}

Point2d EyeFinder::get_eye_angle(int i){
	//we need to call calib tool with the difference of the eyes position to its straight position:
	return eye_calib[i]->calc_angle(eye_pos_straight_fixated[i]-eye_pos_fixated[i] ); //eye_pos_fixated[i]);
}

void EyeFinder::build_lookuptable(int brightness, int brightness2){
	threshold_value = brightness;
	
	int r_255, g_255, rgb;
	for(int r=0; r<256; r++){
		r_255 = r<<16;
		for(int g=0; g<256; g++){
			g_255 = r_255 | (g<<8);
			for(int b=0; b<256; b++){
				rgb = g_255 | b;
				int sum = r+g+b;
				
				if ((sum > brightness)){ /// || (r > 4*g/3)){
					color_lookup[rgb] = 0;
				}else{
					color_lookup[rgb] = 1;
				}
			}
			
		}
	}
	
	blobfinder[0].set_lookuptable(color_lookup);
	blobfinder[1].set_lookuptable(color_lookup);
}

void EyeFinder::init_kalman(int i,double x, double y){
	kalman_filter[i] = new KalmanFilter(4, 2, 0);
	kalman_filter[i]->transitionMatrix = *(Mat_<float>(4, 4) << 1,0,1,0,   0,1,0,1,  0,0,1,0,  0,0,0,1);
	kalman_filter[i]->statePre.at<float>(0) = x;
	kalman_filter[i]->statePre.at<float>(1) = y;
	kalman_filter[i]->statePre.at<float>(2) = 0.0;
	kalman_filter[i]->statePre.at<float>(3) = 0.0;
	setIdentity(kalman_filter[i]->measurementMatrix);
	setIdentity(kalman_filter[i]->processNoiseCov, Scalar::all(1e-3));
	setIdentity(kalman_filter[i]->measurementNoiseCov, Scalar::all(1e-1));
	setIdentity(kalman_filter[i]->errorCovPost, Scalar::all(.1));

	uninitialised[i] = false;
	
}

// void EyeFinder::set_lookuptable(int *table){ 
// 	color_lookup = table; 
// 	blobfinder.set_lookuptable(color_lookup);
// }


double EyeFinder::get_eyelid_angle_lower(int i){
	if (cfg->eyelid_fake_mode){
		return (get_eye_angle(i) - Point2d(eyelid_offset,eyelid_offset)).x;
	}else{
		return eyelid_lower[i].x;
	}
}

double EyeFinder::get_eyelid_angle_upper(int i){
	if (cfg->eyelid_fake_mode){
		return (get_eye_angle(i) + Point2d(eyelid_offset,eyelid_offset)).x;
	}else{
		return eyelid_upper[i].x;
	}
}

void EyeFinder::store_eye_position(int i, double x, double y){
	if (uninitialised[i]){
		init_kalman(i, x,y);
		return;
	}
	
	//copy new position from marker
	// First predict, to update the internal statePre variable
	Mat prediction = kalman_filter[i]->predict();
	Point2d predictPt(prediction.at<float>(0),prediction.at<float>(1));
		
	Mat_<float> measurement(2,1);
	measurement.setTo(Scalar(0));
	measurement(0) = x;
	measurement(1) = y;
		
	//the "correct" phase that is going to use the predicted value and our measurement
	Mat estimated = kalman_filter[i]->correct(measurement);
		
	eye_pos[i].x = estimated.at<float>(0);
	eye_pos[i].y = estimated.at<float>(1);

}


void EyeFinder::overlay_blobs(Mat *image){
	uchar *imagedata_ptr;
	int rgb,x,y;
	
	if (!image->isContinuous()){
		printf("> ERROR: EyeFinder: cant overlay blobs on image (image not contigous)\n");
		return;
	}

	//HACK: iterate to height-1 because we might access ONE byte more than allowed!
	for(y=0; y<image->size().height-1; y+=2){
		imagedata_ptr = image->ptr(y);
		
		for(x=0; x<image->size().width*3; x+=3*2){
			rgb = (*((int*)(imagedata_ptr + x))) & 0xFFFFFF;
			
			if (color_lookup[rgb] == 1){
				(*((int*)(imagedata_ptr+x))) = 0x7F0000;
			}else if (color_lookup[rgb] == 2){
				(*((int*)(imagedata_ptr+x))) = 0x007f00;
			}else if (color_lookup[rgb] == 3){
				(*((int*)(imagedata_ptr+x))) = 0x000f7f;
			}
		}
	}
}

void EyeFinder::set_eye_straight_position(Point2d nose_marker_lower){
	cfg->eye_pos_straight_offset[0] = get_eye_pos_unfixated(0) - nose_marker_lower;
	cfg->eye_pos_straight_offset[1] = get_eye_pos_unfixated(1) - nose_marker_lower;
}

// void EyeFinder::start_calibration(){
// 	eye_calib[0]->init_calib();
// 	eye_calib[1]->init_calib();
// }

void EyeFinder::process_image_eyelids(MarkerModel m){
	//calculate eyelid angles
	eyelid_upper[0] = calc_eyelid_angle(0, m.get_eye_marker(MarkerModel::EYE_LID_LEFT_UPPER));
	eyelid_lower[0] = calc_eyelid_angle(0, m.get_eye_marker(MarkerModel::EYE_LID_LEFT_LOWER));
	eyelid_upper[1] = calc_eyelid_angle(1, m.get_eye_marker(MarkerModel::EYE_LID_RIGHT_UPPER));
	eyelid_lower[1] = calc_eyelid_angle(1, m.get_eye_marker(MarkerModel::EYE_LID_RIGHT_LOWER));
	
}

Point2d EyeFinder::calc_eyelid_angle(int e, Point2d marker){
	Point2d dist = eye_pos_straight_fixated[e] - marker;
	Point2d calib_factor = cfg->eye_radius_px *cfg->eyelid_scale_factor;
	
	if (abs(dist.x) > abs(calib_factor.x)){
		if (dist.x < 0) {
			dist.x = -abs(calib_factor.x);
		}else{
			dist.x = abs(calib_factor.x);
		}
	}
	if (abs(dist.y) > abs(calib_factor.y)){
		if (dist.y < 0) {
			dist.y = -abs(calib_factor.y);
		}else{
			dist.y = abs(calib_factor.y);
		}
	}
	
	double ax = -asin(dist.x / calib_factor.x) * 180.0 / M_PI;
	double ay = asin(dist.y / calib_factor.y) * 180.0 / M_PI;
	return Point2d(ax,ay);
}

void EyeFinder::process_image(Mat *image, MarkerModel m){
	if((image->size().height <= 0) ||(image->size().width <= 0)){
		printf("> input image size = %d x %d?!\n",image->size().width,image->size().width);
		return;
	}
	
	//store eye straight position
	eye_pos_straight[0] = (Point2d)m.get_eye_marker(MarkerModel::EYE_NOSE_LOWER) + cfg->eye_pos_straight_offset[0];
	eye_pos_straight[1] = (Point2d)m.get_eye_marker(MarkerModel::EYE_NOSE_LOWER) + cfg->eye_pos_straight_offset[1];
	
	//process eye coordinates
	process_image_eye(0, image, m.get_eye_marker(MarkerModel::EYE_LID_LEFT_UPPER),  m.get_eye_marker(MarkerModel::EYE_LID_LEFT_LOWER));
	process_image_eye(1, image, m.get_eye_marker(MarkerModel::EYE_LID_RIGHT_UPPER),  m.get_eye_marker(MarkerModel::EYE_LID_RIGHT_LOWER));
	
	
	//process eyelids:
	process_image_eyelids(m);
	
}

void EyeFinder::process_image_eye(int i, Mat *image,  Marker upper, Marker lower){
	double xs = min(upper.center_x,lower.center_x);
	double xe = max(upper.center_x,lower.center_x);
	double h  = xe - xs;
	double y = (upper.center_y+lower.center_y)/2;
	
// 	printf("> %s(...) -> %f,%f h %f w%f\n", __func__,xs,xe,h,y);
// 	cout << image->size();
	if ((h<0) || (xs<0) || (xe<xs) || (y<0)){
		printf("> Eyefinder::process_image_eye(%d) -> invalid eyesize [X=%f->%f:%f] [Y=%f]\n",i,xs,xe,h,y);
		return;
	}
	
	
	
// 	printf("xs %f xe %f h %f y %f\n",xs,xe,h,y);
	
	double w = 100;
	y = max(0.0, y - w/2);
	xs = max(0.0, xs-10);
	h = h +2*10;
	
	w = min(w, image->size().height-y-1);
	h = min(h, image->size().width-xs-1);
	
	w = max(1.0, w);
	h = max(1.0, h);
	
	if (w<0){
		printf("> Eyefinder::process_image_eye(%d) -> invalid width %f\n",i,w);
		return;
	}
	if (h<0){
		printf("> Eyefinder::process_image_eye(%d) -> invalid heigth %f\n",i,h);
		return;
	}
//  	printf("xs %f xe %f  y %f, h %f w %f\n",xs,xe,y,h,w);
	
	((*image)(Rect(xs,y,h,w))).copyTo(eye_roi[i]); //extract ROI
	
	
	
	
// 	cvtColor(eye_roi, eye_roi, CV_BGR2GRAY);

if (cfg->eyedetection_by_hough_circles){
	find_iris(&eye_roi[i], i);
	store_eye_position(i, xs + last_detected_iris_pos[i].x, y +  last_detected_iris_pos[i].y);
	eye_pos_raw[i].x = last_detected_iris_pos[i].x;
	eye_pos_raw[i].y = last_detected_iris_pos[i].y;
}else{
	GaussianBlur(eye_roi[i], eye_roi[i], Size(7, 7), 1.2, 1.2);
	
	//process
	blobfinder[i].process_image(&eye_roi[i]);
	
	blob_vector_t blobs = blobfinder[i].get_blob_vector(1);
	if (blobs.size() > 0){
		//use best blob
		sort(blobs.begin(), blobs.end(), FastBlob::compare_circle_quality);
		//store eye position
		store_eye_position(i, xs + blobs[0].get_centeroid_x(), y +  blobs[0].get_centeroid_y());
		eye_pos_raw[i].x = blobs[0].get_centeroid_x();
		eye_pos_raw[i].y = blobs[0].get_centeroid_y();
	}
}
}
// void EyeFinder::store_calib_point(){
// 	eye_calib[0]->store_calib_point();
// 	eye_calib[1]->store_calib_point();
// }

void EyeFinder::find_iris(Mat *image, int eye_id){
	
	int key = 0;

// 	namedWindow("circles", 1);
// 	createTrackbar("hi", "circles", &hi, 255);
// 	createTrackbar("lo", "circles", &lo, 255);
// 	createTrackbar("min", "circles", &hmin, 255);
// 	createTrackbar("max", "circles", &hmax, 255);

	hi = 20;
	lo = 30;
// 	if (hmin == 0) hmin = 20;
// 	if (hmax == 0) hmax = 30; 
	
	hmin = cfg->eye_radius_px.x/22.5*13.0 * 0.9;
	hmax = cfg->eye_radius_px.x/22.5*13.0 * 1.1;
	
	
	// update display and snooker, so we can play with them
	Mat display = image->clone();
	
	
	//get grayscale image
	cvtColor(*image, eye_roi_hough[eye_id], CV_RGB2GRAY);
	threshold(eye_roi_hough[eye_id], eye_roi_hough[eye_id], cfg->eyefinder_brightness_threshold, 255.0, THRESH_TOZERO_INV);
	
	vector<Vec3f> circles;

	double min_dist = cfg->eye_radius_px.x/22.5*13.0 *0.3;
	
	// also preventing crash with hi, lo threshold here...
 	HoughCircles(eye_roi_hough[eye_id], circles, CV_HOUGH_GRADIENT, 2, min_dist, hi > 0 ? hi : 1, lo > 0 ? lo : 1, hmin, hmax );
	
	
	//find the one closest to the old iris pos:
	int best_pos = 0;
	double best_dist = 9999.0;
	
// 	printf("DIST : ");
	for( size_t i = 0; i < circles.size(); i++ ){
		Point center(cvRound(circles[i][0]), cvRound(circles[i][1]));
		double dist = (last_detected_iris_pos[eye_id].x - center.x)*(last_detected_iris_pos[eye_id].x - center.x) + (last_detected_iris_pos[eye_id].y - center.y)*(last_detected_iris_pos[eye_id].y - center.y);
// 		printf(" %4.2f, ",dist);
		if (dist < best_dist){
			best_dist = dist;
			best_pos = i;
		}
	}
// 	printf("\n");
	
	if (circles.size()>0) last_detected_iris_pos[eye_id] = Point2d(circles[best_pos][0],circles[best_pos][1]);
	
	for( size_t i = 0; i < circles.size(); i++ ){
		Point center(cvRound(circles[i][0]), cvRound(circles[i][1]));
		int radius = cvRound(circles[i][2]);
	
		// draw the green circle center
		circle( eye_roi_hough[eye_id], center, 1, Scalar(255,255,0), -1, 8, 0 );
		
		//get center color
// 		const cv::Vec3b& bgr = image->at<cv::Vec3b>(center.x,center.y);
// 		double color = bgr[0] + bgr[1] + bgr[2];
		
		///if (color < cfg->eyefinder_brightness_threshold2){
		if (best_pos==i){
			// draw the blue circle outline
			circle( eye_roi_hough[eye_id], center, radius, Scalar(255,0,0), 1, 8, 0 );
// 			printf("> iris radius = %d -> set eye radius to %4.2f\n",radius,radius*22.5/13.0);
		}
	}

// 	imshow( "circles", display );
// 	imshow("snooke/*r*/", snooker);

	return;
	
	
	uchar *imagedata_ptr;
	uchar *imagedata_out_ptr;
	int rgb,r,g,b,x,y;
	
	Mat image_gray; //(image->size(), CV_8UC1);
	Mat image_sobel(*image);
	Mat image_sobel16;
	
	//copy to grayscale image
	#if 0
	cvtColor(*image, image_gray, CV_RGB2GRAY);
	#else
	#if 1
	vector <Mat> hsv_vector;
	Mat hsv;
	
	//convert to hsv
	cvtColor(*image, hsv, CV_BGR2HSV);
	//split to single channels
	split(hsv, hsv_vector);
	//equalize V channel
// 	image_gray = hsv_vector[2];
	cvtColor(*image, image_gray, CV_BGR2GRAY);
	#else
	vector <Mat> rgb_vector;
	
	//split to single channels
	split(*image, rgb_vector);
	
	image_gray = rgb_vector[2];
	
	//filter out colors
	for(y=0; y<image->size().height-1; y++){
		imagedata_ptr = image->ptr(y);
		imagedata_out_ptr = image_gray.ptr(y);
		
		for(x=0; x<image->size().width*3; x+=3){
			rgb = (*((int*)(imagedata_ptr + x))) & 0xFFFFFF;
			int r = rgb>>16;
			int g = (rgb>>8) & 0xFF;
			int b = (rgb) & 0xFF;
			
			double factor = (double)r / ((double)g+b) / 2.0;
			
			factor = 1.0 - factor;
			int val = factor * 200;
			if (val > 255) val = 255;
			(*((int*)(imagedata_out_ptr+x/3))) = (val<<16) | (val<<8) | val;
			if (val < threshold_value){
				(*((int*)(imagedata_out_ptr+x/3))) = 0x000000;
			}
			if ((color_lookup[rgb] == 2) || (color_lookup[rgb] == 3)){
				//skin color or color -> erase
//  				(*((int*)(imagedata_out_ptr+x/3))) = 0x000000;
			}
		}
	}
	#endif
	#endif
	
	
	//image->copyTo(image_sobel);
	
	
	Sobel(image_gray, image_sobel16, CV_16S, 1, 0, 3, 2.0); //mat, mat, int ddepth, int xorder, int yorder, int ksize=3, double scale=1, double delta=0,
	imshow("sobel_in", image_gray);
	convertScaleAbs( image_sobel16, image_sobel, 3 );
	
	long sum_v = 0;
	long sum_c = 0;
	
	for(y=0; y<image_sobel.size().height-2; y++){
		imagedata_ptr = image_sobel.ptr(y);
		for(x=0; x<image_sobel.size().width; x++){
			unsigned char v = (*((unsigned char*)(imagedata_ptr + x))) & 0xFF;
			if (v > 20){
				sum_v += v;
				sum_c++;
			}
		}
	}
	
	//what is the avg val?
	int cutoff = 1.5 * sum_v / sum_c;
	printf(">>> %d\n",cutoff);
	for(y=0; y<image_sobel.size().height-2; y++){
		imagedata_ptr = image_sobel.ptr(y);
		
		for(x=0; x<image_sobel.size().width; x++){
			unsigned char v = (*((unsigned char*)(imagedata_ptr + x))) & 0xFF;
			
			if (v > cutoff){
// 				*((unsigned char*)(imagedata_ptr+x)) = 200;
			}else{
// 				*((unsigned char*)(imagedata_ptr+x)) = 0;
			}
		}
	}
	imshow("sobel", image_sobel);
	
	/*
	
	for(int y = 0; y < image_sobel.size().height-1; y++ ){
		imagedata_ptr = image_sobel.ptr(y);
		for( int x = 0; x < image_sobel.size().width*3; x+=3){
			int v = (*((int*)(imagedata_ptr+x)));
			if (v>30) {
 				printf("> %d\n",v);
				(*((int*)(imagedata_ptr+x))) = 0xFF;
			}
		}
	}
	imshow("AA",image_sobel);*/
	
	//find pixels
#if 0
	vector<vector<Point> > contours;

	findContours(image_sobel, contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
	Mat cimage = Mat::zeros(image_sobel.size(), CV_8UC3);
	
	for(size_t i = 0; i < contours.size(); i++){
		size_t count = contours[i].size();
		if( count < 6 )
			continue;

		Mat pointsf;
		Mat(contours[i]).convertTo(pointsf, CV_32F);
		RotatedRect box = fitEllipse(pointsf);
	
		if( MAX(box.size.width, box.size.height) > MIN(box.size.width, box.size.height)*30 )
			continue;
		drawContours(cimage, contours, (int)i, Scalar::all(255), 1, 8);

		ellipse(cimage, box, Scalar(0,0,255), 1, CV_AA);
		ellipse(cimage, box.center, box.size*0.5f, box.angle, 0, 360, Scalar(0,255,255), 1, CV_AA);
		Point2f vtx[4];
		box.points(vtx);
		for( int j = 0; j < 4; j++ )
			line(cimage, vtx[j], vtx[(j+1)%4], Scalar(0,255,0), 1, CV_AA);
	}

imshow("result", cimage);
	
	for(y=0; y<image->size().height-1; y++){
		imagedata_ptr = image->ptr(y);
		imagedata_out_ptr = cimage.ptr(y);
		
		for(x=0; x<image->size().width*3; x+=3){
			if ((*((int*)(imagedata_out_ptr+x))) != 0){
				//skin color or color -> erase
				(*((int*)(imagedata_ptr+x))) = (*((int*)(imagedata_out_ptr+x)));
			}
		}
	}

imshow("rrr", *image);
#endif	
// 	std::vector<Point> points;
// 	for(y=0; y<image_sobel.size().height-1; y++){
// 		imagedata_ptr = image_sobel.ptr(y);
// 		
// 		for(x=0; x<image_sobel.size().width*3; x+=3){
// 			int val = (*((int*)(imagedata_out_ptr+x))) & 0xFFFFFF;
// 			if (val > 40){
// 				points.push_back(Point(x,y));
//  				(*((int*)(imagedata_ptr+x)))  = 0;
// 			}
// 		}
// 	}
// 	printf("> S %d\n",points.size());
// 	if (points.size() > 6){
// 		Mat pointsf;
// 		Mat(points).convertTo(pointsf, CV_32F);
// 		RotatedRect box = fitEllipse(pointsf);
// 		ellipse(*image, box, Scalar(0,0,255), 1, CV_AA);
// 	
// 		imshow("rrr", *image);
// 	}	

}

void EyeFinder::remove_translation_and_rotation(Mat rt, imu_data_t head_euler){
	double x,y,xn,yn;
	
	//convert matrix:
	double m[2][3];
	for(int x=0; x<2; x++){
		for(int y=0; y<3; y++){
			m[x][y] = rt.at<double>(x,y);
		}
	}
	
	//update all markers
	for(int i=0; i<2; i++){
		x = eye_pos[i].x;
		y = eye_pos[i].y;
		xn = m[0][0]*x + m[0][1]*y + m[0][2];
		yn = m[1][0]*x + m[1][1]*y + m[1][2];
		eye_pos_fixated[i].x = xn;
		eye_pos_fixated[i].y = yn;
		
		//we need to transorm the eyes straight position as well:
		x = eye_pos_straight[i].x;
		y = eye_pos_straight[i].y;
		xn = m[0][0]*x + m[0][1]*y + m[0][2];
		yn = m[1][0]*x + m[1][1]*y + m[1][2];
		eye_pos_straight_fixated[i].x = xn;
		eye_pos_straight_fixated[i].y = yn;
	}
	
	
}


void EyeFinder::show_debug_image(){
	//for debugging_ overlay blobs on image
	for(int i=0; i<2; i++){
		if((eye_roi[i].size().height == 0) || (eye_roi[i].size().width == 0)){
			//not yet initialised
			return;
		}
		overlay_blobs(&eye_roi[i]);
		blobfinder[i].overlay_blobs_on_image(&eye_roi[i], 1);
		circle(eye_roi[i], eye_pos_raw[i], 20, Scalar(255,255,255), 3, 8, 0 );
	}
	
	
	cvNamedWindow("EYE LEFT", CV_WINDOW_KEEPRATIO);
	cvNamedWindow("EYE RIGHT", CV_WINDOW_KEEPRATIO);
	imshow("EYE LEFT",eye_roi[0]);
	imshow("EYE RIGHT",eye_roi[1]);
	
	if (cfg->eyedetection_by_hough_circles){
		imshow("EYE LEFT HOUGH",eye_roi_hough[0]);
		imshow("EYE RIGHT HOUGH",eye_roi_hough[1]);
	}
}


void EyeFinder::overlay_blobs_on_image(Mat *image){
	circle(*image, eye_pos[0], 19, CV_RGB(255/3,0,0));
	circle(*image, eye_pos[1], 19, CV_RGB(255/3,0,0));
	circle(*image, eye_pos_fixated[0], 19, CV_RGB(255,0,0));
	circle(*image, eye_pos_fixated[1], 19, CV_RGB(255,0,0));
	
	circle(*image, eye_pos_fixated[0], 5, CV_RGB(255,255,255), -1);
	circle(*image, eye_pos_fixated[1], 5, CV_RGB(255,255,255), -1);
	
	//overlay calib data:
	eye_calib[0]->overlay_blobs(image);
	eye_calib[1]->overlay_blobs(image);
	
	//put angles
	Point2d ea0 = get_eye_angle(0);
	Point2d ea1 = get_eye_angle(1);
	char buf[256];
	snprintf(buf, 256, "[%3.1f,%3.1f]", ea0.x, ea0.y);
	putText(*image, buf, eye_pos_fixated[0] + Point2d(0,80), FONT_HERSHEY_SIMPLEX, .5, CV_RGB(155,155,155), 1, CV_AA);
	snprintf(buf, 256, "[%3.1f,%3.1f]", ea1.x, ea1.y);
	putText(*image, buf, eye_pos_fixated[1] + Point2d(0,80), FONT_HERSHEY_SIMPLEX, .5, CV_RGB(155,155,155), 1, CV_AA);
	
	
// 	circle(*image, eye_pos_straight[0], 20, CV_RGB(0,155,0), -1, CV_AA);
// 	circle(*image, eye_pos_straight[1], 20, CV_RGB(0,155,0), -1, CV_AA);
	
// 	circle(*image, eye_pos_straight_fixated[0], 20, CV_RGB(0,255,0), -1, CV_AA);
// 	circle(*image, eye_pos_straight_fixated[1], 20, CV_RGB(0,255,0), -1, CV_AA);
	
	
}

void EyeFinder::overlay_text_on_image(Mat *image){
	putText(*image, " LEFT",eye_pos_fixated[0], FONT_HERSHEY_SIMPLEX, .5, CV_RGB(155,155,155), 1, CV_AA);
	putText(*image, " RIGHT",eye_pos_fixated[1], FONT_HERSHEY_SIMPLEX, .5, CV_RGB(155,155,155), 1, CV_AA);
}