/* 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.
  ========================================================================= */
#include "MultiBallKFPos.h"
#include "../headers/Reporting.h"

static const bool MBKFDebug = false;

static const unsigned int MAX_NUMBER_TRACKERS = 5; // Num of hypotheses

//------------------------------------------------------------------------
struct grt_kf {
  bool operator()(const BallKFPos & kf1,const BallKFPos & kf2) {
    return (kf1.cached_likelihood > kf2.cached_likelihood);
  }
};

// Compute the distance from the observation to the closest kalman filter
bool MultiBallKFPos::closestDistance(Matrix __z, 
				     double & min_dist,
				     int & min_idx) {

  if (tracks.empty()) { return false; }
  Matrix old_z;
  tracks[0].getState(old_z,string("state"),0);
  min_dist = sqrt(sq(__z.e(0,0)-old_z.e(0,0)) + 
		  sq(__z.e(1,0)-old_z.e(1,0)));
  min_idx = 0;
  for (uint t=1;t<tracks.size();t++) {
    tracks[t].getState(old_z,string("state"),0);
    double dist=sqrt(sq(__z.e(0,0)-old_z.e(0,0)) + 
		     sq(__z.e(1,0)-old_z.e(1,0)));
    if (dist < min_dist) {
      min_dist = dist;
      min_idx=t;
    }
  }
  return true;
}

//------------------------------------------------------------------------
// Constructor
MultiBallKFPos::MultiBallKFPos(ulong timestamp,
			       int nstates,
			       Matrix __x,
			       Matrix __P) {
  nr_states = nstates;
  P=__P;
}

// Destructor
MultiBallKFPos::~MultiBallKFPos() {
  // Is this necessary?
  tracks.clear();
}

//------------------------------------------------------------------------
// Propagate the kinematics model
void MultiBallKFPos::propagate(ulong timestamp) {

  if (tracks.empty()) { return; }
  for (uint i=0;i<tracks.size();i++) {
    tracks[i].propagate(timestamp);
  }
}

void MultiBallKFPos::setProcessNoise(Matrix __Q) {
  for (uint i=0;i<tracks.size();i++) {
    tracks[i].setProcessNoise(__Q);
  }
}

