/*
* 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 "Marker.h"
#define DISABLE_KALMAN 0

Marker::Marker(){
	set(0,0,0,0);
	set_kalman_coefficients(0.01, 0.1, 0.1);
	
}

Marker::Marker(double x, double y, double w, double h){
	set(x,y,w,h); 
	set_kalman_coefficients(0.01, 0.1, 0.1);
}

void Marker::set_kalman_coefficients(double p, double m, double e){
	options_kalman_process_noise_cov = p;
	options_kalman_measurement_noise_cov = m;
	options_kalman_error_cov = e;
	
	setIdentity(kalman_filter->processNoiseCov, Scalar::all(options_kalman_process_noise_cov));
	setIdentity(kalman_filter->measurementNoiseCov, Scalar::all(options_kalman_measurement_noise_cov));
	setIdentity(kalman_filter->errorCovPost, Scalar::all(options_kalman_error_cov));
}

Marker::Marker(FastBlob blob){ 
	set(blob.get_centeroid_x(),blob.get_centeroid_y(),blob.get_width(),blob.get_height()); 
}

double Marker::distance(double x, double y, Marker m){ 
	return sqrt((m.center_x - x)*(m.center_x - x) + (m.center_y - y)*(m.center_y - y)); 
}


bool Marker::has_multiple_detections(){
	return found_counter > 300;
}

void Marker::set_fixated_to(double x, double y){
	fixated_x = x;
	fixated_y = y;
}

void Marker::add_position_to_avg(double x, double y){ 
	sum_x+=x;
	sum_y+=y;
	sum_count++; 
}

double Marker::distance_to_avg(Marker m){ 
	double avg_x = ((double) sum_x)/(double)sum_count;
	double avg_y = ((double) sum_y)/(double)sum_count;
	return distance(avg_x, avg_y, m);
}
	
Marker Marker::center_to(Marker b){ 
	return Marker((center_x+b.center_x)/2.0, (center_y+b.center_y)/2.0, (width+b.width)/2.0, (height+b.height)/2.0); 
}

double Marker::distance_to(Marker b){ 
	return distance(center_x, center_y, b); 
}
	
bool Marker::has_smaller_x_coord_than(Marker b){ 
	return center_x < b.center_x; 
}

bool Marker::is_above(Marker b){ 
	return has_smaller_x_coord_than(b); 
}
	
void Marker::update_position(Marker m){
	//copy new position from marker
	// First predict, to update the internal statePre variable
	Mat prediction = kalman_filter->predict();
	Point2d predictPt(prediction.at<float>(0),prediction.at<float>(1));
	
	Mat_<float> measurement(2,1);
	measurement.setTo(Scalar(0));
	measurement(0) = m.center_x;
	measurement(1) = m.center_y;
	
	//the "correct" phase that is going to use the predicted value and our measurement
	Mat estimated = kalman_filter->correct(measurement);
	
	center_x = estimated.at<float>(0);
	center_y = estimated.at<float>(1);
	width  = m.width;
	height = m.height;
	
	#if DISABLE_KALMAN
	center_x = m.center_x;
	center_y = m.center_y;
	#endif
	
	found = true;
	if (found_counter < 1000) found_counter++;
}

void Marker::set(double x, double y, double w, double h){
	center_x = x; center_y = y; width = w; height = h; 
	kalman_filter = new KalmanFilter(4, 2, 0);
	kalman_filter->transitionMatrix = *(Mat_<float>(4, 4) << 1,0,1,0,   0,1,0,1,  0,0,1,0,  0,0,0,1);
	kalman_filter->statePre.at<float>(0) = x;
	kalman_filter->statePre.at<float>(1) = y;
	kalman_filter->statePre.at<float>(2) = 0.0;
	kalman_filter->statePre.at<float>(3) = 0.0;
	setIdentity(kalman_filter->measurementMatrix);
	setIdentity(kalman_filter->processNoiseCov, Scalar::all(options_kalman_process_noise_cov));
	setIdentity(kalman_filter->measurementNoiseCov, Scalar::all(options_kalman_measurement_noise_cov));
	setIdentity(kalman_filter->errorCovPost, Scalar::all(options_kalman_error_cov));
	
	Mat_<float> measurement(2,1);
	measurement.setTo(Scalar(0));
	measurement(0) = x;
	measurement(1) = y;
	kalman_filter->correct(measurement);
	kalman_filter->correct(measurement);
	kalman_filter->correct(measurement);
	
	sum_x = x;
	sum_y = y;
	sum_count = 1;
	found_counter = 0;
	
	translated_angle_x = -1.0;
	translated_angle_y = -1.0;
}