/*
* 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.
*
*/
/// DEPRECATED!

#include <iostream>
//#include <cvblob.h>
#include <stdio.h>
#include <sstream>
#include <iostream>
#include <math.h>
#include <fstream>
#include <vector>
#include <string>
#include <stdlib.h>
#include <algorithm>
#include <boost/algorithm/string.hpp>
// #include <boost/filesystem.hpp>
#include <libgen.h>
#include <boost/lexical_cast.hpp>
#include "boost/date_time/posix_time/posix_time.hpp"

#include <cv.h>
#include <highgui.h>
using namespace cv;

using namespace std;
using namespace boost;

#define ZOOM_RESULTVIEW 0
// #include <hungarian.h> //libhungarian -> solving assignment

#define WINDOW_SIZE 400

//using namespace cvb;
using namespace std;


Mat input_image;
Mat paint_image;

	
// IplImage *img;
// IplImage *timg;
int filepos = 0;
int selected_blob = -1;
bool reload = false;
int current_image = 0;
int mode_swap = 0;
// int mouse_x = 0;
// int mouse_y = 0;
int max_img;

bool arrow_positioning = false;
double mouse_pos_x = 0.0;
double mouse_pos_y = 0.0;

bool first_click = true;
float head_rot_angle_x = 0.0;
float head_rot_angle_y = 0.0;
float head_rot_angle_z = 0.0;

typedef struct{
	double x;
	double y;
	double z;
}pos3d_t;

typedef std::vector <pos3d_t> rotation_data_t;
rotation_data_t rotation_data;

double config_get_param_as_double(string);


typedef struct{
	double x;
	double y;
	void substract(string a, string b){ substract(config_get_param_as_double(a), config_get_param_as_double(b)); }
	void substract_x(string a){ substract(config_get_param_as_double(a), 0.0); }
	void substract_y(string a){ substract(0.0, config_get_param_as_double(a)); }
	void substract(double a, double b){ x -= a; y-= b; }
}pos2d_t;


// typedef std::vector <pos3d_t> rotation_data_t;
// rotation_data_t rotation_data;
typedef std::map<int, double> id2deg_t;
typedef std::map<int, id2deg_t> time2neckposdatamap_t;
time2neckposdatamap_t neck_positiondata_map;

typedef std::map<int, pos2d_t> id2posmap_t;
typedef std::map<int, id2posmap_t> time2posdatamap_t;
time2posdatamap_t positiondata_map;



typedef std::map<std::string, std::string> mocap_config_t;
mocap_config_t mocap_config;

int viewport_x;
int viewport_y;

bool gl_key_pressed[321];

bool equalize_image;

string config_get_param_as_string(string val_name){
	mocap_config_t::iterator it = mocap_config.find(val_name);
	if (it == mocap_config.end()){
		printf("ERROR: config entry for '%s' not found\n",val_name.c_str());
		exit(-1);
	}
	return it->second; //mocap_config[val_name];
}

double config_get_param_as_double(string val_name){
	double res_d;
	string res = config_get_param_as_string(val_name);

	try{
		res_d = boost::lexical_cast<double>(res);
	}catch(bad_lexical_cast &){
		printf("ERROR: can not convert config value '%s' (='%s') to double!\n",val_name.c_str(),res.c_str());
		exit(-1);
	}
	return res_d;
}

int config_get_param_as_int(string val_name){
	return (int) config_get_param_as_double(val_name);
}


void read_rotationdata(string filename){
// 	rotation_data
	printf("> reading mocap rotation file \"%s\"\n",filename.c_str());

        ifstream inFile(filename.c_str());
        string line;
        int linenum = 0;

	double fac_a = config_get_param_as_double("neck_data_factor_a");
	double fac_b = config_get_param_as_double("neck_data_factor_b");
	
	time2neckposdatamap_t neck_positiondata_map_tmp;
	
	while (getline (inFile, line)){
// 		printf("reading line %d\n",linenum);
		istringstream linestream(line);
                string item;
                int idx = 0;
		id2deg_t map_entry;
                while(getline(linestream, item, ' ')){
			if (idx <= 2){
				map_entry[idx] = atof(item.data());
			}
			idx++;
		}
		neck_positiondata_map_tmp[linenum] = map_entry;
		linenum++;
	}
	
	//reindex to frame numbering:
	int s = config_get_param_as_int("playback_pos_start");
	int e = config_get_param_as_int("playback_pos_end");
	for(int i=s; i<e; i++){
		int rot_datapos = fac_a + fac_b * (double)(i);
		neck_positiondata_map[i] = neck_positiondata_map_tmp[rot_datapos];
	}

}

