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

#define RGBFILTER_USE_NORMALIZED_RGB 0
#define RGBFILTER_USE_NORMALIZED_RG  1

#define RGBFILTER_USE_NORMALIZED_CUT_OFF_MINSUM 20
#define SHOW_INPUT_AS_NORMALIZED     1
#define RGBFILTER_SHOW_INPUT 1

#define LOOKUPTABLE_IMPORT_FILENAME_OVERRIDE "./lookuptable.bin"


#if (RGBFILTER_USE_NORMALIZED_RGB && RGBFILTER_USE_NORMALIZED_RG)
ERROR: select only one
#endif


RGBImageFilter::RGBImageFilter(ConfigOptions *c){
	//set config
	cfg = c;
	
	int colorsample_lookup_size = 256*256*256;
	colorsample_lookup = (int *) malloc((colorsample_lookup_size)*sizeof(int));
	if (colorsample_lookup == NULL){
		printf("> ERROR: can not allocate memory for colorsample lookup [%d MB]\n",colorsample_lookup_size/1024/1024);
		exit(-1);
	}
	
	if (cfg->rgb_lookuptable_from_file()){
		string filename = cfg->rgb_lookuptable_filename();
		printf("> lookuptable initialised with file data (%s)\n",filename.c_str());
		FILE *fp = fopen(filename.c_str(), "r");
		if (fp){
			int cnt[4] = {0,0,0,0};
			for(int i=0; i<=0xFFFFFF; i++){
				colorsample_lookup[i] = getc(fp);
				cnt[colorsample_lookup[i]]++;
	//  			printf("%6d = %d\n",i,colorsample_lookup[i]);
			}
			printf("> classes [%6d, %6d, %6d %6d]\n",cnt[0],cnt[1],cnt[2],cnt[3]);
			fclose(fp);
		}else{
			printf("> ERROR reading file\n");
		}
	}else{
		//initialise:
		lookuptable_update();
	}
}


bool RGBImageFilter::colorclass_add_pixel(int colorclass, int r, int g, int b){
	printf("> rgb_filter: add_pixel colorclass=%d, rgb=%02X%02X%02X [%3d %3d %3d]\n",colorclass,r,g,b,r,g,b);
	
	colorclass_insert(colorclass,r,g,b);
	
	//trigger update
	lookuptable_update();
	
	return true;
}

bool RGBImageFilter::colorclass_insert(int colorclass, int r, int g, int b){
	colorsample_t colorsample;
	colorsample.id = colorclass;
	
	
	#if RGBFILTER_USE_NORMALIZED_RGB
		int sum = 1+r+g+b;
		r = (r * 255) / sum;
		g = (g * 255) / sum;
		b = (b * 255) / sum;
		#ifdef RGBFILTER_USE_NORMALIZED_CUT_OFF_MINSUM
		if (sum < RGBFILTER_USE_NORMALIZED_CUT_OFF_MINSUM){
			r=0; g=0; b=0;
		}
		#endif
	#elif RGBFILTER_USE_NORMALIZED_RG
		int sum = 1+r+g;
		r = (r * 255) / sum;
		g = (g * 255) / sum;
		b = (b * 255) / sum;
		#ifdef RGBFILTER_USE_NORMALIZED_CUT_OFF_MINSUM
		if (sum < RGBFILTER_USE_NORMALIZED_CUT_OFF_MINSUM){
			r=0; g=0; b=0;
		}
		#endif
	#endif
	
	colorsample.r = r;
	colorsample.g = g;
	colorsample.b = b;
	
	//add to vector
	cfg->colorsample_vector.push_back(colorsample);
}

bool RGBImageFilter::colorclass_remove(int colorclass, int index){
	printf("> rgb_filter: remove colorclass=%d index %d\n",colorclass,index);
	return false;
}

bool RGBImageFilter::colorclass_clear(){
	cfg->colorsample_vector.clear();
	lookuptable_update();
	return true;
}


//simplified eucledian distance
inline int RGBImageFilter::get_distance(int r, int g, int b, int r2, int g2, int b2){
	int dx, dy, dz, tmp;
	//simplified^2 eucledian distance
	//0.41dx + 0.9...dy -> 0.5dx + 1dy
	if(r2<r) dx=r-r2; else dx=r2-r;
	if(g2<g) dy=g-g2; else dy=g2-g;
	if(b2<b) dz=b-b2; else dz=b2-b;
	
	if (dx<dy)  tmp=(dx >> 1) + dy;    else tmp = (dy >> 1) + dx;
	if (tmp<dz) tmp = (tmp >> 1) + dz; else tmp = (dz >> 1) + tmp;

	return tmp;
}



