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

#define STEPSIZE 2

BlobFinder::BlobFinder(){
	uninitialised = true;
	visited_matrix_size = 0;
	visited_matrix = NULL;
	visited_matrix_maxid = 0;
	image_width = 0;
	image_height = 0;
	color_lookup = NULL;
	allocated_matrix = false;
	
	#if BLOBFINDER_USE_THREADS
	blobfinder_thread_obj[0] = NULL;
	blobfinder_thread_obj[1] = NULL;
	#endif
}

BlobFinder::~BlobFinder(){
	if (allocated_matrix) 
		free(visited_matrix);
}

void BlobFinder::check_and_allocate(Size size){
	if ((image_width < size.width) || (image_height < size.height)){
		//oh we need more storage!
		printf("> blobfinder: (re)allocating storage for %d x %d image\n",size.width,size.height);
		
		if (allocated_matrix){
			//oh we have to free the old space!
			free(visited_matrix);
		}
		
		image_width = size.width;
		image_height = size.height;
		visited_matrix_size = image_width * image_height;
	
		//request the vector to be able to hold enough elements!
		//-> no reallocation issues during runtime!
		places_to_visit.reserve(visited_matrix_size);
	
		//allocate memory for visited matrix
		visited_matrix = (uchar*) malloc(visited_matrix_size * sizeof(uchar));
		allocated_matrix = true;
		
		uninitialised = false;
	}
}

void BlobFinder::set_lookuptable(int *table){
	color_lookup = table;
}


bool BlobFinder::value_in_range(double value, double min, double max){
	return (value >= min) && (value <= max); 
}


bool BlobFinder::blobs_overlap(FastBlob a, FastBlob b){
	double a_x1 = a.get_centeroid_x() - a.get_width()/2.0;
	double a_x2 = a.get_centeroid_x() + a.get_width()/2.0;
	double a_y1 = a.get_centeroid_y() - a.get_height()/2.0;
	double a_y2 = a.get_centeroid_y() + a.get_height()/2.0;
	
	double b_x1 = b.get_centeroid_x() - b.get_width()/2.0;
	double b_x2 = b.get_centeroid_x() + b.get_width()/2.0;
	double b_y1 = b.get_centeroid_y() - b.get_height()/2.0;
	double b_y2 = b.get_centeroid_y() + b.get_height()/2.0;
	
	return !((a_x2 < b_x1) || (a_y2 < b_y1) || (a_x1 > b_x2) || (a_y1 > b_y2));
}

	/*
	bool x_overlap = value_in_range(a.get_centeroid_x(), b.get_centeroid_x(), b.get_centeroid_x() + b.get_width()); // || value_in_range(b.get_centeroid_x(), a.get_centeroid_x(), a.get_centeroid_x() + a.get_width());
	bool y_overlap = value_in_range(a.get_centeroid_y(), b.get_centeroid_y(), b.get_centeroid_y() + b.get_height()); // || value_in_range(b.get_centeroid_y(), a.get_centeroid_y(), a.get_centeroid_y() + b.get_height());
	return x_overlap && y_overlap;
}*/

