/* LICENSE:
  =========================================================================
    CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7
    Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */

#ifndef _BALL_TRACKER_H_
#define _BALL_TRACKER_H_

#include "../headers/Geometry.h"
#include "../headers/Utility.h"
#include "../headers/Gaussian2.h"
#include "Tracker.h"

/*
 *
 * The Ball Tracker is a front end interface to potentially several
 * different kinds of ball tracking interfaces.  This is an open area
 * of research and as such, there are potentially several different 
 * ways that can and should be explored
 *
 */


class BallTracker {
 
  static const int nr_states = 2;

 public:
  enum tracker_type { tPOS,tMULTIPOS };
  
 protected:

  Matrix R;
  Matrix Q_obs;
  Matrix Q_unobs;

  // A generic tracker type
  Tracker * tracker;

  /// Check for an uninitialized tracker and display an error if so
  bool nullTracker();

  /// Prune hypotheses
  void prune();

  bool closestDistance(vector2d obs,
		       double & min_dist,
		       int & min_idx);

 public:
  /// Constructor
  BallTracker(tracker_type t);

  /// Destructor
  ~BallTracker();

  /// Propagate the kalman filter to the input timestep
  void propagate(ulong timestamp);

  /// Add an observation to the tracker.  AIBO time in unsigned long
  void observe(ulong timestamp,
	       vector2d obs, 
	       vector2d vel,
	       double confidence,  // vision confidence
	       double distance,    // ball distance from robot
	       double head_tilt, // velocity
	       double head_pan, // velocity
	       bool imp_filter);

  /// Perturb the tracker with a new position
  void perturb(ulong timestamp,
	       vector2d pos,
	       int idx);

  /// Reset the tracker
  void reset(ulong timestamp,
	     vector2d pos);

  /// Reset the tracker
  void reset(ulong timestamp,
	     Gaussian2 pos);

  /// Return the estimated ball position
  bool getPosition(Gaussian2 & pos,
		   int hypothesis);
  
  /// Return the estimated ball position
  bool getPositionFromID(Gaussian2 & pos,
			 ulong ID);

  bool getObservationTime(ulong & t,
			  int hypothesis);

  bool getObservationTimeFromID(ulong & t,
				ulong ID);

  // How many hypothesis are we tracking?
  int numHypotheses();

  /// Return the ID of the specified hypothesis
  bool getIDFromHypothesis(ulong & ID, int hypothesis);

  /// Return the hypothesis of the specified ID
  bool getHypothesisFromID(int & hypothesis, ulong ID);
};

#endif