void parse_config(string filename){
	printf("> reading mocap config file \"%s\"\n",filename.c_str());

        ifstream inFile(filename.c_str());
        string line;
        int linenum = 0;
	
	while (getline (inFile, line)){
// 		printf("reading line %d\n",linenum);
		istringstream linestream(line);
                string item;
                int idx = 0;
		string config_name;
                while(getline(linestream, item, '=')){
			trim(item);
			if (idx == 0){
				config_name = item;
			}else if(idx==1){
				vector<string> strs;
				split(strs, item, is_any_of("#"));
				mocap_config[config_name] = strs[0];
				printf("BEF %s\n",mocap_config[config_name].c_str());
				trim(mocap_config[config_name]);
				printf("AF %s\n",mocap_config[config_name].c_str());
				
				//printf("> '%s' = '%s'\n",config_name.c_str(),mocap_config[config_name].c_str());
			}
			idx++;
		}
		linenum++;
	}
	
	printf("> config file version v%.1f\n",config_get_param_as_double("mocap_config_version"));
	if (config_get_param_as_double("mocap_config_version") > 0.3){
		printf("> ERROR: expected config file version <= v0.3! exiting\n");
	}
	if (config_get_param_as_double("mocap_config_version") == 0.3){
		printf("> description = \"%s\"\n",config_get_param_as_string("mocap_description").c_str());
	}
}

//use: http://www.arndt-bruenner.de/mathe/scripts/gleichungssysteme.htm
///mocap1
///insert 2 peaks: rotation maxima on frame 629->3575 / frame 1308->5814
///                a + 629b = 3575; a + 1308b = 5814
///double rotationdata_pos_start = 1501;
///double rotationdata_scale_factor  = 3.2974963181;
///mocap2
///insert 2 peaks: rotation maxima on frame 24->433 / frame 896->1970
///                a + 24b = 433; a + 869b = 1970
double rotationdata_pos_start = 389.3456;
double rotationdata_scale_factor  = 1.8189349112;
pos3d_t rotationdata_estimate_rotation(int framenumber){
	int rot_datapos = rotationdata_pos_start + rotationdata_scale_factor * (double)(framenumber);
	if (rot_datapos > rotation_data.size()){
		printf("error: position out of range, returning 0,0,0 as rot\n");
		pos3d_t pos;
		pos.x = 0.0;
		pos.y = 0.0;
		pos.z = 0.0;
		return pos;
	}
	return rotation_data[rot_datapos];
}





int get_posdata(int frameidx, id2posmap_t *posmap){
	//get trackingdata
	int offset = config_get_param_as_int("playback_pos_start");
	int idx_corr = frameidx; // - offset;
	printf("IDX CORR %d\n",idx_corr);
	if (positiondata_map.find(idx_corr) != positiondata_map.end()){
		*posmap = positiondata_map.find(idx_corr)->second;
		return 1;
	}
	return 0;
}

int set_blobdata(int frameidx, int id, pos2d_t pos){
	int offset = config_get_param_as_int("playback_pos_start");
	int idx_corr = current_image; // - offset;
	if (positiondata_map.find(idx_corr) != positiondata_map.end()){
		if (positiondata_map.find(idx_corr)->second.find(id) != positiondata_map.find(idx_corr)->second.end()){
			printf("replacing %d (%f, %f) with (%f %f)\n",id,positiondata_map.find(idx_corr)->second[id].x,positiondata_map.find(idx_corr)->second[id].y,pos.x,pos.y);
			positiondata_map.find(idx_corr)->second[id] = pos;
			return 1;
		}
	}
	return 0;
}

int swap_blobdata(int frameidx, int id1, int id2){
	int offset = config_get_param_as_int("playback_pos_start");
	int idx_corr = current_image; // - offset;
	if (positiondata_map.find(idx_corr) != positiondata_map.end()){
		if (positiondata_map.find(idx_corr)->second.find(id1) != positiondata_map.find(idx_corr)->second.end()){
// 			printf("replacing %d (%f, %f) with (%f %f)\n",id,posdata[id].x,posdata[id].y,pos.x,pos.y);
			pos2d_t pos = positiondata_map.find(idx_corr)->second[id1];
			positiondata_map.find(idx_corr)->second[id1] = positiondata_map.find(idx_corr)->second[id2];
			positiondata_map.find(idx_corr)->second[id2] = pos;
			return 1;
		}
	}
	return 0;
}