void BlobFinder::process_image(Mat *image){
	if (color_lookup == NULL){
		printf("> ERROR: do not use this class without notification setting lookuptable ptr!\n");
		exit(1);
	}
	
	if (!image->isContinuous()){
		printf("> ERROR: input image is not continous -> linear access speedup not possible. exiting\n");
		return; //exit(1);
	}
	
	if ((BLOBFINDER_USE_THREADS) && ((image->size().height * image->size().width) > (200*200))){
		#if BLOBFINDER_USE_THREADS
		//start two threads with overlapping images:
		Mat source_image_half[2];
		source_image_half[0] = (*image)(Rect(0,0,				image->size().width,image->size().height/2+BLOBFINDER_THREADS_OVERLAP_REGION_PX));
		source_image_half[1] = (*image)(Rect(0,image->size().height/2-BLOBFINDER_THREADS_OVERLAP_REGION_PX,	image->size().width,image->size().height/2-1+BLOBFINDER_THREADS_OVERLAP_REGION_PX));
// 		imshow("UPPER", source_image_half[0]);
// 		imshow("LOWER", source_image_half[1]);
// 		waitKey(1);

		//make two copies of this object:
		boost::thread *blobfinder_thread[2];
		for(int i=0; i<2; i++){
			if (blobfinder_thread_obj[i] == NULL) {
				blobfinder_thread_obj[i] = new BlobFinder();
				blobfinder_thread_obj[i]->set_lookuptable(color_lookup);
			}
			blobfinder_thread_obj[i]->check_and_allocate(source_image_half[i].size());
			blobfinder_thread[i] = new boost::thread(boost::bind(&BlobFinder::grow, blobfinder_thread_obj[i] , &source_image_half[i]));
		}
		
		//join & delete threads
		for(int i=0; i<2; i++){
			blobfinder_thread[i]->join();
			delete blobfinder_thread[i];
		}
		
		//ok, now we have all blobs in the two objects
		//next is merging of all blobs into one list:
		found_objects = blobfinder_thread_obj[0]->get_blob_vector(-1);
		blob_vector_t found_objects_2 = blobfinder_thread_obj[1]->get_blob_vector(-1);
		
		//merge into on list:
		for (int i = 0; i < (int)found_objects_2.size(); i++){
			FastBlob b = found_objects_2[i];
			//we have to move this blob to second image half:
			b.move(0.0, image->size().height/2-BLOBFINDER_THREADS_OVERLAP_REGION_PX);
			
			found_objects.push_back(b);
		}
		
		//filter out same objects:
		remove_overlapping_regions();
		#endif
	}else{
		//check if we allocated enough space (or realloc)
		check_and_allocate(image->size());
		grow(image);
	}
}

blob_vector_t BlobFinder::get_blob_vector(int colorclass){
// 	printf("> returning %d blobs\n",found_objects.size());
	if (colorclass == -1){
		//return all
		return found_objects;
	}else{
		blob_vector_t found_objects_by_color;
		for (int i = 0; i < (int)found_objects.size(); i++){
			if (found_objects[i].id == colorclass){
				found_objects_by_color.push_back(found_objects[i]);
			}
		}
		return found_objects_by_color;
	}
}

void BlobFinder::overlay_text_on_image(Mat *image){
}

void BlobFinder::overlay_blobs_on_image(Mat *image, int colorclass){
	//show blobs
// 	printf("P %d\n",colorclass);
	for (int i = 0; i < (int)found_objects.size(); i++){
		FastBlob bl = found_objects[i];
		if (bl.id == colorclass){
			
// 			printf("> blob at %f %f\n",bl.get_centeroid_x(),bl.get_centeroid_y());
			//paint it!
	// 		rectangle(*image, Point(bl.get_x(),bl.get_y()), Point(bl.get_x()+bl.get_width(),bl.get_y()+bl.get_height()), CV_RGB(255,255,0), 1);
			
			line(*image, Point(bl.get_centeroid_x() - 10,bl.get_centeroid_y()), Point(bl.get_centeroid_x() + 10,bl.get_centeroid_y()), CV_RGB(0,255,255), 1);
			line(*image, Point(bl.get_centeroid_x(),bl.get_centeroid_y() -10), Point(bl.get_centeroid_x(),bl.get_centeroid_y() + 10), CV_RGB(0,255,255), 1);
			
// 			rectangle(*image, Point2d(bl.get_centeroid_x() - bl.get_width()/2,bl.get_centeroid_y() -bl.get_height()/2), Point2d(bl.get_centeroid_x() + bl.get_width()/2,bl.get_centeroid_y() +bl.get_height()/2), CV_RGB(255,255,0), 1);
			
			//draw triplet id
			//char buf[100];
	 		//snprintf(buf,100,"%d",bl.object_id);
			//putText(*image, buf, Point2d(bl.get_centeroid_x() + bl.get_width() / 2, bl.get_centeroid_y() + bl.get_height() / 2), FONT_HERSHEY_SIMPLEX, .5, CV_RGB(255,255, 255), 1, CV_AA);
	 		
		}
	}
}


