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

// #define MOUTH_SEARCH_MIN 640-250
// #define MOUTH_Y_DIST_MIN 40

#define MOUTH_SEARCH_MIN_X (cfg->mouth_search_split_position)
#define EYES_SEARCH_MAX_X MOUTH_SEARCH_MIN_X

#define DEBUG_MARKERMODEL_FILTER 0
#define RENDER_UNROTATED 0

MarkerModel::MarkerModel(ConfigOptions *c){
	cfg = c;
	boundingbox_size_min = 40;
	boundingbox_size_max = 800;
	pixelcount_min = 50;
	model_uninitialised = true;
	initialise_face_model();
	frame_number = 0;
}
/*

blob_vector_t MarkerModel::get_candidates(){
	return candidates;
}
*/


//call this to reinitialise the marker assignment
void MarkerModel::initialise_face_model(){
	model_uninitialised = true;
	
	#if 1
	face_mouth_marker_vector.clear();
	face_eye_marker_vector.clear();
	
	face_mouth_marker_vector.push_back( Marker(640-364-40,350,20,20) ); //MOUTH LEFT UPPER
	face_mouth_marker_vector.push_back( Marker(640-421-40,338,20,20) ); //MOUTH LEFT LOWER
	face_mouth_marker_vector.push_back( Marker(640-351-40,251,20,20) ); //MOUTH CENTER UPPER
	face_mouth_marker_vector.push_back( Marker(640-388-40,252,20,20) ); //MOUTH CENTER LOWER
	face_mouth_marker_vector.push_back( Marker(640-386-40,159,20,20) ); //MOUTH RIGHT UPPER
	face_mouth_marker_vector.push_back( Marker(640-439-40,170,20,20) ); //MOUTH_RIGHT_LOWER
	
	//EYES
	face_eye_marker_vector.push_back( Marker(640-120,350,20,20) ); //EYE_LID_LEFT_UPPER
	face_eye_marker_vector.push_back( Marker(640-200,350,20,20) ); //EYE_LID_LEFT_LOWER
	face_eye_marker_vector.push_back( Marker(640-120,120,20,20) ); //EYE_LID_RIGHT_UPPER
	face_eye_marker_vector.push_back( Marker(640-200,120,20,20) ); //EYE_LID_RIGHT_LOWER
	
	face_eye_marker_vector[EYE_LID_LEFT_UPPER].set_kalman_coefficients(0.1, 0.1, 0.1);
	face_eye_marker_vector[EYE_LID_LEFT_LOWER].set_kalman_coefficients(0.1, 0.1, 0.1);
	face_eye_marker_vector[EYE_LID_RIGHT_UPPER].set_kalman_coefficients(0.1, 0.1, 0.1);
	face_eye_marker_vector[EYE_LID_RIGHT_LOWER].set_kalman_coefficients(0.1, 0.1, 0.1);
	
	
	
	//BROWS
	face_eye_marker_vector.push_back( Marker(640-70,260,20,20) ); //EYE_BROW_RIGHT_I
	face_eye_marker_vector.push_back( Marker(640-50,330,20,20) ); //EYE_BROW_RIGHT_O
	face_eye_marker_vector.push_back( Marker(640-70,190,20,20) ); //EYE_BROW_LEFT_I
	face_eye_marker_vector.push_back( Marker(640-50,100,20,20) ); //EYE_BROW_LEFT_O
	
	//NOSE
	face_eye_marker_vector.push_back( Marker(640-150,230,20,20) ); //EYE_NOSE_LOWER
	face_eye_marker_vector.push_back( Marker(640-100,230,20,20) ); //EYE_NOSE_UPPER
	//use different kallman settings for this:
	face_eye_marker_vector[EYE_NOSE_UPPER].set_kalman_coefficients(0.001, 0.1, 0.1);
	face_eye_marker_vector[EYE_NOSE_LOWER].set_kalman_coefficients(0.001, 0.1, 0.1);
	
	
	
	
	#else
	TESTCASE: happy swapping
	
	#endif
		
	model_uninitialised = false;
}