bool autoplay = false;

void update_zoom(){
	const int cutsize = 100;
	int zoom_factor = 4; // x4
	Mat zoom;
	

	int pos_x = mouse_pos_y - cutsize/2;
	int pos_y = (input_image.size().height - mouse_pos_x) - cutsize/2; //flipped
	
	
	if (pos_x < 1) pos_x = 1;
	if (pos_y < 1) pos_y = 1;
	if (pos_x >= input_image.size().width  - cutsize) pos_x = input_image.size().width - cutsize -1;
	if (pos_y >= input_image.size().height - cutsize) pos_y = input_image.size().height - cutsize -1;

	//printf("posx %d / posy %d  [w %d / h %d]\n",pos_x,pos_y,input_image.size().width,input_image.size().height);

	((input_image)(Rect(pos_x,pos_y, cutsize,cutsize))).copyTo(zoom);
	resize(zoom, zoom, Size(zoom_factor*cutsize,zoom_factor*cutsize)); //zoom
	
	//rotate
	flip(zoom, zoom, 0);
	transpose(zoom, zoom);
	
	line(zoom, Point(0,cutsize*zoom_factor/2), Point(cutsize*zoom_factor/2-5, cutsize*zoom_factor/2), CV_RGB(255,0,0), 1, CV_AA, 0);
	line(zoom, Point(cutsize*zoom_factor,cutsize*zoom_factor/2), Point(cutsize*zoom_factor/2+5, cutsize*zoom_factor/2), CV_RGB(255,0,0), 1, CV_AA, 0);
	
	line(zoom, Point(cutsize*zoom_factor/2,0), Point(cutsize*zoom_factor/2,cutsize*zoom_factor), CV_RGB(255,0,0), 1, CV_AA, 0);
	circle(zoom, Point(cutsize*zoom_factor/2, cutsize*zoom_factor/2), 48, CV_RGB(255,255,255), 1, CV_AA, 0);
	circle(zoom, Point(cutsize*zoom_factor/2, cutsize*zoom_factor/2), 75, CV_RGB(255,255,255), 1, CV_AA, 0);
	
	imshow("zoom", zoom);
	#if 0
	cvSetImageROI(zimg, cvRect(mouse_pos_x-50,mouse_pos_y-50, 100,100));
	cvResize(zimg, img_z);
	
	//pos can be incremented in micro steps (0.25) thus extract those:
	int center_pos_x = img_z->width/2 + 4*(mouse_pos_x  - floor(mouse_pos_x));
	int center_pos_y = img_z->height/2 + 4*(mouse_pos_y  - floor(mouse_pos_y));
	
	cvRectangle(img_z,cvPoint(0, center_pos_y),cvPoint(img_z->width, center_pos_y),CV_RGB(255, 0, 0),1, 8, 0);
	cvRectangle(img_z,cvPoint(center_pos_x,0),cvPoint(center_pos_x,img_z->height),CV_RGB(255, 0, 0),1, 8, 0);
	cvCircle(img_z, cvPoint(center_pos_x,center_pos_y), cvRound(48), CV_RGB(255,255,255), 1);
// 	cvCircle(img_z, cvPoint(center_pos_x,center_pos_y), cvRound(45), CV_RGB(255,255,255), 1);
// 	cvCircle(img_z, cvPoint(center_pos_x,center_pos_y), cvRound(40), CV_RGB(255,255,255), 1);
	cvCircle(img_z, cvPoint(center_pos_x,center_pos_y), cvRound(15), CV_RGB(255,255,255), 1);
	cvShowImage("zoom", img_z);
	
	IplImage *frame = cvCreateImage(cvGetSize(img_z), img_z->depth, img_z->nChannels);
	IplImage *Y=cvCreateImage(cvGetSize(img_z),8,1);
	cvCvtColor( img_z, frame, CV_BGR2YCrCb );//YUV For codebook method
	cvSplit(frame,Y,NULL,NULL,NULL);
	cvShowImage("zoom bw",Y);
	
 	cvReleaseImage(&zimg);
	cvReleaseImage(&img_z);
	cvReleaseImage(&Y);
	cvReleaseImage(&frame);
	#endif
	
}