// #if 1
void BlobFinder::grow(Mat *image){
	int runid = 1;
	int pos = 0;
	int color;
	int color2;

	//setup data ptr
// 	image_data_ptr = imgdata;

	//we use a vector to store the elements we have to process
	places_to_visit.clear();
	
	//clear visited array
	memset(visited_matrix,0, visited_matrix_size);
	
	//delete objects
	found_objects.clear();

	int countx = 0;
	
	//copy ptr to cur image 
	current_image = image;
	
	uchar *image_data_ptr = current_image->ptr(0);
	uchar *image_data_ptr_now = image_data_ptr;
	
	int image_width  = current_image->size().width;
	int image_height = current_image->size().height;
	int image_size   = image_width * image_height;
	int rgb1, rgb2;

	for (int startpos = 0; startpos < image_size - STEPSIZE; startpos += STEPSIZE){
		image_data_ptr_now += 3 * STEPSIZE;
		
		//cycle through every n-th pixel col/row
		if (countx == image_width / STEPSIZE){
			countx = 0;
			startpos += (STEPSIZE - 1) * image_width;
		}
		if (visited_matrix[startpos] == 0){
			//new blob?!
			rgb1 = (*((int*)(image_data_ptr_now))) & 0xFFFFFF;
			rgb2 = (*((int*)(image_data_ptr_now+3*(STEPSIZE/2))))& 0xFFFFFF;
// 			printf("access 0x%08X 0x%08X (img at 0x%08X)\n",image_data_ptr_now,image_data_ptr_now+3*(STEPSIZE/2),image_data_ptr);
			color = color_lookup[rgb1];
			color2 = color_lookup[rgb2];
			
// 			(*((int*)(image_data_ptr_now))) = 0xFFffff;

			//two following pixels are of the same color (and not black)
			if ((color != 0) && (color == color2)){
// 				*(image_data_ptr_now) = 255;
	
// 				printf(".");
				//not bg -> process!
				//init current blob:
				current_blob.init(startpos % image_width, startpos / image_width);
// 				current_blob.set_rect(startpos % image_width, startpos / image_width, startpos % image_width, startpos / image_width);
				current_blob.id = color;
				current_blob.runid = runid;
// 				current_blob.pixelcount = 0;
// 
				//let the blob grow
				fill(startpos, color);

				//update runid
				runid++;

				//push blob to vector
				if (found_objects.size() < 3000){
					current_blob.object_id = 0;
// 					printf("count %d\n",current_blob.get_pixelcount());
					if ((current_blob.get_pixelcount() > 20) 
// 							&& (current_blob.pixelcount > current_blob.get_boundingboxsize() / 5)
// 							&& (current_blob.get_boundingboxsize() > 30)
							){ // && (current_blob.get_boundingboxsize() < 20000)){
						found_objects.push_back(current_blob);
					}
				}
			}
		}
	}

	visited_matrix_maxid = runid;
	
	//remove markers that overlay others:
	remove_overlapping_regions();
}