void MarkerModel::remove_translation_and_rotation(Mat rt){
	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(marker_vector_t::iterator it=face_eye_marker_vector.begin(); it!= face_eye_marker_vector.end(); it++){
		x = it->center_x;
		y = it->center_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];
		it->set_fixated_to(xn,yn);
	}
	for(marker_vector_t::iterator it=face_mouth_marker_vector.begin(); it!= face_mouth_marker_vector.end(); it++){
		x = it->center_x;
		y = it->center_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];
		it->set_fixated_to(xn,yn);
	}

	
	
	for(int i=0; i<MOUTH_ENUM_SIZE; i++){
// 		mouth_zero_pos[i] = cfg->mouth_center_pos[i] + Point2d(cfg->mouth_zero_distance, 0.0);
		
		mouth_center_pos_fixated[i].x = m[0][0]*cfg->mouth_center_pos[i].x + m[0][1]*cfg->mouth_center_pos[i].y + m[0][2];
		mouth_center_pos_fixated[i].y = m[1][0]*cfg->mouth_center_pos[i].x + m[1][1]*cfg->mouth_center_pos[i].y + m[1][2];
// 		mouth_zero_pos_fixated[i].x = m[0][0]*mouth_zero_pos[i].x + m[0][1]*mouth_zero_pos[i].y + m[0][2];
// 		mouth_zero_pos_fixated[i].y = m[1][0]*mouth_zero_pos[i].x + m[1][1]*mouth_zero_pos[i].y + m[1][2];
	}
}

void MarkerModel::render_line(Mat *image, marker_vector_t m, int index_a, int index_b, int r, int g, int b){
	Marker ma = m[index_a];
	Marker mb = m[index_b];
	#if RENDER_UNROTATED
	//render actual position in low density
	line(*image,  ma.to_point2d(), mb.to_point2d(), CV_RGB(r/3,g/3,b/3), 1, CV_AA, 0);
	#endif
	//render fixated position
	line(*image,  ma.to_fixated_point2d(), mb.to_fixated_point2d(), CV_RGB(r,g,b), 1, CV_AA, 0);
}

void MarkerModel::render_line_zero(Mat *image, marker_vector_t m, int index_a, int index_zero, int r, int g, int b){
	Marker ma = m[index_a];
	#if RENDER_UNROTATED
	//render actual position in low density
	line(*image,  ma.to_point2d(), cfg->mouth_center_pos[index_zero], CV_RGB(r/3,g/3,b/3), 1, CV_AA, 0);
	#endif
	//render fixated position
	line(*image,  ma.to_fixated_point2d(), mouth_center_pos_fixated[index_zero], CV_RGB(r,g,b), 1, CV_AA, 0);
}

void MarkerModel::render_circle(Mat *image, marker_vector_t m, int index_a, int r, int g, int b){
	Marker ma = m[index_a];
	#if RENDER_UNROTATED
	circle(*image, ma.to_point2d(), ma.width/2, CV_RGB(r/3,g/3,b/3), -1, CV_AA);
	#endif
	circle(*image, ma.to_fixated_point2d(), ma.width/2, CV_RGB(r,g,b), -1, CV_AA);
}

void MarkerModel::render_ellipse(Mat *image, marker_vector_t m, int index_a, int index_b, int r, int g, int b){
	Point2d center = (m[index_a].to_point2d() + m[index_b].to_point2d())*.5;
	Point2d fcenter = (m[index_a].to_fixated_point2d() + m[index_b].to_fixated_point2d())*.5;

	
	double dist_y = abs(m[index_a].center_x - m[index_b].center_x)/2.0;
	if (dist_y >= 1){
		#if RENDER_UNROTATED
		ellipse(*image, center, Size(dist_y,50),0.0, 30.0, 330.0, CV_RGB(r/3,g/3,b/3),1, CV_AA,0);
		#endif
		ellipse(*image, fcenter, Size(dist_y,50),0.0, 30.0, 330.0, CV_RGB(r,g,b),1, CV_AA,0);
	}
}


