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

EyeCalib::EyeCalib(ConfigOptions *c, int id){
	cfg = c;
	eye_id = id;
	if (eye_id == 0){
		eye_name = "LEFT";
	}else{
		eye_name = "RIGHT";
	}
	
	//clear
	for(int i=0; i<POS_HISTORY_SIZE; i++){
		pos_history[i] = Point2d(0,0);
	}
	pos_history_ptr = 0;
	pos_history_avg = Point2d(0,0);
	
	/*
	calc_angle(cfg->eyecalib_positions[eye_id][CALIB_POS_LOWER_LEFT]);
	calc_angle(cfg->eyecalib_positions[eye_id][CALIB_POS_LOWER_RIGHT]);
	calc_angle(cfg->eyecalib_positions[eye_id][CALIB_POS_UPPER_LEFT]);
	calc_angle(cfg->eyecalib_positions[eye_id][CALIB_POS_STRAIGHT] + Point2d(30.0, 40.0));
	exit(1);*/
	
}


void EyeCalib::init_calib(){
	calib_index = 0;
	///eye_position_straight = eye_position_history_average;
	
	//set eye straight offset
// 	cfg->eye_pos_straight_offset[eye_id] = eye_position_history_average - ;
// // 	eye_pos_straight_offset
	
	
	/*
	calibration_index = 0;
	
	cfg->eyecalib_positions[eye_id][CALIB_POS_STRAIGHT] = pos_history_avg;
	cfg->eyecalib_positions[eye_id][CALIB_POS_UPPER_LEFT] = cfg->eyecalib_positions[eye_id][CALIB_POS_STRAIGHT] + Point2d(30.0, 40.0);
	cfg->eyecalib_positions[eye_id][CALIB_POS_UPPER_RIGHT] = cfg->eyecalib_positions[eye_id][CALIB_POS_STRAIGHT] + Point2d(30.0, -40.0);
	cfg->eyecalib_positions[eye_id][CALIB_POS_LOWER_LEFT] = cfg->eyecalib_positions[eye_id][CALIB_POS_STRAIGHT] + Point2d(-30.0, 30.0);
	cfg->eyecalib_positions[eye_id][CALIB_POS_LOWER_RIGHT] = cfg->eyecalib_positions[eye_id][CALIB_POS_STRAIGHT] + Point2d(-30.0, -30.0);
	
	cfg->eyecalib_positions[eye_id][CALIB_POS_CAM] = cfg->eyecalib_positions[eye_id][CALIB_POS_STRAIGHT] + Point2d(-10.0, -10.0) ;
	*/
}

void EyeCalib::clear_history(){
	pos_history_avg = Point2d(0,0);
	for(int i=0; i<POS_HISTORY_SIZE; i++){
		pos_history[i] = Point2d(0,0);
	}
	pos_history_ptr_max = 0;
	pos_history_ptr = 0;
	
	eye_position_history_average_sum = Point2d(0.0, 0.0);
	eye_position_history_average     = Point2d(0.0, 0.0);
	eye_position_history_ptr = 0.0;
	for(int i=0; i<EYE_POSITION_HISTORY_SIZE; i++){
		eye_position_history[i] = Point2d(0,0);
	}
	eye_position_history_filled = false;
}

void EyeCalib::set_name(string n){
	eye_name = n;
	printf("> new eye calibrator '%s'\n",eye_name.c_str());
}

void EyeCalib::update_smoothed_eyeposition(Point2d p){
	eye_position_history_ptr++;
	if (eye_position_history_ptr >= EYE_POSITION_HISTORY_SIZE){
		eye_position_history_ptr = 0;
		eye_position_history_filled = true;
	}
	Point2d removed_entry = eye_position_history[eye_position_history_ptr];
	eye_position_history[eye_position_history_ptr] = p;
	
	//update sum:
	eye_position_history_average_sum -= removed_entry;
	eye_position_history_average_sum += p;
	eye_position_history_average = eye_position_history_average_sum * (1.0/EYE_POSITION_HISTORY_SIZE);
// 	printf("EEEEE %f %f %f %f\n",eye_position_history_average.x,eye_position_history_average.y,eye_position_straight.x,eye_position_straight.y);
}