void set_filepos(int pos){
	if ((pos > 0) && (pos < max_img))
		filepos = pos;
	
	setTrackbarPos("pos", "INPUT", filepos);
	reload = true;
	update_zoom();
}

void mouse_handler(int event, int x, int y, int flags, void* param){
	
	/*if (event == CV_EVENT_RBUTTONDOWN){
		autoplay = false;
		selected_blob = -1;
		reload = true;
		//printf("POS: %d, %d\n",y,input_image.size().height-x);
		if (filepos > 0) filepos--;
	}*/
	
	if (event == CV_EVENT_LBUTTONDOWN){
		autoplay = false;
		if (selected_blob == -1){
			set_filepos(filepos+1);
			arrow_positioning = false;
		}else{
			//move blob:
			pos2d_t pos;
			printf("> MOUSE %f %f\n",mouse_pos_x,mouse_pos_y);
			pos.x = mouse_pos_y;
			pos.y = input_image.size().height - mouse_pos_x;
			set_blobdata(current_image, selected_blob, pos);
			
			//trigger repaint
			set_filepos(filepos+1);
// 			selected_blob = -1;
		}
	}
	
	mouse_pos_x = x;
	mouse_pos_y = y;
	update_zoom();
}


int swap_from = -1;


void do_swapping(){
	if (mode_swap != 0){
		if (mode_swap == 2){
			swap_from = selected_blob;
			mode_swap--;
		}else if (mode_swap == 1){
			//do swapping:
			swap_blobdata(current_image, swap_from, selected_blob);
			selected_blob = -1;
			
			mode_swap = 0;
		}
	}
}

void read_rotationdata(char *filename){
// 	rotation_data
	printf("> reading mocap rotation file \"%s\"\n",filename);

        ifstream inFile(filename);
        string line;
        int linenum = 0;

	while (getline (inFile, line)){
// 		printf("reading line %d\n",linenum);
		istringstream linestream(line);
                string item;
                int idx = 0;
		pos3d_t position;
                while(getline(linestream, item, ' ')){
			switch(idx){
				///wiimote
				///case(2): position.x = -20 + atof(item.data()); break;
				///case(0): position.y = 40 - atof(item.data()); break;
				///case(1): position.z = atof(item.data()) - 30; break;
				
				///razor imu
				case(0): position.z = atof(item.data()); break; //yaw
				case(1): position.y = atof(item.data()); break; //tilt
				case(2): position.x = atof(item.data()); break; //pan 
			}
			idx++;
		}
		rotation_data.push_back(position);
		linenum++;
	}

}
void read_trackingdataset(string filename){
	printf("> reading mocap file \"%s\"\n",filename.c_str());

        ifstream inFile(filename.c_str());
        string line;
        int linenum = 0;

	while (getline (inFile, line)){
// 		printf("reading line %d\n",linenum);
		istringstream linestream(line);
                string item;
                int idx = 0;
		pos2d_t position;
		
		id2posmap_t posdata;
		bool extract_index=true;
		int index=0;
                while(getline(linestream, item, ' ')){
// 			printf("s=%s f=%f\n",item.c_str(), atof(item.data()));
			if (extract_index){
				index = atoi(item.data());
				extract_index = false;
			}else{
				if ((idx & 1) == 0){
					position.x = atof(item.data()); //+201;
				}else if ((idx & 1) == 1){
					int id = idx / 2;
					position.y = atof(item.data()); //+140;
					posdata[id] = position;
	// 				printf("id %d = %f, %f\n",id, position.x,position.y);
				}
				idx++;
			}
		}
		positiondata_map[linenum] = posdata;
		linenum++;
	}
}


void write_trackingdataset(const char *filename){
	printf("> writing mocap file \"%s\"\n",filename);

	FILE *out;
	if (!(out = fopen(filename, "w"))){
		printf("ERROR: cant write data to '%s'\n",filename);
		return;
	}
	
	
	int fpos = 0;
	for(time2posdatamap_t::iterator it1=positiondata_map.begin(); it1!=positiondata_map.end(); ++it1){
		fprintf(out, "%d ",fpos);
		fpos++;
		id2posmap_t posdata = it1->second;
		for (id2posmap_t::iterator it2=posdata.begin(); it2!=posdata.end(); ++it2){
			pos2d_t pos = it2->second;
			fprintf(out, "%f %f ",pos.x,pos.y);
		}
		fprintf(out, "\n");
	}
	
        fclose(out);
	printf("done\n");
}