double MarkerModel::get_mouth_displacement_x(int enum_index){
	//calculate displacement of mouth marker
	//1) get center position:
	double center_pos_x = mouth_center_pos_fixated[enum_index/2].x; //works because enum is LL CC RR and zero is L C R
	
	//2) calculate distance in px:
	double dist_px_x = center_pos_x - face_mouth_marker_vector[enum_index].fixated_x;
	
	return dist_px_x * cfg->mouth_scale_factor + cfg->mouth_offset;
	
}

void MarkerModel::overlay_blobs_on_image(Mat *image){
	line(*image,  Point2d(MOUTH_SEARCH_MIN_X, 0.0), Point2d(MOUTH_SEARCH_MIN_X, image->size().width), CV_RGB(255,255,0), 1, CV_AA, 0);
	
	//mouth zero
// 	line(*image, cfg->mouth_zero_pos[MOUTH_LEFT], cfg->mouth_zero_pos[MOUTH_CENTER], CV_RGB(200/3,0,0), 1, CV_AA, 0);
// 	line(*image, cfg->mouth_zero_pos[MOUTH_CENTER], cfg->mouth_zero_pos[MOUTH_RIGHT], CV_RGB(200/3,0,0), 1, CV_AA, 0);
// 	circle(*image, cfg->mouth_zero_pos[MOUTH_LEFT], 5, CV_RGB(200/3,0,0), -1, CV_AA);
// 	circle(*image, cfg->mouth_zero_pos[MOUTH_CENTER], 5, CV_RGB(200/3,0,0), -1, CV_AA);
// 	circle(*image, cfg->mouth_zero_pos[MOUTH_RIGHT], 5, CV_RGB(200/3,0,0), -1, CV_AA);
	
	//fixated
// 	line(*image, mouth_zero_pos_fixated[MOUTH_LEFT], mouth_zero_pos_fixated[MOUTH_CENTER], CV_RGB(200,220,0), 1, CV_AA, 0);
// 	line(*image, mouth_zero_pos_fixated[MOUTH_CENTER], mouth_zero_pos_fixated[MOUTH_RIGHT], CV_RGB(200,220,0), 1, CV_AA, 0);
// 	circle(*image, mouth_zero_pos_fixated[MOUTH_LEFT], 5, CV_RGB(200,220,0), -1, CV_AA);
// 	circle(*image, mouth_zero_pos_fixated[MOUTH_CENTER], 5, CV_RGB(200,220,0), -1, CV_AA);
// 	circle(*image, mouth_zero_pos_fixated[MOUTH_RIGHT], 5, CV_RGB(200,220,0), -1, CV_AA);
	
	line(*image, mouth_center_pos_fixated[MOUTH_LEFT], mouth_center_pos_fixated[MOUTH_CENTER], CV_RGB(200,0,0), 1, CV_AA, 0);
	line(*image, mouth_center_pos_fixated[MOUTH_CENTER], mouth_center_pos_fixated[MOUTH_RIGHT], CV_RGB(200,0,0), 1, CV_AA, 0);
	circle(*image, mouth_center_pos_fixated[MOUTH_LEFT], 5, CV_RGB(200,0,0), -1, CV_AA);
	circle(*image, mouth_center_pos_fixated[MOUTH_CENTER], 5, CV_RGB(200,0,0), -1, CV_AA);
	circle(*image, mouth_center_pos_fixated[MOUTH_RIGHT], 5, CV_RGB(200,0,0), -1, CV_AA);
	
	if (model_uninitialised){
		return;
	}
	
	//show blobs
	#if RENDER_UNROTATED
	for(marker_vector_t::iterator it=mouth_input_vector.begin(); it!= mouth_input_vector.end(); it++){
		rectangle(*image, Point2d(it->center_x - it->width/2,it->center_y - it->height/2), Point2d(it->center_x+it->width/2,it->center_y+it->height/2), CV_RGB(255,255,0), 1);
	}
	for(marker_vector_t::iterator it=eyes_input_vector.begin(); it!= eyes_input_vector.end(); it++){
		rectangle(*image, Point2d(it->center_x - it->width/2,it->center_y - it->height/2), Point2d(it->center_x+it->width/2,it->center_y+it->height/2), CV_RGB(255,0,255), 1);
	}
	#endif
	
	//show detected features:
	//mouth upper
	render_line(image, face_mouth_marker_vector,  MOUTH_LEFT_UPPER, MOUTH_CENTER_UPPER, 255, 255, 255);
	render_line(image, face_mouth_marker_vector, MOUTH_RIGHT_UPPER, MOUTH_CENTER_UPPER, 255, 255, 255);
	render_circle(image, face_mouth_marker_vector, MOUTH_LEFT_UPPER, 255,255,255);
	render_circle(image, face_mouth_marker_vector, MOUTH_CENTER_UPPER, 255,255,255);
	render_circle(image, face_mouth_marker_vector, MOUTH_RIGHT_UPPER, 255,255,255);
	
	//mouth lower
	render_line(image, face_mouth_marker_vector, MOUTH_LEFT_LOWER, MOUTH_CENTER_LOWER, 255, 255, 255);
	render_line(image, face_mouth_marker_vector, MOUTH_RIGHT_LOWER, MOUTH_CENTER_LOWER, 255, 255, 255);
	render_circle(image, face_mouth_marker_vector, MOUTH_LEFT_LOWER, 255,255,255);
	render_circle(image, face_mouth_marker_vector, MOUTH_CENTER_LOWER, 255,255,255);
	render_circle(image, face_mouth_marker_vector, MOUTH_RIGHT_LOWER, 255,255,255);
	
	
	//mouth tracking:
	render_line_zero(image, face_mouth_marker_vector, MOUTH_LEFT_LOWER, MOUTH_LEFT, 255, 0, 0);
	render_line_zero(image, face_mouth_marker_vector, MOUTH_LEFT_UPPER, MOUTH_LEFT, 255, 0, 0);
	render_line_zero(image, face_mouth_marker_vector, MOUTH_CENTER_LOWER, MOUTH_CENTER, 255, 0, 0);
	render_line_zero(image, face_mouth_marker_vector, MOUTH_CENTER_UPPER, MOUTH_CENTER, 255, 0, 0);
	render_line_zero(image, face_mouth_marker_vector, MOUTH_RIGHT_LOWER, MOUTH_RIGHT, 255, 0, 0);
	render_line_zero(image, face_mouth_marker_vector, MOUTH_RIGHT_UPPER, MOUTH_RIGHT, 255, 0, 0);
	
	//eye left
	render_circle(image, face_eye_marker_vector, EYE_LID_LEFT_UPPER, 255,255,0);
	render_circle(image, face_eye_marker_vector, EYE_LID_LEFT_LOWER, 255,255,0);
	render_ellipse(image, face_eye_marker_vector, EYE_LID_LEFT_UPPER, EYE_LID_LEFT_LOWER, 255,255,0);
	
	//eye right
	render_circle(image, face_eye_marker_vector, EYE_LID_RIGHT_UPPER, 255,255,0);
	render_circle(image, face_eye_marker_vector, EYE_LID_RIGHT_LOWER, 255,255,0);
	render_ellipse(image, face_eye_marker_vector, EYE_LID_RIGHT_UPPER, EYE_LID_RIGHT_LOWER, 255,255,0);
	
	//BROW left
	render_line(image, face_eye_marker_vector, EYE_BROW_LEFT_I, EYE_BROW_LEFT_O, 0, 255, 0);
	render_circle(image, face_eye_marker_vector, EYE_BROW_LEFT_I, 0,155,0);
	render_circle(image, face_eye_marker_vector, EYE_BROW_LEFT_O, 0,255,0);
	
	//BROW right
	render_line(image, face_eye_marker_vector, EYE_BROW_RIGHT_I, EYE_BROW_RIGHT_O, 0, 255, 0);
	render_circle(image, face_eye_marker_vector, EYE_BROW_RIGHT_I, 0,155,0);
	render_circle(image, face_eye_marker_vector, EYE_BROW_RIGHT_O, 0,255,0);
	
	//NOSE
	render_line(image, face_eye_marker_vector, EYE_NOSE_UPPER, EYE_NOSE_LOWER, 255, 0, 255);
	render_circle(image, face_eye_marker_vector, EYE_NOSE_UPPER, 255,0,255);
	render_circle(image, face_eye_marker_vector, EYE_NOSE_LOWER, 155,0,155);
}