#if 0
void EyeCalib::set_eyeposition(Point2d p, imu_data_t head_euler){
	//build up history
	update_smoothed_eyeposition(p);
	
	if (eye_position_history_filled){
		//update calibration dataset
		if (calib_index >= POS_HISTORY_SIZE){
			return;
		}
// 		if (eye_id == 0)  printf("PPP %f %f %f %f\n",eye_position_straight.x,eye_position_straight.y,p.x,p.y);
		Point2d dist = p-cfg->eye_pos_straight[eye_id]; ///eye_position_straight;
		
	
		calib_positions[calib_index].x = dist.x / tan(head_euler.x/180.0*M_PI); //(asin(head_euler.x/180.0*M_PI)); //-head_euler.x)/(sin((dist.x)/180.0*M_PI));
		calib_positions[calib_index].y = dist.y / tan(head_euler.z/180.0*M_PI); // (asin(head_euler.z/180.0*M_PI)); //(-head_euler.z)/(sin((dist.y)/180.0*M_PI));
		calib_index++;
		
		//recalc avg
		calib_factor = Point2d(0,0);
		for(int i=0; i<calib_index; i++){
			calib_factor += calib_positions[i];
		}
		calib_factor = calib_positions[calib_index-1]; // calib_factor * (1.0/calib_index);
		if (eye_id == 1) fprintf(stderr, "%d\n",calib_index);
		
// 		printf("%f %f %f %f NNN \n",eye_position_history_average.x-eye_position_straight.x,eye_position_history_average.y-eye_position_straight.y,head_euler.x,head_euler.z);
// 		if (eye_id == 1) printf("EULER %f %f \n",calib_positions[calib_index-1].x,calib_positions[calib_index-1].y);
// 		if (eye_id == 1) printf("%f %f %f %f %f %f %f %fTT\n",dist.x,dist.y,head_euler.x,head_euler.z,calib_positions[calib_index-1].x,calib_positions[calib_index-1].y, calib_factor.x,calib_factor.y);
	}
}
#endif

// double EyeCalib::resolve_radius(int index_r, int index_l, double calib_full_angle, int xory){
// 	//get distance in px
// 	Point2d dist = (cfg->eyecalib_positions[eye_id][index_l] - cfg->eyecalib_positions[eye_id][index_r]) * 0.5;
// 	
// // 	printf("distances %f %f\n",dist.x, dist.y);
// 	//estimate the eyes radius in px (xy swapped)
// 	double r;
// 	if (xory == 0){
// 		r = dist.y / (sin(M_PI*calib_full_angle/2.0/180.0));
// 	}else{
// 		r = dist.x / (sin(M_PI*calib_full_angle/2.0/180.0));
// 	}
// 	return r;
// }
/*
void EyeCalib::estimate_eye_radii(){
	double rxa[2], rya[2];
	
	//estimate eye radii in px:
	rxa[0] = resolve_radius(CALIB_POS_UPPER_RIGHT, CALIB_POS_UPPER_LEFT, cfg->calibration_angle_x, 0);
	rxa[1] = resolve_radius(CALIB_POS_LOWER_RIGHT, CALIB_POS_LOWER_LEFT, cfg->calibration_angle_x, 0);
	cfg->calibration_r_x = (rxa[0] + rxa[1])/2.0;
	//
	rya[0] = resolve_radius(CALIB_POS_LOWER_RIGHT, CALIB_POS_UPPER_RIGHT, cfg->calibration_angle_y, 1);
	rya[1] = resolve_radius(CALIB_POS_LOWER_LEFT, CALIB_POS_UPPER_LEFT, cfg->calibration_angle_y, 1);
	cfg->calibration_r_y = (rya[0] + rya[1])/2.0;
	//
	printf("> estimated eye radii as %4.2f,%4.2f\n",cfg->calibration_r_x,cfg->calibration_r_y);
	
	//FIXME: test this and then remove exit
// 	exit(1);
}*/

Point2d EyeCalib::calc_angle(Point2d dist){
	double angle_x, angle_y;
	
	//get distance between input & center:
	///Point2d dist = cfg->eye_pos_straight[eye_id] - input; ///eye_position_straight - input;
	
// 	printf("DIST %f %f\n",dist.x,dist.y);
	
	calib_factor = cfg->eye_radius_px;
	
	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);
#if 0	
	/// FIXME
	/// FIXME
	/// FIXME
	/// FIXME
	return dist;
	/// FIXME
	/// FIXME
	/// FIXME
	/// FIXME
	
	
	//calc angle:
	if (dist.y > cfg->calibration_r_x ){
		printf("> WARN: eye[%s] x=%4.2f > rx=%4.2f -> limiting!",eye_name.c_str(),dist.y, cfg->calibration_r_x);
		dist.y = cfg->calibration_r_x ;
	}
	
	if (dist.x > cfg->calibration_r_y){
		printf("> WARN: eye[%s] y=%4.2f > ry=%4.2f -> limiting!",eye_name.c_str(),dist.x,cfg->calibration_r_y);
		dist.x = cfg->calibration_r_y;
	}
	
	angle_x = asin(dist.y / cfg->calibration_r_x) * 180.0 / M_PI;
	angle_y = -asin(dist.x / cfg->calibration_r_y) * 180.0 / M_PI;
	
