/*
* 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 "FastBlob.h"
// #include "Triplet.hpp"

FastBlob::FastBlob(){
//     sum_x = 0;
//     sum_y = 0;
//     sum_count = 1;
}

FastBlob::~FastBlob(){}

void FastBlob::init(int px, int py){
	sum_x = px;
	sum_y = py;
	
	pixelcount = 1;
	x=px;
	width=1;
	y=py;
	height=1;
	possible_candidate=true;
}

// int FastBlob::distance_to(FastBlob b){
//      int dist_x = (x - b.x);
//      int dist_y = (y - b.y);
//     return sqrt(dist_x*dist_x+dist_y*dist_y);
// }
// /*
// int FastBlob::distance_to(Triplet t){
//      int dist_x = (x - t.center_x);
//      int dist_y = (y - t.center_y);
//     return sqrt(dist_x*dist_x+dist_y*dist_y);
// }*/
// int FastBlob::get_radius(void){
//      return (sqrt((width*height)/3.14));
// }
// 
// void FastBlob::set_rect( int xs,  int ys,  int xe,   int ye){
//     x = xs;
//     y = ys;
//     width = abs(xe-xs);
//     height = abs(ye-ys);
// }
// 
//  int FastBlob::get_boundingboxsize(){
//     return width*height;
// }

void FastBlob::move(double _x, double _y){
	sum_x = (get_centeroid_x()+ _x) * pixelcount;
	sum_y = (get_centeroid_y()+ _y) * pixelcount;
	x = _x;
	y = _y;
}

double FastBlob::get_centeroid_x(){
	return (double)sum_x / (double)pixelcount;
}
double FastBlob::get_centeroid_y(){
	return (double)sum_y / (double)pixelcount;
}

void FastBlob::include_pixel( int px,  int py){
    //expand blob so that we include the given pixel
//      int diff=0;

	sum_x += px;
	sum_y += py;
// 	sum_count++;

	pixelcount++;
	
	boundingbox_update(px, py);
}

void FastBlob::boundingbox_update(int px, int py){
     if (px < x){ width=width+x-px; x=px; }else if (px > (x+width)){ width=px-x; }
     if (py < y){ height=height+y-py; y=py; }else if (py > (y+height)){ height=py-y; }
}


int FastBlob::get_boundingboxsize(){
    return width*height;
}

void FastBlob::merge_with_blob(FastBlob newblob){
	//merge center of gravity! 
	//scale factors: the sum_count of both:
	double pixelcount_sum = ((double)get_pixelcount() + newblob.get_pixelcount());
	double factor_this    = (double)get_pixelcount() / pixelcount_sum;
	double factor_newblob = (double)newblob.get_pixelcount() / pixelcount_sum;
	
	double pos_x = get_centeroid_x() * factor_this + newblob.get_centeroid_x()* factor_newblob;
	double pos_y = get_centeroid_y() * factor_this + newblob.get_centeroid_y()* factor_newblob;
	
	//update boundingbox to include blob
	boundingbox_update(newblob.get_x(), newblob.get_y());
	boundingbox_update(newblob.get_x()+newblob.get_width(), newblob.get_y()+newblob.get_height());
	
	//set x,y
// 	x = pos_x;
// 	y = pos_y;
	sum_x = pixelcount_sum * pos_x;
	sum_y = pixelcount_sum * pos_y;
	pixelcount = pixelcount_sum;
	
	
}

//  int FastBlob::get_centeroid_x(){
//     return sum_x/sum_count;
// }
// 
//  int FastBlob::get_centeroid_y(){
//     return sum_y/sum_count;
// }
/*
 int FastBlob::get_bbox_center_x(){
    return x+width/2;
}

 int FastBlob::get_bbox_center_y(){
    return y+height/2;
}

 int FastBlob::get_searchradius_min(){
    return 0.1*get_radius();
}
 
 int FastBlob::get_searchradius_max(){
    return 2*2.0*get_radius();
}

 int FastBlob::is_valid_clustering_partner(FastBlob bl2){
    //now check distances
    if ((distance_to(bl2) < get_searchradius_max()) && (distance_to(bl2) > get_searchradius_min())){
        return 1;
    }else{
        return 0;
    }
}*/