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

#ifndef Marker_H
#define Marker_H

#include <math.h>
#include <stdlib.h>
#include "BlobFinder.h"

class Marker{
public:
	Marker();
	Marker(double x, double y, double w, double h);
	Marker(FastBlob blob);
	
	double center_x;
	double center_y;
	double width;
	double height;
	
	double fixated_x;
	double fixated_y;
	
	static double distance(double x, double y, Marker m);
	long sum_x;
	long sum_y;
	long sum_count;
	bool has_multiple_detections();
	void add_position_to_avg(double x, double y);
	double distance_to_avg(Marker m);
	
	//operator Point2d() { Point2d p; p.x = center_x; p.y = center_y; return p; };
	//operator Point() { Point p; p.x = center_x; p.y = center_y; return p; };
	operator Point2d() { return to_point2d(); };
	operator Point() { Point p; p.x = center_x; p.y = center_y; return p; };
// 	
	Point2d to_point2d(){ Point2d p; p.x = center_x; p.y = center_y; return p; }
	Point2d to_fixated_point2d(){ Point2d p; p.x = fixated_x; p.y = fixated_y; return p; }
	
	Marker center_to(Marker b);
	double distance_to(Marker b);
	
	void set_fixated_to(double x, double y);
		
	bool has_smaller_x_coord_than(Marker b);
	bool is_above(Marker b);
	
	void update_position(Marker m);
	
	double translated_angle_x;
	double translated_angle_y;

	bool found;
	void set_kalman_coefficients(double, double, double);
private:
	int found_counter;
	
	double options_kalman_process_noise_cov;
	double options_kalman_measurement_noise_cov;
	double options_kalman_error_cov;
	
	void set(double x, double y, double w, double h);
	KalmanFilter *kalman_filter; 

};

typedef vector<Marker> marker_vector_t;
#endif