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

#include <deque>
#include "Tracker.h"
#include "BallKFPos.h"

using std::deque;

class MultiBallKFPos : public Tracker {

  int nr_states;

  /// A deque of simple position Kalman Filters
  deque<BallKFPos> tracks;

  /// Cache the last P so we can create new trackers
  Matrix P;

  /// Remove old tracks from the list
  void prune();

  bool closestDistance(Matrix __z, 
		       double & min_dist,
		       int & min_idx);
    
 public:

  /// Constructor
  MultiBallKFPos(ulong timestamp,
		 int nstates,
		 Matrix __x,
		 Matrix __P);

  /// Destructor
  virtual ~MultiBallKFPos();

  //------------------------------------------------------------------------
  /// Propagate the kinematics model
  void propagate(ulong timestamp);

  /// Set the propagation noise
  void setProcessNoise(Matrix __Q);

  //------------------------------------------------------------------------
  /// Add an observation to the filter
  void observe(ulong timestamp,
	       Matrix __z, 
	       Matrix __Q,
	       Matrix __R,
	       bool imp_filter);

  //------------------------------------------------------------------------
  // These functions allow us to "kick" the mean and covariance of the state
  /// Perturb the estimate
  void perturb(ulong timestamp,
	       Matrix __x,
	       int hypothesis,
	       bool clone);

  void erase(int hypothesis);

  //------------------------------------------------------------------------
  /// Reset the tracker
  void reset(ulong timestamp,
	     Matrix __x,
	     Matrix __P);

  //------------------------------------------------------------------------
  /// Number of hypothesis
  int numHypotheses();

  //------------------------------------------------------------------------
  /// Specify a return data type of some kind by name
  bool getState(Matrix & val, 
		const string & param, 
		int hypothesis);

  bool getLikelihood(double & val,int hypothesis);

  bool getTime(double & t,
	       int hypothesis);

  bool getObservationTime(double & t,
			  int hypothesis);

  bool getIDFromHypothesis(ulong & ID, 
			   int hypothesis);

  bool getHypothesisFromID(int & hypothesis, 
			   ulong ID);

  bool getStateByID(Matrix & val,
		    const string & param,
		    ulong ID);

  bool getTimeByID(double & t, 
		   ulong ID);

  bool getObservationTimeByID(double & t, 
			      ulong ID);
};

#endif