// 	printf("> eye[%s] angle = %f,%f\n",eye_name.c_str(), angle_x,angle_y);
	
	return Point2d(angle_x, angle_y);
	#endif
}
#if 0
void EyeCalib::store_calib_point(){
	
	
	
	printf("> CCC %d\n",calibration_index);
	//check
	if ((calibration_index <0) || (calibration_index > CALIB_POS_CAM)){
		return;
	}
	
	if (calibration_index == CALIB_POS_STRAIGHT){
		//looking straight  -> store
		cfg->eyecalib_positions[eye_id][calibration_index] = pos_history_avg;
	}else if (calibration_index == CALIB_POS_CAM){
		//looking to cam -> store
		cfg->eyecalib_positions[eye_id][calibration_index] = pos_history_avg;
		//re estimate eye radii
		estimate_eye_radii();
	}else{
		//looking order might be out of order -> assign automatically:
		Point2d cam = cfg->eyecalib_positions[eye_id][CALIB_POS_STRAIGHT];
		Point2d avg = pos_history_avg;
		int index = 0;
		if (avg.x < cam.x){
			//lower
			if (avg.y > cam.y){
				//lower left
				index = CALIB_POS_LOWER_LEFT;
			}else{
				//lower right
				index = CALIB_POS_LOWER_RIGHT;
			}
		}else{
			//upper
			if (avg.y > cam.y){
				//upper left
				index = CALIB_POS_UPPER_LEFT;
			}else{
				//upper right
				index = CALIB_POS_UPPER_RIGHT;
			}
		}
		cfg->eyecalib_positions[eye_id][index] = avg;
	}
	calibration_index++;

}
#endif

void EyeCalib::overlay_blobs(Mat *image){
	//overlay calib info
// 	line(*image,  cfg->eyecalib_positions[eye_id][CALIB_POS_UPPER_LEFT], cfg->eyecalib_positions[eye_id][CALIB_POS_UPPER_RIGHT], CV_RGB(255,0,0), 1, CV_AA, 0);
// 	line(*image,  cfg->eyecalib_positions[eye_id][CALIB_POS_UPPER_RIGHT], cfg->eyecalib_positions[eye_id][CALIB_POS_LOWER_RIGHT], CV_RGB(255,0,0), 1, CV_AA, 0);
// 	line(*image,  cfg->eyecalib_positions[eye_id][CALIB_POS_LOWER_RIGHT], cfg->eyecalib_positions[eye_id][CALIB_POS_LOWER_LEFT], CV_RGB(255,0,0), 1, CV_AA, 0);
// 	line(*image,  cfg->eyecalib_positions[eye_id][CALIB_POS_LOWER_LEFT], cfg->eyecalib_positions[eye_id][CALIB_POS_UPPER_LEFT], CV_RGB(255,0,0), 1, CV_AA, 0);
	
	//crosshatch for cam position:
// 	line(*image, Point(cfg->eyecalib_positions[eye_id][CALIB_POS_CAM].x - 10,cfg->eyecalib_positions[eye_id][CALIB_POS_CAM].y), Point(cfg->eyecalib_positions[eye_id][CALIB_POS_CAM].x + 10,cfg->eyecalib_positions[eye_id][CALIB_POS_CAM].y), CV_RGB(0,255,0), 1);
// 	line(*image, Point(cfg->eyecalib_positions[eye_id][CALIB_POS_CAM].x,cfg->eyecalib_positions[eye_id][CALIB_POS_CAM].y-10), Point(cfg->eyecalib_positions[eye_id][CALIB_POS_CAM].x,cfg->eyecalib_positions[eye_id][CALIB_POS_CAM].y + 10), CV_RGB(0,255,0), 1);
	
// 	putText(*image, eye_name,  cfg->eyecalib_positions[eye_id][CALIB_POS_STRAIGHT], FONT_HERSHEY_SIMPLEX, .5, CV_RGB(155,155,155), 1, CV_AA);
	
// 	circle(*image, cfg->eyecalib_positions[eye_id][CALIB_POS_STRAIGHT], 3, CV_RGB(255,0,0), -1);
}