void MarkerModel::overlay_text_on_image(Mat *image){
	if (model_uninitialised){
		return;
	}
	
	//show detected features:
	putText(*image, " MUL",face_mouth_marker_vector[MOUTH_LEFT_UPPER].to_fixated_point2d(), FONT_HERSHEY_SIMPLEX, .5, CV_RGB(155,155,155), 1, CV_AA);
	putText(*image, " MUC",face_mouth_marker_vector[MOUTH_CENTER_UPPER].to_fixated_point2d(), FONT_HERSHEY_SIMPLEX, .5, CV_RGB(155,155,155), 1, CV_AA);
	putText(*image, " MUR",face_mouth_marker_vector[MOUTH_RIGHT_UPPER].to_fixated_point2d(), FONT_HERSHEY_SIMPLEX, .5, CV_RGB(155,155,155), 1, CV_AA);
	putText(*image, " MLL",face_mouth_marker_vector[MOUTH_LEFT_LOWER].to_fixated_point2d(), FONT_HERSHEY_SIMPLEX, .5, CV_RGB(155,155,155), 1, CV_AA);
	putText(*image, " MLC",face_mouth_marker_vector[MOUTH_CENTER_LOWER].to_fixated_point2d(), FONT_HERSHEY_SIMPLEX, .5, CV_RGB(155,155,155), 1, CV_AA);
	putText(*image, " MLR",face_mouth_marker_vector[MOUTH_RIGHT_LOWER].to_fixated_point2d(), FONT_HERSHEY_SIMPLEX, .5, CV_RGB(155,155,155), 1, CV_AA);
	
	putText(*image, " ELL",face_eye_marker_vector[EYE_LID_LEFT_LOWER].to_fixated_point2d(), FONT_HERSHEY_SIMPLEX, .5, CV_RGB(155,155,155), 1, CV_AA);
	putText(*image, " ELU",face_eye_marker_vector[EYE_LID_LEFT_UPPER].to_fixated_point2d(), FONT_HERSHEY_SIMPLEX, .5, CV_RGB(155,155,155), 1, CV_AA);
	putText(*image, " ERL",face_eye_marker_vector[EYE_LID_RIGHT_LOWER].to_fixated_point2d(), FONT_HERSHEY_SIMPLEX, .5, CV_RGB(155,155,155), 1, CV_AA);
	putText(*image, " ERU",face_eye_marker_vector[EYE_LID_RIGHT_UPPER].to_fixated_point2d(), FONT_HERSHEY_SIMPLEX, .5, CV_RGB(155,155,155), 1, CV_AA);
	
	putText(*image, " EBL",face_eye_marker_vector[EYE_BROW_LEFT_O].to_fixated_point2d(), FONT_HERSHEY_SIMPLEX, .5, CV_RGB(155,155,155), 1, CV_AA);
	putText(*image, " EBR",face_eye_marker_vector[EYE_BROW_RIGHT_O].to_fixated_point2d(), FONT_HERSHEY_SIMPLEX, .5, CV_RGB(155,155,155), 1, CV_AA);
	
	
}