const char *get_textid(int id){
	stringstream buffer;
	if (id <= 9){
		buffer << id;
	}else{
		buffer << (char)(id - 10 + 'a');
	}
	return buffer.str().c_str();
}

int wait_gl_and_cv(int delay){
	#if SHOW_GL_HEAD
// 	gl_process_events();
	#endif
	return cvWaitKey(delay);
}

void fetch_image(Mat *img, string path_dir, int num){
	char infilename[128];
	
	string image_filenames = path_dir + "/" + config_get_param_as_string("mocap_images_dir")+"/"+ config_get_param_as_string("mocap_images_filename");
	sprintf(infilename,image_filenames.c_str(),num);
	printf("> reading %s\n",infilename);
	*img = imread(infilename);
	
	if (img->data == NULL){
		printf("can not read file %s. exiting\n",infilename);
		exit(-1);
	}
	
	Mat rot;
 	flip(*img, *img, 0);
	flip(*img, *img, 1);
 	//transpose(*img, *img);
	
	if(equalize_image){
		Mat hsv;
		vector <Mat> hsv_vector;
		
		//convert to hsv
		cvtColor(*img, hsv, CV_BGR2HSV);
		//split to single channels
		split(hsv, hsv_vector);
		//equalize V channel
		equalizeHist(hsv_vector[2],hsv_vector[2]);
		//merge back
		merge(hsv_vector, hsv);
		//back to rgb
		cvtColor(hsv, *img, CV_HSV2BGR);
		//cleanup
		hsv.release();
		hsv_vector[0].release();
		hsv_vector[1].release();
		hsv_vector[2].release();
	}
	
	
	
}

void show_info_window(){
	//show info text
	CvFont font =  fontQt("Liberation Mono", 12, CV_RGB(255,255,255));
	Mat info_image(Size(400,200), CV_8UC3, Scalar::all(0));
	
	addText(info_image, "1-9a-z      = select id",  Point(10,10+10), font);
	addText(info_image, "^           = swap mode",  Point(10,10+30), font);
	addText(info_image, "ALT key     = next frame", Point(10,10+50), font);
	addText(info_image, "WINDOWS key = prev frame", Point(10,10+70), font);
	addText(info_image, "dbl   click = animate",    Point(10,10+90), font);
	//addText(info_image, "arrows      = sub pixel movement (sel blob)",    Point(10,10+110), font);
	imshow("info", info_image);
}


//global helper
void slider_adjusting(int pos, void *data){
// 	set_slider_position(pos);
};


//global helper
void button_wrapper_bool2(int state, void *data){
	if (data != NULL){
// 		printf("BUT not null \n");
		if (state){
			*((bool*)data) = true;
		}else{
			*((bool*)data) = false;
		}
	}
};