//------------------------------------------------------------------------
// Add an observation to the filter (also does propagation)
void MultiBallKFPos::observe(ulong timestamp,
			     Matrix __z,
			     Matrix __Q,
			     Matrix __R,
			     bool imp_filter) {

  // Iterate through the list of Kalman filters and determine which
  // one is closest via the mahalanobis distance.
  //
  // If all are greater than 3 (3 std dev), then start a new tracker.
  //
  // If there there are 'n' kalman filters already, then we delete a filter
  // 
  // What does it mean to do improbability filtering, anyway in this
  // sort of a framework?  Is there additional information that we can
  // throw away even though we're doing the multiple hypothesis
  // tracking?

  // The index of the track to update
  int idx=0;

  if (tracks.empty()) {
    tracks.push_back(BallKFPos(timestamp,nr_states,__z,P));
    if (MBKFDebug) {
#ifdef PLATFORM_APERIOS
      ulong ID;
      tracks.front().getIDFromHypothesis(ID,0);
      pprintf(TextOutputStream,"MBKF: Adding the first track ID=%lu\n",ID);
#endif
    }
    idx=0;
  }

#if 1
  //=======================================================================
  //=======================================================================
  //=======================================================================
  // Find the closest track to the observation

  double min_dist;
  int min_idx;
  if (!closestDistance(__z,min_dist,min_idx)) { return; }
  double mdist = tracks[min_idx].mahalanobis(__z,__R);
  if (3 < mdist) {

    if (MBKFDebug) {
#ifdef PLATFORM_APERIOS
      pprintf(TextOutputStream,"MBKF: mahalanobis=%f\n",mdist);
#endif
    }
    
    // Are we at the maximum number of trackers
    if (tracks.size() < MAX_NUMBER_TRACKERS) {
      // Add a new kalman filter to the deque
      tracks.push_back(BallKFPos(timestamp,nr_states,__z,P));
      idx = tracks.size()-1;

      if (MBKFDebug) {
#ifdef PLATFORM_APERIOS
	ulong ID;
	tracks.back().getIDFromHypothesis(ID,0);
	pprintf(TextOutputStream,
		"MBKF: Adding a new track ID=%lu\n",ID);
#endif
      }

    } else {
      // Replace the least likely kalman filter
      tracks.back().reset(timestamp,__z,P);
      idx = tracks.size()-1;

      if (MBKFDebug) {
#ifdef PLATFORM_APERIOS
	ulong ID;
	tracks.back().getIDFromHypothesis(ID,0);
	pprintf(TextOutputStream,"MBKF: Resetting old track ID=%lu\n",ID);
#endif
      }

    }
  } else {
    idx = min_idx;
  }

  //=======================================================================
  //=======================================================================
  //=======================================================================
#else
  //=======================================================================
  //=======================================================================
  //=======================================================================
  // Find the closest match given the existing tracks
  double min_val = tracks[0].mahalanobis(__z,__R);
  double max_val = min_val;
  int min_idx=0;
  int max_idx=0;
  for (uint i=1;i<tracks.size();i++) {
    double val = tracks[i].mahalanobis(__z,__R);
    if (val < min_val) {
      min_val = val;
      min_idx = i;
    }
    if (val > max_val) {
      max_val = val;
      max_idx = i;
    }
  }

  // If it's outside the allowable range, start a new track (or perturb an
  // old one if it's too close..)
  if (3 < min_val) {
#ifdef PLATFORM_APERIOS
    if (MBKFDebug)
      pprintf(TextOutputStream,"MBKF: mahalanobis=%f\n",min_val);
#endif
    
    // Are we at the maximum number of trackers
    if (tracks.size() < MAX_NUMBER_TRACKERS) {
      // Add a new kalman filter to the deque
      tracks.push_back(BallKFPos(timestamp,nr_states,__z,P));
      idx = tracks.size()-1;
      if (MBKFDebug) {
#ifdef PLATFORM_APERIOS
	ulong ID;
	tracks.back().getIDFromHypothesis(ID,0);
	pprintf(TextOutputStream,
		"MBKF: Adding a new track ID=%lu\n",ID);
#endif
      }
    } else {
      // Replace the kalman filter with the largest mahalanobis distance
      tracks[max_idx].reset(timestamp,__z,P);
      idx = max_idx;
      if (MBKFDebug) {
#ifdef PLATFORM_APERIOS
	ulong ID;
	tracks[max_idx].getIDFromHypothesis(ID,0);
	pprintf(TextOutputStream,"MBKF: Resetting old track ID=%lu\n",ID);
#endif
      }
    }
  } else {
    idx = min_idx;
  }
  //=======================================================================
  //=======================================================================
  //=======================================================================
#endif

  // Add the observation to the current track
  // but don't bother with improbability filtering since it's already
  // essentially been taken care of in the above
  tracks[idx].observe(timestamp,__z,__Q,__R,false);

  // Since propagation is taken care of in the observe() method of BallKFPos
  // we need to propagate the other hypotheses since they haven't been
  // touched yet
  for (uint i=0;i<tracks.size();i++) {
    if ((int)i != idx) {
      // Use the old process noise model
      tracks[i].propagate(timestamp);
    }
  }

  // This is kludgy, but the comparison operator for the sort() function
  // needs a constant data structure passed in as a parameter and
  // the likelihood() method cannot be used on a constant data structure
  for (uint i=0;i<tracks.size();i++) {
     tracks[i].getLikelihood(tracks[i].cached_likelihood,0);
  }
  sort(tracks.begin(),tracks.end(),grt_kf());
}

//------------------------------------------------------------------------
// Make a new observation in the new position?
void MultiBallKFPos::perturb(ulong timestamp,
			     Matrix __x,
			     int hypothesis,
			     bool clone) {
  if ((hypothesis < 0) || (hypothesis >= (int)tracks.size())) { return; }

  int idx = hypothesis;
  if (clone) {
    if (tracks.size() < MAX_NUMBER_TRACKERS) {
      // Add a new kalman filter to the deque
      if (MBKFDebug) {
#ifdef PLATFORM_APERIOS
	pprintf(TextOutputStream,"MBKF: perturb %d - adding new track\n",hypothesis);
#endif
      }
      tracks.push_back(tracks[hypothesis]);
    } else {
      if (MBKFDebug) {
#ifdef PLATFORM_APERIOS
	pprintf(TextOutputStream,"MBKF: perturb %d - modifying track\n",hypothesis);
#endif
      }
      tracks.back()=tracks[hypothesis];
    }
    idx = tracks.size()-1;
  }

  // Do the actual perturbation
  tracks[idx].perturb(timestamp,__x,0,false);
  
  // Grow the covariance of the old track since it's more unlikely
  if (clone) {
    tracks[hypothesis].scaleCov(1.1);
    // Sort the tracks since their relative likelihoods have changed
    for (uint i=0;i<tracks.size();i++) {
      tracks[i].getLikelihood(tracks[i].cached_likelihood,0);
    }
    sort(tracks.begin(),tracks.end(),grt_kf());
  }
}