void MarkerModel::check_above_and_swap(marker_vector_t *vect, int a, int b){
	//only do swappng if all candidates were found in this frame! otherwise keep permutation
	if ((vect->at(a).found) && (vect->at(b).found)){
		if (vect->at(a).is_above(vect->at(b))){
			swap (vect->at(a), vect->at(b));
		}
	}
}

void MarkerModel::check_left_and_swap3(marker_vector_t *vect, int a, int b, int c){
	//only do swappng if all candidates were found in this frame! otherwise keep permutation
	if ((vect->at(a).found) && (vect->at(b).found) && (vect->at(c).found)){
		//kind of stupid: unrolled bubble sort ;)
		if (vect->at(a).center_y > vect->at(b).center_y) swap(vect->at(a), vect->at(b));
		if (vect->at(b).center_y > vect->at(c).center_y) swap(vect->at(b), vect->at(c));
		if (vect->at(a).center_y > vect->at(b).center_y) swap(vect->at(a), vect->at(b));
	}
}

void MarkerModel::check_left_and_swap(marker_vector_t *vect, int a, int b){
	//only do swappng if all candidates were found in this frame! otherwise keep permutation
	if ((vect->at(a).found) && (vect->at(b).found)){
		if (vect->at(a).center_y > vect->at(b).center_y) swap(vect->at(a), vect->at(b));
	}
}

