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

OutputVisualizer::OutputVisualizer(ConfigOptions *c, MarkerModel *m, EyeFinder *e, HeadTracker *h){
	cfg = c;
	marker_model = m;
	eye_finder = e;
	head_tracker = h;
}


void OutputVisualizer::render_angle(Mat *image, Point2d center, double angle, int r, int g, int b){
	double radius = 30;
	
	circle(*image, center, radius, CV_RGB(155,155,155), 1, CV_AA);
	Point2d target = center;
	target.x += (radius-2)*cos((angle-90.0)*M_PI/180.0);
	target.y += (radius-2)*sin((angle-90.0)*M_PI/180.0);
		
	line(*image, center+Point2d(0,-25), center+Point2d(0,-35), CV_RGB(255,255,255), 1, CV_AA, 0);
	
	line(*image, center, target, CV_RGB(r,g,b), 1, CV_AA, 0);
	
	char buf[256];
	snprintf(buf, 256, "%3.1f", angle);
	putText(*image, buf, center + Point2d(-15,45), FONT_HERSHEY_SIMPLEX, .5, CV_RGB(255,255, 255), 1, CV_AA);
}

void OutputVisualizer::render_eyelid(Mat *image, int e, double angle){
	Point2d eye_pos       = eye_finder->get_eye_pos_straight_fixated(e);
	//HACK: eyelids are a bit larger than the eye
	Point2d eyelid_radius = cfg->eye_radius_px * cfg->eyelid_scale_factor;
	
	if (angle > 90.0){
		angle = 90.0;
	}else if (angle < -90.0){
		angle = -90.0;
	}
	
	double radius_x = eyelid_radius.x * sin(angle/180.0*M_PI);
	
	if (isnan(radius_x)){
		radius_x = 0.0;
	}
	//printf(" @ %f %f - %f %f\n",eye_pos.x,eye_pos.y,radius_x, eyelid_radius.y);
	if (radius_x == 0.0){
		//straight line
		line(*image, eye_pos-Point2d(0.0, eyelid_radius.y), eye_pos-Point2d(0.0, eyelid_radius.y), CV_RGB(0,255,0), 1, CV_AA, 0);
	}if (radius_x < 0.0){
		
		ellipse(*image, eye_pos, Size(-radius_x, eyelid_radius.y), 0.0, 90.0, 270.0,  CV_RGB(0,255,0), 1, CV_AA);
	}else{
		ellipse(*image, eye_pos, Size(radius_x, eyelid_radius.y), 0.0, -90.0, 90.0,  CV_RGB(0,255,0), 1, CV_AA);
	}
}

void OutputVisualizer::render_eye(Mat *image, int e){
	Point2d eye_angle     = eye_finder->get_eye_angle(e);
	Point2d eye_pupil_pos = eye_finder->get_eye_pos_fixated(e);
	Point2d eye_pos       = eye_finder->get_eye_pos_straight_fixated(e);
	//cfg->eye_pos_straight_offset[e];
	
	ellipse(*image, eye_pos, Size(cfg->eye_radius_px.x,cfg->eye_radius_px.y), 0.0, 0.0, 360.0,  CV_RGB(200,200,0), 1, CV_AA);
	
	//calculate pupil position
	Point2d pupil_pos_calculated;
	pupil_pos_calculated.x = cfg->eye_radius_px.x * sin(eye_angle.x/180.0*M_PI);
	pupil_pos_calculated.y = -cfg->eye_radius_px.y * sin(eye_angle.y/180.0*M_PI);
// 	printf("%f %f TTT\n",pupil_pos_calculated.x,pupil_pos_calculated.y);

	double iris_size = cfg->eye_radius_px.x/22.5*13.0; //eye diameter = 44mm / iris diameter = 12-14mm
	circle(*image, eye_pos+pupil_pos_calculated, iris_size, CV_RGB(0,255,255), 1, CV_AA);
	//circle(*image, eye_pupil_pos, 10, CV_RGB(200,0,200));
	
	//paint eyelids:
	render_eyelid(image, e, eye_finder->get_eyelid_angle_upper(e));
	render_eyelid(image, e, eye_finder->get_eyelid_angle_lower(e));
	
	
}

void OutputVisualizer::render_mouth_single(Mat *image, int index){
	char buf[256];
	snprintf(buf, 256, "%3.1f", marker_model->get_mouth_displacement_x(index));
	putText(*image, buf, marker_model->get_mouth_marker_vector()[index], FONT_HERSHEY_SIMPLEX, .5, CV_RGB(255,255, 255), 1, CV_AA);
}

void OutputVisualizer::render_mouth(Mat *image){
	for(int i=0; i<6; i++){
		render_mouth_single(image, i);
	}
}


void OutputVisualizer::overlay_blobs_on_image(Mat *image){
	//show euler angles
	imu_data_t euler = head_tracker->get_euler_angles();
	render_angle(image, Point2d(100,410), euler.x, 255,   0,   0);
	render_angle(image, Point2d(200,410), euler.y,   0, 255,   0);
	render_angle(image, Point2d(300,410), euler.z,   0,   0, 255);
	
	//render eyes
	render_eye(image, 0);
	render_eye(image, 1);
	
	render_mouth(image);
	
}