void MultiBallKFPos::erase(int hypothesis) {
  if ((hypothesis < 0) || (hypothesis >= (int)tracks.size())) { return; }
  int ctr=0;
  deque<BallKFPos>::iterator itr=tracks.begin();
  while (ctr != hypothesis) { itr++;ctr++; }
  if (itr!=tracks.end()) {
    tracks.erase(itr);
  }
}

//------------------------------------------------------------------------
// Reset the tracker
void MultiBallKFPos::reset(ulong timestamp,
			   Matrix __x,
			   Matrix __P) {

  // Delete all tracks except for one
  tracks.clear();
  P=__P;
  tracks.push_back(BallKFPos(timestamp,nr_states,__x,P));
}

//------------------------------------------------------------------------
int MultiBallKFPos::numHypotheses() {
  return tracks.size();
}

//------------------------------------------------------------------------
// Specify a return data type of some kind by name
bool MultiBallKFPos::getState(Matrix & val, 
			      const string & param, 
			      int hypothesis) {
  if ((hypothesis < 0) || (hypothesis >= (int)tracks.size())) { return false; }
  return tracks[hypothesis].getState(val,param,0);
}

bool MultiBallKFPos::getLikelihood(double & val,int hypothesis) {
  if ((hypothesis < 0) || (hypothesis >= (int)tracks.size())) { return false; }
  return tracks[hypothesis].getLikelihood(val,hypothesis);
}


bool MultiBallKFPos::getTime(double & t,
			     int hypothesis) {
  if ((hypothesis < 0) || (hypothesis >= (int)tracks.size())) { return 0; }
  return tracks[hypothesis].getTime(t,0);
}

bool MultiBallKFPos::getObservationTime(double & t,
					int hypothesis) {
  if ((hypothesis < 0) || (hypothesis >= (int)tracks.size())) { return 0; }
  return tracks[hypothesis].getObservationTime(t,0);
}

bool MultiBallKFPos::getIDFromHypothesis(ulong & ID, 
					 int hypothesis) {
  if ((hypothesis < 0) || (hypothesis >= (int)tracks.size())) { return false; }
  return tracks[hypothesis].getIDFromHypothesis(ID,0);
}

bool MultiBallKFPos::getHypothesisFromID(int & hypothesis, 
					 ulong ID) {
  for (int i=0;i<(int)tracks.size();i++) {
    // The hypothesis returned from KFPos is always 0 since that is a 
    // single hypothesis tracker.  Thus, we need a scratch variable to
    // pass into that interface.
    int _h;
    if (!tracks[i].getHypothesisFromID(_h,ID)) {
      // Once we match the ID, then we know what hypothesis we want from
      // the MultiKFPos tracker
      hypothesis = i;
      return true;
    }
  }
  return false;
}

bool MultiBallKFPos::getStateByID(Matrix & val,
				  const string & param,
				  ulong ID) {
  int hypothesis=0;
  if (!getHypothesisFromID(hypothesis,ID)) { return false;}
  return tracks[hypothesis].getState(val,param,0);
}

bool MultiBallKFPos::getTimeByID(double & t, ulong ID) {
  int hypothesis=0;
  if (!getHypothesisFromID(hypothesis,ID)) { return false;}
  return tracks[hypothesis].getTime(t,0);
}

bool MultiBallKFPos::getObservationTimeByID(double & t, ulong ID) {
  int hypothesis=0;
  if (!getHypothesisFromID(hypothesis,ID)) { return false;}
  return tracks[hypothesis].getObservationTime(t,0);
}