void MarkerModel::mouth_model_process(){
// 	if (mouth_input_vector.size() < 6){
// 		if (DEBUG_MARKERMODEL_FILTER) printf("> OOPS: not enough mouth blobs!\n");
// 		return;
// 	}
	
	//set all markers to "not found"
	for(int j=0; j<face_mouth_marker_vector.size(); j++){
		face_mouth_marker_vector[j].found = false;
	}
	
	//try to find mouth markers: find nearest neighbour for every known marker:
// 	printf("> COST MATRIX:\n");
	int size = max(mouth_input_vector.size(), face_mouth_marker_vector.size());
	int cost_array[size][size]; //mouth_input_vector.size()][face_marker_vector.size()];
	
	for(int j=0; j<size; j++){
// 		printf("> [%2d][] = ",j);
		for(int k=0; k<size; k++){
			int cost = 999;
			if ((k<face_mouth_marker_vector.size()) && (j<mouth_input_vector.size())){
				cost = face_mouth_marker_vector[k].distance_to(mouth_input_vector[j]);
				
				//disallow huge costs
				if (cost > 200){ cost = 2.0*cost; }
				//vote against markers far away from avg center:
				if (face_mouth_marker_vector[k].distance_to_avg(mouth_input_vector[j]) > 150){
					cost = 2.0 * cost;
// 					printf("> far from avg\n");
				}
			}
			
			cost_array[j][k] = cost;
// 			printf("%4d ",cost);
		}
// 		printf("\n");
	}
	
	
	
	if(munk_resolver.init_matrix(size,size,(int*)cost_array)){
		munk_resolver.run();
		
		for(int c=0; c<face_mouth_marker_vector.size(); c++){
			int row = munk_resolver.get_row_match_for_col(c);
// 			printf("> ASSIGN: %d -> %d\n",c,row);
			if (row != -1){
				//check for cost too high:
				if ((face_mouth_marker_vector[c].has_multiple_detections()) && (cost_array[row][c] > 50)){
					///printf("> discarding match with high cost %d\n", cost_array[row][c]);
				}else{
					if (cost_array[row][c] < 999){
						face_mouth_marker_vector[c].update_position(mouth_input_vector[row]);
					}
				}
			}
		}
	}
	
	//ok, we got the ideal assignment. however we want to check for plausibility here:
	//step1: upper markers should be ABOVE lower markers:
	check_above_and_swap(&face_mouth_marker_vector, MOUTH_LEFT_UPPER, MOUTH_LEFT_LOWER);
	check_above_and_swap(&face_mouth_marker_vector,MOUTH_CENTER_UPPER, MOUTH_CENTER_LOWER);
	check_above_and_swap(&face_mouth_marker_vector,MOUTH_RIGHT_UPPER, MOUTH_RIGHT_LOWER);
	
	//step2: left should be left of center, center should be left from right:
	check_left_and_swap3(&face_mouth_marker_vector, MOUTH_RIGHT_UPPER, MOUTH_CENTER_UPPER, MOUTH_LEFT_UPPER);
	check_left_and_swap3(&face_mouth_marker_vector, MOUTH_RIGHT_LOWER, MOUTH_CENTER_LOWER, MOUTH_LEFT_LOWER);
	
}


