/* 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 _TEAMMATE_BALL_TRACKER_H_
#define _TEAMMATE_BALL_TRACKER_H_

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

/*
 * This class takes ball observations from other robots and 
 * keeps track of them.
 *
 * One notion is that if two observations are too close to one another,
 * they could be merged into a single observation.
 *
 */

class TeammateBallTracker {

  static const int nr_states = 2;

 public:
  enum tracker_type { tPOS,tMULTIPOS };
  
  // OBS = observed
  // UNOBS = un observed
  enum ball_state { sOBS,sUNOBS };

 protected:

  Matrix Q;

  Tracker * tracker;
  bool nullTracker();

  void prune();

 public:
  TeammateBallTracker(tracker_type t);

  ~TeammateBallTracker();

  void propagate(ulong timestamp);

  void observe(ulong timestamp,
	       Gaussian2 obs,
	       bool imp_filter);

  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