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

// The different kinds of trackers
// Single hypothesis
#include "BallKFPos.h"
// Multi hypotheses
#include "MultiBallKFPos.h"

static const bool TBTDebug = false;

static const double cP=600; // Initial covariance of a new tracker
static const double cR=300;
static const double cQ=500; // Propagation noise model

// Initial covariance estimate
static const float BALL_P_INIT [] = {cP*cP, 0.0, 0.0, cP*cP}; 
static const float BALL_Q_NOISE [] = {cQ*cQ, 0.0, 0.0, cQ*cQ};
static const float BALL_R_NOISE [] = {cR*cR, 0.0, 0.0, cR*cR};

// cutoff thresh is e^-16.5.  Ln values are easier to deal with than
// double constants with tons of decimal places
static const double PRUNE_THRESHOLD = pow(M_E,-16.5);

static const double MINIMUM_KALMAN_DISTANCE = 200;  

void TeammateBallTracker::prune() {
  
  if (0 == numHypotheses()) { return; }

  // Prune unlikely hypotheses
  int last_hypothesis = numHypotheses()-1;
  Gaussian2 pos;
  double val;
  if (tracker->getLikelihood(val,last_hypothesis)) {
    if (val < PRUNE_THRESHOLD) {
      if (TBTDebug) {
#ifdef PLATFORM_APERIOS
	pprintf(TextOutputStream,"TBT: Popping unlikely last track, log(val)=%f\n",log(val));
#endif
      }
      tracker->erase(last_hypothesis);
    }
  }

  // Prune hypotheses that are right on top of each other
  bool done=false;
  while (!done) {
    done = true;
    
    int idx1;
    int idx2;
    
    idx1=0;
    while (idx1 < numHypotheses()) {
      idx2 = idx1+1;
      while(idx2 < numHypotheses()) {
	
	Matrix x1,x2;
	
	if (!tracker->getState(x1,string("state"),idx1)) { 
	  if (TBTDebug) {
#ifdef PLATFORM_APERIOS
	    pprintf(TextOutputStream,"TBT: prune() ERROR getting state1\n");
#endif
	  }
	  return;
	}
	
	if (!tracker->getState(x2,string("state"),idx2)) { 
	  if (TBTDebug) {
#ifdef PLATFORM_APERIOS
	    pprintf(TextOutputStream,"TBT: prune() ERROR getting state2\n");
#endif
	  }
	  return;
	}
	
	double dist=sqrt(sq(x1.e(0,0)-x2.e(0,0)) + 
			 sq(x1.e(1,0)-x2.e(1,0)));
	
	if (dist < MINIMUM_KALMAN_DISTANCE) {
	  if (TBTDebug) {
#ifdef PLATFORM_APERIOS
	    ulong ID1, ID2;
	    tracker->getIDFromHypothesis(ID1,idx1);
	    tracker->getIDFromHypothesis(ID2,idx2);
	    pprintf(TextOutputStream,"TBT: Tracks %d (ID=%lu) and %d (ID=%lu) are too close (dist=%f), erasing track 1 (ID=%lu)\n",idx1,ID1,idx2,ID2,dist,ID2);
#endif
	  }
	  tracker->erase(idx2);
	  done = false;
	  break;
	}
	idx2++;
      }
      idx1++;
    }
  }
}

bool TeammateBallTracker::nullTracker() {
    if (NULL==tracker) {
#ifdef PLATFORM_APERIOS
    pprintf(TextOutputStream,"TeammateBallTracker: ERROR!!! NULL TRACKER\n");
#endif
    return true;
  } 
  return false;
}

TeammateBallTracker::TeammateBallTracker(tracker_type t) {
  Matrix x;
  Matrix P;
  x.resize(nr_states,1);
  P.resize(nr_states);
  Q.resize(nr_states);
  x.e(0,0)=0.0;
  x.e(1,0)=0.0;

  P.set(BALL_P_INIT);
  Q.set(BALL_Q_NOISE);
  
  // Initialize the tracker
  switch(t) {
  case tPOS:
    tracker = new BallKFPos(0,nr_states,x,P);
    break;
  case tMULTIPOS:
    tracker = new MultiBallKFPos(0,nr_states,x,P);
    break;
  default:
    tracker=NULL;
  }
}