void MarkerModel::eye_model_process(){
// 	if (eyes_input_vector.size() < 10){
// 		if (DEBUG_MARKERMODEL_FILTER) printf("> OOPS: not enough eye blobs!\n");
// 		return;
// 	}
	
	//try to find mouth markers: find nearest neighbour for every known marker:
// 	printf("> COST MATRIX:\n");
	int size = max(eyes_input_vector.size(), face_eye_marker_vector.size());
	int cost_array[size][size]; //mouth_input_vector.size()][face_marker_vector.size()];
	
	for(int j=0; j<size; j++){
// 		printf("> [%2d][] = ",j);
		for(int k=0; k<size; k++){
			int cost = 999;
			if ((k<face_eye_marker_vector.size()) && (j<eyes_input_vector.size())){
				cost = face_eye_marker_vector[k].distance_to(eyes_input_vector[j]);
				
				//disallow huge costs
				if (cost > 200){ cost = 2.0*cost; }
				//vote against markers far away from avg center:
				if (face_eye_marker_vector[k].distance_to_avg(eyes_input_vector[j]) > 150){
// 					printf("> far from avg\n");
					cost = 2.0 * cost;
				}
			}
			
			cost_array[j][k] = cost;
// 			printf("%4d ",cost);
		}
// 		printf("\n");
	}
		
	
	if(munk_resolver.init_matrix(size,size,(int*)cost_array)){
		munk_resolver.run();
		
		for(int c=0; c<face_eye_marker_vector.size(); c++){
			int row = munk_resolver.get_row_match_for_col(c);
// 			printf("> ASSIGN: %d -> %d\n",c,row);
			if (row != -1){
				if ((face_eye_marker_vector[c].has_multiple_detections()) && (cost_array[row][c] > 50)){
// 					printf("> discarding match with high cost %d\n", cost_array[row][c]);
				}else{
					if (cost_array[row][c] < 999){
						face_eye_marker_vector[c].update_position(eyes_input_vector[row]);
					}
				}
			}
		}
		
	}
	
	
	//some model based corrections: FIXME: this could be a lot more advanced!
	
	//eyebrows:
	//always above eyelids!
	check_above_and_swap(&face_eye_marker_vector, EYE_BROW_RIGHT_I,EYE_LID_RIGHT_LOWER);
	check_above_and_swap(&face_eye_marker_vector, EYE_BROW_RIGHT_I,EYE_LID_RIGHT_UPPER);
	check_above_and_swap(&face_eye_marker_vector, EYE_BROW_RIGHT_O,EYE_LID_RIGHT_LOWER);
	check_above_and_swap(&face_eye_marker_vector, EYE_BROW_RIGHT_O,EYE_LID_RIGHT_UPPER);
	
	check_above_and_swap(&face_eye_marker_vector, EYE_BROW_LEFT_I,EYE_LID_LEFT_LOWER);
	check_above_and_swap(&face_eye_marker_vector, EYE_BROW_LEFT_I,EYE_LID_LEFT_UPPER);
	check_above_and_swap(&face_eye_marker_vector, EYE_BROW_LEFT_O,EYE_LID_LEFT_LOWER);
	check_above_and_swap(&face_eye_marker_vector, EYE_BROW_LEFT_O,EYE_LID_LEFT_UPPER);
	
	
	check_left_and_swap(&face_eye_marker_vector, EYE_BROW_RIGHT_O, EYE_BROW_RIGHT_I);
	check_left_and_swap(&face_eye_marker_vector, EYE_BROW_LEFT_I, EYE_BROW_LEFT_O);
	
	//eyelids & nose
	check_above_and_swap(&face_eye_marker_vector, EYE_NOSE_UPPER,EYE_NOSE_LOWER);
	check_left_and_swap3(&face_eye_marker_vector, EYE_LID_RIGHT_LOWER, EYE_NOSE_LOWER, EYE_LID_LEFT_LOWER);
	check_left_and_swap3(&face_eye_marker_vector, EYE_LID_RIGHT_UPPER, EYE_NOSE_UPPER, EYE_LID_LEFT_UPPER);
	
	check_above_and_swap(&face_eye_marker_vector, EYE_LID_LEFT_UPPER, EYE_LID_LEFT_LOWER);
	check_above_and_swap(&face_eye_marker_vector, EYE_LID_RIGHT_UPPER, EYE_LID_RIGHT_LOWER);
	
	check_left_and_swap(&face_eye_marker_vector, EYE_LID_RIGHT_UPPER, EYE_LID_LEFT_UPPER);
	check_left_and_swap(&face_eye_marker_vector, EYE_LID_RIGHT_LOWER, EYE_LID_LEFT_LOWER);
	
	
}