int main(int args, char *argv[]){
	//show usage
	if (args != 2){
		printf("usage: %s <mocap config file>\n\n",argv[0]);
		return -1;
	}
	
	//get path of config file (we assume head+face file is in same dir as config!)
// 	filesystem::path config_path(argv[1]);
	string path_file = argv[1];
	string path_dir  = dirname(argv[1]);
	printf("path is %s\n",path_dir.c_str());
	
	//step0: read & parse config file 
	parse_config(path_file);
	
	//check config rev
	string cv = config_get_param_as_string("mocap_config_version");
	
	//step1: load animation dataset:
	read_trackingdataset(path_dir + "/" + config_get_param_as_string("mocap_file_face"));
	read_rotationdata   (path_dir + "/" + config_get_param_as_string("mocap_file_head"));
	
	char *trackingdata_filename = argv[1];
	char *rotdata_filename = argv[2];
	string trackingdata_filename_out_s = path_dir + "/" + config_get_param_as_string("mocap_file_face") + ".saved";
	const char *trackingdata_filename_out = trackingdata_filename_out_s.c_str();
	const char *dirname = path_dir.c_str();

	bool done=false;
	equalize_image = true;
	
	
	
	namedWindow("INPUT", CV_WINDOW_AUTOSIZE);
// 	cvNamedWindow("zoom", CV_WINDOW_AUTOSIZE);
// 	cvNamedWindow("zoom bw", CV_WINDOW_AUTOSIZE);
// 	cvNamedWindow("info", CV_WINDOW_AUTOSIZE);
// 	cvNamedWindow("seek position", CV_WINDOW_AUTOSIZE);
	
	setMouseCallback("INPUT", mouse_handler, 0);
	
	int offset_img = config_get_param_as_int("playback_pos_start");
	filepos = offset_img;
	max_img = config_get_param_as_int("playback_pos_end");
	float fps = 0.5*config_get_param_as_double("mocap_fps_cam");

	//add gui elements
	createButton("equalize brightness",&button_wrapper_bool2,&equalize_image,CV_CHECKBOX,equalize_image);
	createTrackbar("pos", "INPUT", &filepos, max_img, slider_adjusting, NULL);
	
	
//  	moveWindow("input", 200, 200);
// 	cvMoveWindow("zoom", 500+200, 200);
// 	cvMoveWindow("zoom bw", 500+200+450, 200);
// 	cvMoveWindow("info", 500+200, 680);
	
	fetch_image(&input_image, path_dir, current_image);
	
	imshow("INPUT", input_image);
	
	show_info_window();
// 	while(1) waitKey(19);
	
	
	current_image = -1;
	

	while (!done){
		//wait for new selection
		while((filepos == current_image) && (!reload) && (!autoplay)){
// 			printf("> waiting %d %d\n",filepos,current_image);
			unsigned int key = (waitKey(200)) & 0xFFFF;
			if ((key >= '0') && (key <= '9')){
				selected_blob = key - '0';
				reload = true;
				if (mode_swap) do_swapping();
				update_zoom();
			}else if ((key >= 'a') && (key <= 'z')){
				selected_blob = 10 + key - 'a';
				reload = true;
				if (mode_swap) do_swapping();
				update_zoom();
			}else{
				
				switch (key){
					default: 
						if (key != 0xFFFF) printf("got %d\n",key);
						break;
				
					case(' '):
						write_trackingdataset(trackingdata_filename_out);
						break;
						
					case('^'):
						mode_swap = 2; //swap!
						break;
					
					case(60): //< > key
					case(35): //arrow right
						autoplay = false;
						selected_blob = -1;
						reload=true;
						set_filepos(filepos+1);
						break;
						
					case(34): //arrow left
						autoplay = false;
						selected_blob = -1;
						reload = true;
						//printf("POS: %d, %d\n",y,input_image.size().height-x);
						set_filepos(filepos-1);
						break;
						
					case(65364): //arrow up
						break;
						
					case(65362): //arrow down
						break;

				}
			}
		}
		
		if (current_image == 0){
			first_click = true;
		}
		
		if (autoplay){
			waitKey(1000.0/fps );
			if (filepos < max_img){
				filepos++;
			}
		}
		
		reload = false;
		
		current_image = filepos;
		int current_ani_pos = filepos; // - offset_img;
		
		
		fetch_image(&input_image, path_dir, current_image);
// 		imshow("INPUT", input_image);
		///update_zoom();
		
		
		if (input_image.data == NULL){
			done = 1;
		}else{
			paint_image = input_image.clone();
			if (current_ani_pos > 0){
				//get trackingdata
				id2posmap_t posdata;
				if (get_posdata(filepos, &posdata)){
					for (id2posmap_t::iterator it=posdata.begin(); it!=posdata.end(); ++it){
						int id = it->first;
						
						pos2d_t pos = it->second;
						if (id == 0){
						printf("got %f %f\n",pos.x,pos.y);
						}
						if (selected_blob == id){
							circle(paint_image, Point(pos.x,pos.y), 40, CV_RGB(255,255,0), 3, CV_AA, 0);
						}else{
							if (selected_blob == -1){
								if (id & 1){
									circle(paint_image, Point(pos.x,pos.y), 20, CV_RGB(0,155,255), 1, CV_AA, 0);
								}else{
									circle(paint_image, Point(pos.x,pos.y), 20, CV_RGB(255,155,0), 1, CV_AA, 0);
								}
							}
						}
						
						line(paint_image, Point(pos.x-15,pos.y), Point(pos.x+15,pos.y), CV_RGB(255,0,0), 1, CV_AA, 0);
						line(paint_image, Point(pos.x,pos.y-15), Point(pos.x,pos.y+15), CV_RGB(255,0,0), 1, CV_AA, 0);
					}
				}
			
				
				//rotate image:
				flip(paint_image, paint_image, 0);
				transpose(paint_image, paint_image);
			
				if (get_posdata(filepos, &posdata)){
					CvFont font =  fontQt("Liberation Mono", 12, CV_RGB(255,255,255));
					
					for (id2posmap_t::iterator it=posdata.begin(); it!=posdata.end(); ++it){
						int id = it->first;
// 						printf("ADDING id %d\n",id);
						pos2d_t pos = it->second;
						addText(paint_image, get_textid(id), Point(paint_image.size().width-pos.y+20,pos.x), font);
					}
				}
			}else{
				//show only
				//rotate image:
				flip(paint_image, paint_image, 0);
				transpose(paint_image, paint_image);
			}
			
			printf("> update INPUT\n");
			
			Mat gray;
			cvtColor( paint_image, gray, CV_BGR2GRAY );
			equalizeHist( gray, gray );
// 			imshow("INPUT2", gray);
									
			
			imshow("INPUT", paint_image);
// 			showInput();
			
		}
// 		cvReleaseImage(&img);
// 		cvReleaseImage(&img_copy);
	}
	
// 	cvReleaseImage(&img);

// 	cvDestroyWindow("input");
// #endif
	return 0;
}