void RGBImageFilter::lookuptable_update(){
	int normalized_r, normalized_g, normalized_b, sum_rgb;
	int r_base, g_base, val;
	int i;
	int minval, minpos;
	int dist;
	
	int sample_count = cfg->colorsample_vector.size();
	if (sample_count == 0){
		printf("> updating lookuptable: no samples\n");
		for(int i=0; i<=0xFFFFFF; i++){
			colorsample_lookup[i] = 0;
		}
		return;
	}
	printf("> updating lookuptable: processing [%d] samples\n", sample_count);
	
	//fill lookuptable
	int r_255,g_255,b_255;
	int r, g, b;
	int rgb = 0;
	
	for (r = 0; r < 256; r++){
		//print statusbar
		printf("\r> ");
		for (int p = 0; p < 25; p++){
			if (p < r / 10)
				printf("#");
			else
				printf(" ");
		}
		printf(" %3d%% done", (r * 100) / 255);
		fflush(stdout);

		r_base = r << 16;
		r_255  = r*255;
		for (g = 0; g < 256; g++){
			g_base = r_base + (g << 8);
			g_255  = g*255;
			for (b = 0; b < 256; b++){
				//val = g_base + b;
				b_255  = b*255;
				
				//convert to normalized rgb
				#if RGBFILTER_USE_NORMALIZED_RGB
					sum_rgb = 1+r+g+b;
	
					normalized_r = r_255 / sum_rgb;
					normalized_g = g_255 / sum_rgb;
					normalized_b = b_255 / sum_rgb;
				#elif RGBFILTER_USE_NORMALIZED_RG
					sum_rgb = 1+r+g;
	
					normalized_r = r_255 / sum_rgb;
					normalized_g = g_255 / sum_rgb;
					normalized_b = b_255 / sum_rgb;
				#endif
				
				#ifdef RGBFILTER_USE_NORMALIZED_CUT_OFF_MINSUM
				if (sum_rgb < RGBFILTER_USE_NORMALIZED_CUT_OFF_MINSUM){
					//too dark -> colorclass 0
					minpos = 0;
				}else{
				#else
				
				if (1){
				#endif
					//check distance to all colors:
					minval = 0x7FFFFFFF;
					minpos = 1;
					for (i = 0; i < sample_count; i++){
						colorsample_t color_sample_i = cfg->colorsample_vector[i];
						#if RGBFILTER_USE_NORMALIZED_RGB
						dist = get_distance(normalized_r, normalized_g,normalized_b, color_sample_i.r, color_sample_i.g, color_sample_i.b);
						#elif RGBFILTER_USE_NORMALIZED_RG
						dist = get_distance(normalized_r, normalized_g,normalized_b, color_sample_i.r, color_sample_i.g, color_sample_i.b);
						#else
						dist = get_distance(r,g,b, color_sample_i.r, color_sample_i.g, color_sample_i.b);
						#endif
						if (dist < minval){
							minval = dist;
							minpos = color_sample_i.id;
						}
					}
				}
				colorsample_lookup[rgb] = minpos;
				rgb++;
			}
		}
	}
// 	#endif
	//statistics:
	int count[sample_count];
	count[0] = 0;
	count[1] = 0;
	count[2] = 0;
	count[3] = 0;
	
	for(int i=0; i<=0xFFFFFF; i++){
		count[colorsample_lookup[i]]++;
	}
	printf("\n> stats: DONTCARE[%3d] RED[%3d] GREEN[%3d] BLUE[%3d]\n",count[0],count[1],count[2],count[3]);
	
	printf(" -> finished.\n");
	
// 	printf("AAAA\n");
// 	for(int i=0; i<0xFFFFFF; i++){
// 		printf("%c", colorsample_lookup[i]);
// 	}
// 	printf("%c\n", 0);
}

void RGBImageFilter::filter_image(Mat *image, Mat *output, Mat *pixelfilter_result){
	int x,y;
	uchar r,g,b;
	int rgb[16];
	int div;
	uchar *imagedata_ptr;
	uchar *outputdata_ptr;
	uchar *pixelfilter_result_ptr;
	
	//for(x=0; x<16; x++){rgb[x] = 0;}
	//paint everythink black:
	#if !RGBFILTER_SHOW_INPUT
	for(y=0; y<image->size().height-1; y++){
 		memset((int*)output->ptr(y),0x00000000,image->size().width*3);
	}
	#endif 
	
	if (!image->isContinuous()){
		printf("> ERROR: input image is not continous -> linear access speedup not possible. exiting\n");
		return; //exit(1);
	}

	//clear output
	*pixelfilter_result = Mat::zeros(pixelfilter_result->size(), pixelfilter_result->type());

	//HACK: iterate to height-1 because we might access ONE byte more than allowed!
	for(y=0; y<image->size().height-1; y++){
		imagedata_ptr = image->ptr(y);
		outputdata_ptr = output->ptr(y);
		pixelfilter_result_ptr = pixelfilter_result->ptr(y);
		
		for(x=0; x<image->size().width; x+=48/3){
			//this is actually much faster... cachiing?! 
			//maybe further optimizable by using 32bit-wise access and/or SSE?!
			rgb[ 0] = (*((int*)(imagedata_ptr+ 0))) & 0xFFFFFF;
			rgb[ 1] = (*((int*)(imagedata_ptr+ 3))) & 0xFFFFFF;
			rgb[ 2] = (*((int*)(imagedata_ptr+ 6))) & 0xFFFFFF;
			rgb[ 3] = (*((int*)(imagedata_ptr+ 9))) & 0xFFFFFF;
			rgb[ 4] = (*((int*)(imagedata_ptr+12))) & 0xFFFFFF;
			rgb[ 5] = (*((int*)(imagedata_ptr+15))) & 0xFFFFFF;
			rgb[ 6] = (*((int*)(imagedata_ptr+18))) & 0xFFFFFF;
			rgb[ 7] = (*((int*)(imagedata_ptr+21))) & 0xFFFFFF;
			rgb[ 8] = (*((int*)(imagedata_ptr+24))) & 0xFFFFFF;
			rgb[ 9] = (*((int*)(imagedata_ptr+27))) & 0xFFFFFF;
			rgb[10] = (*((int*)(imagedata_ptr+30))) & 0xFFFFFF;
			rgb[11] = (*((int*)(imagedata_ptr+33))) & 0xFFFFFF;
			rgb[12] = (*((int*)(imagedata_ptr+36))) & 0xFFFFFF;
			rgb[13] = (*((int*)(imagedata_ptr+39))) & 0xFFFFFF;
			rgb[14] = (*((int*)(imagedata_ptr+42))) & 0xFFFFFF;
			rgb[15] = (*((int*)(imagedata_ptr+45))) & 0xFFFFFF;
			
			
			for(int z=0; z<16; z++){
#if 0
				(*((int*)(outputdata_ptr+z*3))) = rgb[z]; //(*((int*)imagedata_ptr+z*3));
#else						
				switch(colorsample_lookup[rgb[z]]){
					default:
					case(0):
						#if RGBFILTER_SHOW_INPUT
							#if (RGBFILTER_USE_NORMALIZED_RGB && SHOW_INPUT_AS_NORMALIZED)
							r = ((rgb[z]>>16)&0xFF);
							g = ((rgb[z]>>8)&0xFF);
							b = (rgb[z] & 0xFF);
							div = 1+(r+g+b);
							r = 255*r/div;
							g =255* g/div;
							b = 255*b/div;
							(*((int*)(outputdata_ptr+z*3))) = (b<<16) | (g<<8) | r;
							#elif (RGBFILTER_USE_NORMALIZED_RG && SHOW_INPUT_AS_NORMALIZED)
							r = ((rgb[z]>>16)&0xFF);
							g = ((rgb[z]>>8)&0xFF);
							b = (rgb[z] & 0xFF);
							div = 1+(r+g);
							
							r = 255*r/div;
							g = 255*g/div;
							b = 255*b/div;
							#ifdef RGBFILTER_USE_NORMALIZED_CUT_OFF_MINSUM
								if (div < RGBFILTER_USE_NORMALIZED_CUT_OFF_MINSUM){
									r=0; g=0; b=0;
								}
							#endif
							(*((int*)(outputdata_ptr+z*3))) = (b<<16) | (g<<8) | r;
							#else
							(*((int*)(outputdata_ptr+z*3))) = rgb[z]; //(*((int*)imagedata_ptr+z*3));
							#endif
						#endif
						break;

					case(1):
						//red
						(*((int*)(outputdata_ptr+z*3))) = 0xFF0000;
						*(pixelfilter_result_ptr+z) = 128;
						break;

					case(2):
						//green
						(*((int*)(outputdata_ptr+z*3))) = 0x00FF00;
						*(pixelfilter_result_ptr+z) = 64;
						break;

					case(3):
						//blue
						(*((int*)(outputdata_ptr+z*3))) = 0x0000FF;
						*(pixelfilter_result_ptr+z) = 32;
						break;
				}
				
#endif
			}

			imagedata_ptr += 48;
			outputdata_ptr += 48;
			pixelfilter_result_ptr += 16;
		}
	}
}