/* 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.
  ========================================================================= */
#ifdef PLATFORM_LINUX
#include <iostream>
using namespace std;
#endif

#include "../headers/Utility.h"
#include "../headers/Reporting.h"
#include "BallKFPos.h"

static const bool BKFDebug = false;

static ulong ID_COUNTER = 0;

//-----------------------------------------------------------------------
BallKFPos::BallKFPos(ulong timestamp,
		     int nstates,
		     Matrix __x,
		     Matrix __P) : I(nstates) {
  
  nr_states = nstates;
  reset(timestamp,__x,__P);
  my_id=ID_COUNTER++;
}

//-----------------------------------------------------------------------

void BallKFPos::reset(ulong timestamp,
		      Matrix __x,
		      Matrix __P) {

  // Set the state
  x.resize(nr_states,1);
  x=__x;

  // Set the covariance
  P.resize(nr_states);
  P=__P;
  
  Q.identity(nr_states);

  // Set the time
  observation_time=time=TimeToSec(timestamp);
}

int BallKFPos::numHypotheses() {
  return 1;
}

//-----------------------------------------------------------------------
void BallKFPos::propagate(ulong timestamp) {
  double stime = TimeToSec(timestamp);
  if (stime <= time) { return; }
  Q.scale(stime-time);
  P=P+Q;
  time=stime;
}

void BallKFPos::setProcessNoise(Matrix __Q) {
  Q=__Q;
}

//-----------------------------------------------------------------------
void BallKFPos::observe(ulong timestamp, 
			Matrix __z,
			Matrix __Q,
     			Matrix __R,
			bool imp_filter) {

  // Observation is in local coordinates
  double stime = TimeToSec(timestamp);

  // Propagate with the specified process noise
  Q=__Q;
  propagate(timestamp);

  // Don't accept observations that are in the past
  // But do accept it if stime=time (set by propagate above)
  if (stime < time) { return; }
 
  // Do improbability filtering here
  if (imp_filter) {
    double val = mahalanobis(__z,__R);
    if (3 < val) {
      if (BKFDebug) {
#ifdef PLATFORM_LINUX
	cerr << "BKF: Mahalanobis distance exceeded: " << val << endl;
#endif
#ifdef PLATFORM_APERIOS
	pprintf(TextOutputStream,"BKF: Mahalanobis distance exceeded: %lf\n",val);
#endif
      }
      return;
    }
  }

  Matrix K;
  K.resize(nr_states);
  //  Matrix K = P * (_H)^T * 
  //    inverse(_H * P * (_H)^T + _R);
  // for static world
  // H = I so ->  K = P * inv(P + R);
  // K <- P * Ht * (H * P * Ht + R)^-1
  K=P;
  K=K+__R;
  K.inverse();
  K=P*K;
  
  // State update = K * (z - h(x))
  __z = __z-x;
  x = x+(K*__z);

  // Update covariance matrix
  // P = (I - K * _H) * P = (I - K) * P = P - K*P
  P = (I-K)*P;

  observation_time=stime;
}

//-----------------------------------------------------------------------
void BallKFPos::perturb(ulong timestamp,
			Matrix __x,
			int hypothesis,
			bool clone) {
  x=__x;
  time = TimeToSec(timestamp);
  my_id=ID_COUNTER++;
}

void BallKFPos::erase(int hypothesis) {}

void BallKFPos::scaleCov(double val) {
  P.scale(val);
}

//-----------------------------------------------------------------------

// Likelihood of the current tracker state
bool BallKFPos::getLikelihood(double & val,int hypothesis) {
  // return a number proportional (maintaining monotonic
  // order) of estimate of how good the tracker is doing
  val = (1.0 / pow(2*M_PI, nr_states/2)/ sqrt(P.determinant()));
  return true;
}

// Likelihood of the observation (before it goes into the state)
double BallKFPos::obsLikelihood(Matrix __z) {
  Matrix D(nr_states,1);
  for (int i=0;i<nr_states;i++) {
    D.e(i) = __z.e(i) - x.e(i);
  }
  
  double likelihood = 1.0;
  for(int i = 0; i < D.nrows(); i++) {
    likelihood *= exp(-sq(D.e(i)) / (2 * P.e(i, i)));
  }
  return likelihood;
}

//-----------------------------------------------------------------------

// Compute the mahalanobis distance of the observation
// Assume the state vector is one-dimensional
double BallKFPos::mahalanobis(Matrix __z,
			      Matrix __R) {

  Matrix S=P;
  S=S+__R;
  S.inverse();

  Matrix res(nr_states,1);
  Matrix res_t(1,nr_states);
  
  for (int i=0;i<nr_states;i++) {
    res.e(i,0)=res_t.e(0,i)=(__z.e(i,0)-x.e(i,0));
  }

  Matrix out = res_t*S*res;
  return out.e(0,0);
}

//-----------------------------------------------------------------------

// Predict the state with a delta time
Matrix BallKFPos::predict(ulong dt) {
  return x;
}

// Predict the covariance with a delta time
Matrix BallKFPos::predictCovariances(ulong dt,
				     Matrix __Q) {
  double stime = TimeToSec(dt);
  __Q.scale(stime);
  return P+__Q;;
}

//-----------------------------------------------------------------------
// Specify a return data type of some kind by name
bool BallKFPos::getState(Matrix & val, 
			 const string & param,
			 int hypothesis) {
  if (string("state")==param) {
    val = x;
    return true;
  } else if (string("covariance")==param) {
    val = P;
    return true;
  }
  if (BKFDebug) 
#ifdef PLATFORM_APERIOS
  pprintf(TextOutputStream,"BallKFPos::getState() no such param %s\n",param.c_str());
#endif
  return false;
}

// Return the latest time
bool BallKFPos::getTime(double & t, 
			int hypothesis) { 
  if (0==hypothesis) {
    t=time; 
    return true;
  } 
  return false;
}

bool BallKFPos::getObservationTime(double & t,
				   int hypothesis) {
  if (0==hypothesis) {
    t = observation_time;
    return true;
  } 
  return false;
}

bool BallKFPos::getIDFromHypothesis(ulong & ID, 
				    int hypothesis) {
  if (0==hypothesis) {
    ID=my_id;
    return true;
  }
  return false;
}

bool BallKFPos::getHypothesisFromID(int & hypothesis, 
				    ulong ID) {
  if (ID==my_id) {
    hypothesis = 0;
    return true;
  }
  return false;
}

bool BallKFPos::getStateByID(Matrix & val,
			     const string & param,
			     ulong ID) {
  if (ID == my_id) {
    return getState(val,param,0);
  } else {
    return false;
  }
}

bool BallKFPos::getTimeByID(double & t,
			    ulong ID) {
  if (ID == my_id) {
    return getTime(t,0);
  } else {
    return false;
  }
}

bool BallKFPos::getObservationTimeByID(double & t,
				       ulong ID) {
  if (ID == my_id) {
    return getObservationTime(t,0);
  } else {
    return false;
  }
}