/*
bool gl_process_events(){
	SDL_Event event;
	if( SDL_PollEvent(&event) ){
// 	   printf("pressed %d\n", event.key.keysym.sym);
		switch( event.type ){
			case SDL_KEYDOWN : gl_key_pressed[ event.key.keysym.sym ]=true ;   break;
			case SDL_KEYUP   : gl_key_pressed[ event.key.keysym.sym ]=false;   break;
			case SDL_QUIT    : exit(-1); break; //return false; break;
		}
	}
	return true;
}


//Initialze OpenGL perspective matrix
void gl_setup(int width, int height){
	SDL_Init(SDL_INIT_VIDEO);
	
	const SDL_VideoInfo* info = SDL_GetVideoInfo();
	int vidFlags = SDL_OPENGL | SDL_GL_DOUBLEBUFFER;
	
	if (info == NULL){
		printf("ERROR: SDL_GetVideoInfo() returned NULL\n!");
		exit(-1);
	}
	
	if (info->hw_available) {
		vidFlags |= SDL_HWSURFACE;
	}else {
		vidFlags |= SDL_SWSURFACE;
	}
	
	int bpp = info->vfmt->BitsPerPixel;
	SDL_SetVideoMode(width, height, bpp, vidFlags);

	glViewport( 0, 0, width, height );
	glMatrixMode( GL_PROJECTION );
	glEnable( GL_DEPTH_TEST );
	gluPerspective( 45, (float)width/height, 0.1, 100 );
	glMatrixMode( GL_MODELVIEW );
}


GLMmodel* pmodel1 = NULL;

void gl_draw_model(void){
	
    if (!pmodel1){
// 	    glEnable(GL_TEXTURE_2D);
	printf("> loading head.obj file (openGL view)\n");
	pmodel1 = glmReadOBJ("head.obj");
	if (!pmodel1) exit(-1);
        glmUnitize(pmodel1);
        glmFacetNormals(pmodel1);
        glmVertexNormals(pmodel1, 90.0);

    }
    // This is the call that will actually draw the model
    // Don't forget to tell it if you want textures or not :))
    glmDraw(pmodel1, GLM_SMOOTH|  GLM_MATERIAL); // | GLM_TEXTURE);
}

void gl_update_view(){
	pos3d_t rot = rotationdata_estimate_rotation(current_image);
	
	glClearColor(0.7f, 0.7f, 0.9f, 0.0f);	
	
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
		glLoadIdentity();
		glTranslatef(0,0, -1.5);
		
		glRotatef(rot.x, 1, 0, 0);
		glRotatef(rot.y, 0, 1, 0);
		glRotatef(rot.z, 0, 0, 1);

		gl_draw_model();
		
		SDL_GL_SwapBuffers();
}*/