TeammateBallTracker::~TeammateBallTracker() {
  if (NULL != tracker) {
    delete tracker;
  }
}

// #$#
// This method will be called at every frame.  However, the observations
// will be at slightly different timestamps since the robot's times will
// be different.  Therefor, instead of displaying the current time, we 
// should prune the tracker based on the age of the reported observation.
//
// All state observations should in fact be predictions from the latest
// state rather than true propagations.

void TeammateBallTracker::propagate(ulong timestamp) {
  if (nullTracker()) { return; }
  tracker->setProcessNoise(Q);
  tracker->propagate(timestamp);
  prune();
}

void TeammateBallTracker::observe(ulong timestamp,
				  Gaussian2 obs,
				  bool imp_filter) {
  if (nullTracker()) { return; }

  Matrix z;
  Matrix R;

  z.resize(nr_states,1);
  R.resize(nr_states);
  
  z.e(0,0)=obs.mean.x;
  z.e(1,0)=obs.mean.y;

  // Determine what process noise we should use 
  Gaussian2 r;
  r.mean=obs.mean;
  r.setsxsy(cR*cR,cR*cR,0.0);

  // Set the observation uncertainty.
  // Chose the less likely of the two because we don't want 
  // the covariance to be an overestimate
  if (r.eval(obs.mean) > obs.eval(obs.mean)) {
    R.e(0,0)=obs.sx;
    R.e(1,1)=obs.sy;
    R.e(0,1)=R.e(1,0)=obs.psxsy;
  } else {
    R.set(BALL_R_NOISE);
  }

  tracker->observe(timestamp,z,Q,R,imp_filter);
  prune();
}

bool TeammateBallTracker::getPosition(Gaussian2 & pos,
				      int hypothesis) {
  if (nullTracker()) { return false; }
  Matrix x;
  Matrix P;
  
  if (!tracker->getState(x,string("state"),hypothesis)) {
    return false;
  }
  if (!tracker->getState(P,string("covariance"),hypothesis)) {
    return false;
  }
  double t;
  if (!tracker->getTime(t,hypothesis)) {
    return false;
  }
  pos.time=SecToTime(t);
  pos.mean=vector2d(x.e(0,0),x.e(1,0));
  pos.setsxsy(P.e(0,0),P.e(1,1),P.e(1,0));
  return true;
}

bool TeammateBallTracker::getPositionFromID(Gaussian2 & pos,
				    ulong ID) {
  if (nullTracker()) { return false; }
  int hypothesis;
  if (!getHypothesisFromID(hypothesis,ID)) { return false; }
  return getPosition(pos,hypothesis);
}

bool TeammateBallTracker::getObservationTime(ulong & t,
				     int hypothesis) {
  if (nullTracker()) { return false; }
  double _t;
  bool retval = tracker->getObservationTime(_t,hypothesis);
  if (retval) {
    t=SecToTime(_t);
    return true;
  } else {
    return false;
  }
}

bool TeammateBallTracker::getObservationTimeFromID(ulong & t,
					   ulong ID) {
  if (nullTracker()) { return false; }
  int hypothesis;
  if (!getHypothesisFromID(hypothesis,ID)) { return false; }
  return getObservationTime(t,hypothesis);
}

int TeammateBallTracker::numHypotheses() {
  if (nullTracker()) { return 0; }
  return tracker->numHypotheses();
}

// Return the ID of the specified hypothesis
bool TeammateBallTracker::getIDFromHypothesis(ulong & ID, int hypothesis) {
  if (nullTracker()) { return false; }
  return tracker->getIDFromHypothesis(ID,hypothesis);
}

// Return the hypothesis of the specified ID
bool TeammateBallTracker::getHypothesisFromID(int & hypothesis, ulong ID) {
  if (nullTracker()) { return false; }
  return tracker->getHypothesisFromID(hypothesis,ID);
}