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

UnitConverter::UnitConverter(){
	//FIXME: get this values dynamically!
	conversion_factor_pxx2cm = 0.50 / 25.0; //measured_nose_length_cm / measured_nose_length_px
	conversion_factor_pxy2cm = 3.3 / 109; //measured eye width cm / measured eye withd px
	
	//constants
	const_eyeball_radius = 24.0/2.0; //adult human is 23-24mm
	
	//init
	zero_uninitialized = true;
}

UnitConverter::~UnitConverter(){
}

void UnitConverter::process_data(MarkerModel *m, EyeFinder *e, int frame){
	//FIXME:
	return; 
	
	//process incoming data & calculate angles
	if (zero_uninitialized){
		printf("> eye zero not set -> can not calculate angles\n");
		return;
	}
	
	//set eye positions:
	convert_eye_angles(e->get_eye_pos_unfixated(0), e->get_eye_pos_unfixated(1));
}

//convert eye angles
void UnitConverter::convert_eye_angles(Point2d left, Point2d right){
	//y angle
	double angle_y_both = -(convert_px_to_cm_x(left.x - eye_zero_position[0].x) + convert_px_to_cm_x(right.x - eye_zero_position[1].x))/2.0;
	translated_angle_eye[0].y = angle_y_both;
	translated_angle_eye[1].y = angle_y_both;
	//x angle
	translated_angle_eye[0].x = convert_px_to_cm_y(left.y - eye_zero_position[0].y);
	translated_angle_eye[1].x = convert_px_to_cm_y(right.y - eye_zero_position[1].y);
	printf("> eye angles = left[%4.2f,%4.2f] right[%4.2f,%4.2f]\n",translated_angle_eye[0].x,translated_angle_eye[0].y,translated_angle_eye[1].x,translated_angle_eye[1].y);
}

//we got the eye zero position
void UnitConverter::set_eye_zero(Point2d left, Point2d right){
	eye_zero_position[0] = left;
	eye_zero_position[1] = right;
	zero_uninitialized = false;
	printf("> eye zeroset\n");
}



double UnitConverter::convert_px_to_cm_x(double pixel){
	return ((pixel)*conversion_factor_pxx2cm);
}

double UnitConverter::convert_px_to_cm_y(double pixel){
	return ((pixel)*conversion_factor_pxy2cm);
}

double UnitConverter::calc_deg_from_cm(double radius, double cm){
	if (abs(cm) > abs(radius)){
		printf("> WARNING: %s(): measured %4.2fcm which is > r=%4.2fcm -> limited\n",__func__,cm,radius);
		cm = (cm<0.0)?-radius:radius;
	}
	return (asin((cm) / (radius)) * 180.0 / M_PI);
}

double UnitConverter::calc_eye_deg_from_px_x(double pixel){
	return calc_deg_from_cm(const_eyeball_radius, convert_px_to_cm_x(pixel));
}

double UnitConverter::calc_eye_deg_from_px_y(double pixel){
	return calc_deg_from_cm(const_eyeball_radius, convert_px_to_cm_y(pixel));
}