void MarkerModel::update(blob_vector_t blobs){
	if (model_uninitialised){
		return;
	}
	
	if (DEBUG_MARKERMODEL_FILTER) printf("> filtering %d blobs\n", (int)blobs.size());
	
	mouth_input_vector.clear();
	eyes_input_vector.clear();
	
	int removed_bb_min=0;
	int removed_bb_max=0;
	int removed_pcount=0;
	bool possible_candidate;
	for(blob_vector_t::iterator it=blobs.begin(); it!= blobs.end(); it++){
		possible_candidate = true;
		
		//filter small objects
		if (it->get_boundingboxsize() < boundingbox_size_min){
			possible_candidate = false;
			removed_bb_min++;
		}
		
		//filter huge objects
		if (it->get_boundingboxsize() > boundingbox_size_max){
			possible_candidate = false;
			removed_bb_max++;
		}
		
		//filter low pixelcount objects
		if (it->get_pixelcount() < pixelcount_min){
			possible_candidate = false;
			removed_pcount++;
		}
		
		/// add ?
		if (possible_candidate){
			//possible candidate!
			if (it->get_centeroid_x() < MOUTH_SEARCH_MIN_X){
				mouth_input_vector.push_back(Marker(*it));
			}
			if (it->get_centeroid_x() > EYES_SEARCH_MAX_X){
				eyes_input_vector.push_back(Marker(*it));
			}
		}
	}
	
	if (DEBUG_MARKERMODEL_FILTER) printf("> filter results: [mouth %2d] [eyes %2d] marker left [%2d, %2d, %2d]\n",(int)mouth_input_vector.size(),(int)eyes_input_vector.size(),removed_bb_min,removed_bb_max,removed_pcount);
	
	//process data
	mouth_model_process();
	eye_model_process();
	
// 	for(int i=0; i<6; i++){
// 		printf("cost[%d][] = ",i);
// 		for(int j=0; j<marker_input_vector.size(); j++){
// 			 printf("%8.2f ",cost_matrix[i][j]);
// 		}
// 		printf("\n");
// 	}

	
	
// 	std::sort (marker_input_vector.begin(), marker_input_vector.end(), face_model.mouth_left_lower.closer_to_marker);
	
	
	// 	exit(-1);
}