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

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

/*
 *
 * This is an attempt to track the position and the velocity of the ball.
 *
 * Currently, this does not work as the AIBO's ball estimates are simply
 * too noisy.  For this to work, one possibilty would be to encorporate
 * a line fitting technique from the world model instead.
 * 
 */


class BallEKFPosVel : private Kalman,
		      public Tracker {

 protected:
  bool _reset;
  vector2d added_vel;

  // Define the specific functions for the Kalman Filter
  virtual Matrix& f(const Matrix &x, Matrix &I); // noiseless dynamics
  virtual Matrix& h(const Matrix &x); // noiseless observation

  virtual Matrix& Q(const Matrix &x); // Covariance of propagation noise
  virtual Matrix& R(const Matrix &x); // Covariance of observation noise

  virtual Matrix& A(const Matrix &x); // Jacobian of f w.r.t. x
  virtual Matrix& W(const Matrix &x); // Jacobian of f w.r.t. noise
  virtual Matrix& H(const Matrix &x); // Jacobian of h w.r.t. x
  virtual Matrix& V(const Matrix &x); // Jacobian of h w.r.t. noise

 public:
  /// Constructor
  BallEKFPosVel();

  /// Destructor
  virtual ~BallEKFPosVel() {}

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

  /// Add an observation to the tracker.  AIBO time in unsigned long
  void observe(vector2d obs, 
	       double confidence,
	       ulong timestamp,
	       bool imp_filter=false);

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

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

  /// Return the number of hypotheses
  int numHypotheses();

  /// Return the estimated ball position
  Gaussian2 position(ulong dt=0);
  /// Return the estimated ball velocity
  Gaussian2 velocity(ulong dt=0);

  /// Add velocity to the current state (should also add a covariance est)
  void addVelocity(vector2d v);

  bool getState(Matrix & val,const string & param, int idx=0,ulong dt=0);
  bool getState(Gaussian2 & val,const string & param, int idx=0,ulong dt=0);
  bool getState(vector2d & val,const string & param, int idx=0,ulong dt=0);
};

#endif