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

#include "Tracker.h" 

// Kalman filter which tracks the ball's position assuming a static
// position estimate that is corrupted by process noise
//
// This is borrowed from the Segway ball tracker

class BallKFPos : public Tracker {

 private:
  /// Current time
  double time;

  /// Time of the last observation
  double observation_time;

  /// Size of the state vector
  int nr_states;

  /// State and covariance
  Matrix x, P;
  // 2x2 identity matrix
  Matrix I;
  // Process noise
  Matrix Q;

  ulong my_id;

 public:

  //------------------------------------------------------------------------
  /// Parameterized constructor
  BallKFPos(ulong timestamp,
	    int nstates,
	    Matrix __x,
	    Matrix __P);

  /// Destructor
  virtual ~BallKFPos() {}

  //------------------------------------------------------------------------
  /// Propagate the kinematics model using the cached process noise
  void propagate(ulong timestamp);

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

  //------------------------------------------------------------------------
  /// Add an observation to the model
  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);

  /// This doesn't do anything for the single tracker case
  void erase(int hypothesis);

  /// Add some percentage of the covariance matrix to itself
  void scaleCov(double val);

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

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

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

  /// Return the latest time
  bool getTime(double & t, 
	       int hypothesis);

  /// Return the latest observation time
  bool getObservationTime(double & t, 
			  int hypothesis);

  /// Return the ID associated with the unique hypothesis
  bool getIDFromHypothesis(ulong & ID, 
			   int hypothesis);

  /// Return the hypothesis associated with the unique ID
  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);
  
  //------------------------------------------------------------------------
  /// Get the current likelihood of the tracker
  bool getLikelihood(double & val, int hypothesis);

  /// Dummy storage for caching the likelihood
  double cached_likelihood;

  /// Get the likelihood of the observation given the current tracker
  double obsLikelihood(Matrix __z);

  /// Return the mahalanobis distance of observation w.r.t the current state
  double mahalanobis(Matrix __z,
		     Matrix __R);

  //-----------------------------------------------------------------------
  /// Predict the state with a delta timestamp
  Matrix predict(ulong dt);
  
  /// Predict the covariance with a delta timestamp
  Matrix predictCovariances(ulong dt);

  /// Predict the covariance with a delta timestamp
  Matrix predictCovariances(ulong dt,
			    Matrix __Q);

};

#endif