void BlobFinder::remove_overlapping_regions(){

for (blob_vector_t::iterator it1=found_objects.begin(); it1<found_objects.end(); ){
		bool to_be_removed = false;
		int min_x = it1->get_x();
		int max_x = min_x + it1->get_width();
		int min_y = it1->get_y();
		int max_y = min_y + it1->get_height();
		
		for (blob_vector_t::iterator it2=found_objects.begin(); it2<found_objects.end(); it2++){
			if (it1 != it2){
				if (it1->id == it2->id){
					//check if this objects center of gravity is inside the other one:
					int cog_x = it2->get_centeroid_x();
					int cog_y = it2->get_centeroid_y();
					if ((((cog_x > min_x) && (cog_x < max_x)) && ((cog_y > min_y) && (cog_y < max_y))) || (blobs_overlap(*it1, *it2))){
						//inside of it1!
						if (it2->get_boundingboxsize() >= it1->get_boundingboxsize()){
							//it2 is the bigger one
							//->merge with blob1
							it1->merge_with_blob(*it2);
							//and mark it1 for removal
							to_be_removed = true;
						}
					}
				}
			}
		}
		if (to_be_removed){
			it1 = found_objects.erase(it1);
		}else{
			it1++;
		}
	}
/*
printf("ID %d\n",found_objects[0].id);

	found_objects.clear();
	FastBlob *a = new FastBlob();
	a->id = 1;
	a->object_id = 1;
	a->init(50,50);
	a->include_pixel(50,50);
	a->include_pixel(100,100);
	found_objects.push_back(*a);
	
	FastBlob *b = new FastBlob();
	b->id = 1;
	b->object_id = 2;
	b->init(60,60);
	b->include_pixel(55,55);
	b->include_pixel(66,66);
	found_objects.push_back(*b);
	
	blob_vector_t new_list;
	
	for (blob_vector_t::iterator it1=found_objects.begin(); it1<found_objects.end(); ){
		bool add = true;
		
		for (blob_vector_t::iterator it2=found_objects.begin(); it2<found_objects.end(); it2++){
			if (it1 != it2){
				if (it1->id == it2->id){
					printf("checking %d in %d? = ",it1->object_id,it2->object_id);
					if (blobs_overlap(*it1, *it2) ){
						printf(" is inside!\n");
						//inside of it1!
						//->merge with blob1
						it1->merge_with_blob(*it2);
						printf("merged -> pos = %f %f (w=%d, h=%d)\n",it1->get_centeroid_x(),it1->get_centeroid_y(),it1->get_width(), it1->get_height());
						//and mark it1 for removal
						add = false;
					}else{
						printf(" NOT\n");
					}
				}
			}
		}
		if (add) new_list.push_back(*it1);
		it1++;
	}*/
// 	found_objects = new_list;
	
}

void BlobFinder::fill(int startpos, int color){
	int pos = 0;
	
	//add startpixel
	places_to_visit.push_back(startpos);
	
	//get ptr to imagedata
	uchar *image_data_ptr = current_image->ptr(0);
	int image_width  = current_image->size().width;
	int image_height = current_image->size().height;
	int image_size   = image_width * image_height;
	
	
	
	//do main loop:
	while (!places_to_visit.empty()){
		//fetch next element
		pos = places_to_visit.back();
		places_to_visit.pop_back();

		if (visited_matrix[pos] == 0){
			//check if this pixel has the right colorclass
			if (color == color_lookup[(*((int*)(image_data_ptr + 3*pos))) & 0xFFFFFF]){
// 				*(image_data_ptr+3*pos) = 255;
				
				//yes, interesting pixel
				//update blobdata:
				current_blob.include_pixel(pos % image_width, pos / image_width);
// 				current_blob.pixelcount++;

				/// -> add 8neighbours
				///LEFT x-1
				if (pos > 1){
					places_to_visit.push_back(pos - 1);
				}
				///UPPER y-1
				if (pos > image_width){
					places_to_visit.push_back(pos - image_width);
				}
				///RIGHT x+1
				if (pos < image_width * image_height){
					places_to_visit.push_back(pos + 1);
				}
				///LOWER y+1
				if (pos < image_width * image_height - image_width){
					places_to_visit.push_back(pos + image_width);
				}
				///UPPER_RIGHT x+1 y-1
				if (pos > (image_width - 1)){
					places_to_visit.push_back(pos - (image_width - 1));
				}
				///UPPER_LEFT x-1 y-1
				if (pos > (image_width + 1)){
					places_to_visit.push_back(pos - (image_width + 1));
				}
				///LOWER_RIGHT x+1 +1
				if (pos < image_width * image_height - (image_width + 1)){
					places_to_visit.push_back(pos + (image_width + 1));
				}
				///LOWER_LEFT x-1 y+1
				if (pos < image_width * image_height - (image_width - 1)){
					places_to_visit.push_back(pos + (image_width - 1));
				}

				//mark as visited
				visited_matrix[pos] = color;
			}
		}
	}
}
// #endif