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

DataOutput::DataOutput(ConfigOptions *c){
	cfg = c;
	output_active = false;
}

// DataOutput::DataOutput(){
// 	cfg = NULL;
// }

DataOutput::~DataOutput(){
}


void DataOutput::process_data(MarkerModel *m, EyeFinder *e, HeadTracker *h, int frame_number){
	headtracker = h;
	
	imu_data_t head = headtracker->get_euler_angles();
	if (cfg->output_lcm_mirror_mode){
		//mirror l/r
		head.y = -head.y;
		head.z = -head.z;
	}
	
	target_angle[NECK_PAN]  = (-head.z) * cfg->orientation_scale_factor.x;
	target_angle[NECK_TILT] = (-head.x) * cfg->orientation_scale_factor.y;
	target_angle[NECK_ROLL] = (-head.y) * 1.0;
	
	//eye angles
	Point2d eye_angle_l = e->get_eye_angle(0);
	Point2d eye_angle_r = e->get_eye_angle(1);
	double ud = (eye_angle_l.x + eye_angle_r.x)/2.0;
	
	if (cfg->output_lcm_mirror_mode){
		//mirror l/r
		double tmp = eye_angle_l.y;
		eye_angle_l.y = -eye_angle_r.y;
		eye_angle_r.y = -tmp;
	}
	
	if (cfg->fixation_2d_correction){
		//user is looking on the mocap screen -> both eyes fixate point in 2D 
		//remove vergence and look straight!
		double angle_both = (eye_angle_l.y+eye_angle_r.y)/2.0;
		eye_angle_l.y = angle_both;
		eye_angle_r.y = angle_both;
	}
	
	target_angle[EYES_TILT]     = ud;
	target_angle[EYE_LEFT_PAN]  = eye_angle_l.y;
	target_angle[EYE_RIGHT_PAN] = eye_angle_r.y;
		
	//eyelids
	int pos_left = 0;
	int pos_right = 1;
	if (cfg->output_lcm_mirror_mode){
		//mirror l/r
		pos_left = 1;
		pos_right = 0;
	}
	
	target_angle[EYE_LEFT_LID_LOWER]  =  e->get_eyelid_angle_lower(pos_left);
	target_angle[EYE_LEFT_LID_UPPER]  =  e->get_eyelid_angle_upper(pos_left);
	target_angle[EYE_RIGHT_LID_LOWER] =  e->get_eyelid_angle_lower(pos_right);
	target_angle[EYE_RIGHT_LID_UPPER] =  e->get_eyelid_angle_upper(pos_right);
	#if 0
	//check if eyelids are too close:
	double eyelid_distance[2];
	
	eyelid_distance[0] = target_angle[EYE_LEFT_LID_UPPER] - target_angle[EYE_LEFT_LID_LOWER];
	eyelid_distance[1] = target_angle[EYE_RIGHT_LID_UPPER] - target_angle[EYE_RIGHT_LID_LOWER];
	
	if ((eyelid_distance[0] > 25.0) && (eyelid_distance[1] > 25.0)){
		//safe
	}else{
		//keep last values
		target_angle[EYE_LEFT_PAN] = eye_last_pos[0];
		target_angle[EYE_RIGHT_PAN] = eye_last_pos[1];
		target_angle[EYES_TILT] = eye_last_pos[2];
	}
	
	eye_last_pos[0] = target_angle[EYE_LEFT_PAN];
	eye_last_pos[1] = target_angle[EYE_RIGHT_PAN];
	eye_last_pos[2] = target_angle[EYES_TILT];
	#endif
		
	//brows
	if (cfg->output_lcm_mirror_mode){
		//mirror l/r
	}
	
	//mouth
	target_angle[MOUTH_LEFT_UPPER]   = m->get_mouth_displacement_x(MarkerModel::MOUTH_LEFT_UPPER);
	target_angle[MOUTH_LEFT_LOWER]   = m->get_mouth_displacement_x(MarkerModel::MOUTH_LEFT_LOWER);
	
	target_angle[MOUTH_CENTER_UPPER] = m->get_mouth_displacement_x(MarkerModel::MOUTH_CENTER_UPPER);
	target_angle[MOUTH_CENTER_LOWER] = m->get_mouth_displacement_x(MarkerModel::MOUTH_CENTER_LOWER);
	
	///add safety checks for LOWER+UPPER!
	if ((target_angle[MOUTH_CENTER_LOWER] - target_angle[MOUTH_CENTER_UPPER]) <= 8.0){
		target_angle[MOUTH_CENTER_LOWER] = target_angle[MOUTH_CENTER_UPPER] + 8.0;
	}
	
	
	target_angle[MOUTH_RIGHT_UPPER]  = m->get_mouth_displacement_x(MarkerModel::MOUTH_RIGHT_UPPER);
	target_angle[MOUTH_RIGHT_LOWER]  = m->get_mouth_displacement_x(MarkerModel::MOUTH_RIGHT_LOWER);
	
	///FIXME: add this again (copy calc from old source mocap_to_csv)
	target_angle[EYE_LEFT_BROW]  = 0.0;
	target_angle[EYE_RIGHT_BROW] = 0.0;
	
	if (output_active){
		process_new_data(frame_number);
	}
}