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

// The different kinds of trackers

// Single hypothesis
#include "BallKFPos.h"
// Multi hypotheses
#include "MultiBallKFPos.h"

#ifdef PLATFORM_LINUX
#include <cstdio>
#endif

static const bool BTDebug = false;

//========================================================================
// Constants for tracking the ball
// All distances are in millimeters

static const double cP=500; // Initial covariance of a new tracker

static const double cQ=400; // Propagation noise model while observed
static const double cQU=500; // Propagation noise model while unobserved

static const double cR=200; // Sensor noise model

// The cross correlation terms are probably not accurate being zero

// Initial covariance estimate
static const double BALL_P_INIT [] = {cP*cP, 0.0, 0.0, cP*cP}; 
    
// Process noise for the ball
//   When the ball is observed
static const double BALL_Q_NOISE_OBS [] = {cQ*cQ,0.0,0.0,cQ*cQ};
//   When the ball is unobserved
static const double BALL_Q_NOISE_UNOBS [] = {cQU*cQU, 0.0, 0.0, cQU*cQU};
//   Any more?

// Sensor noise -- this will be adjusted
static const double BALL_R_NOISE [] = {cR*cR, 0.0, 0.0, cR*cR};

// If the ball is observed at this distance, then we pretty much trust
// our vision absolutely
static const double GRAB_DISTANCE = 1000.0;

// 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 = 150;  // KFs must be no closer than 15cm

//========================================================================
bool BallTracker::closestDistance(vector2d obs, 
				  double & min_dist,
				  int & min_idx) {
  if (nullTracker() || (0==numHypotheses())) { return false; }

  Matrix old_z;
  tracker->getState(old_z,string("state"),0);
  min_dist = sqrt(sq(obs.x-old_z.e(0,0)) + 
		  sq(obs.y-old_z.e(1,0)));
  min_idx = 0;
  for (int t=1;t<numHypotheses();t++) {
    tracker->getState(old_z,string("state"),t);
    double dist=sqrt(sq(obs.x-old_z.e(0,0)) + 
		     sq(obs.y-old_z.e(1,0)));
    if (dist < min_dist) {
      min_dist = dist;
      min_idx=t;
    }
  }
  return true;
}

//========================================================================
void BallTracker::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 (BTDebug) {
#ifdef PLATFORM_APERIOS
	pprintf(TextOutputStream,"BT: 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 (BTDebug) {
#ifdef PLATFORM_APERIOS
	    pprintf(TextOutputStream,"BT: prune() ERROR getting state1\n");
#endif
	  }
	  return;
	}
	
	if (!tracker->getState(x2,string("state"),idx2)) { 

	  if (BTDebug) {
#ifdef PLATFORM_APERIOS
	    pprintf(TextOutputStream,"BT: 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 (BTDebug) {
#ifdef PLATFORM_APERIOS
	    ulong ID1, ID2;
	    tracker->getIDFromHypothesis(ID1,idx1);
	    tracker->getIDFromHypothesis(ID2,idx2);
	    pprintf(TextOutputStream,"BT: 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++;
    }
  }
}

//========================================================================
// Constructor
BallTracker::BallTracker(tracker_type t) {

  Matrix x;
  Matrix P;

  x.resize(nr_states,1);
  P.resize(nr_states);
  Q_obs.resize(nr_states);
  Q_unobs.resize(nr_states);
  R.resize(nr_states);

  x.e(0,0)=0.0;
  x.e(1,0)=0.0;

  P.set(BALL_P_INIT);
  Q_obs.set(BALL_Q_NOISE_OBS);
  Q_unobs.set(BALL_Q_NOISE_UNOBS);
  R.set(BALL_R_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;
  }
}

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

//----------------------------------------------------------------------------
// Reset the tracker
void BallTracker::reset(ulong timestamp,
			vector2d pos) {
  if (nullTracker()) { return; }

  Matrix x;
  Matrix P;

  x.resize(nr_states,1);
  P.resize(nr_states);
  x.e(0,0)=pos.x;
  x.e(1,0)=pos.y;
  P.set(BALL_P_INIT);

  tracker->reset(timestamp,x,P);
}

// Reset the tracker
void BallTracker::reset(ulong timestamp,
			Gaussian2 pos) {
  if (nullTracker()) { return; }
  Matrix x;
  Matrix P;

  x.resize(nr_states,1);
  P.resize(nr_states);

  x.e(0,0)=pos.mean.x;
  x.e(1,0)=pos.mean.y;

  P.e(0,0)=pos.sx;
  P.e(1,1)=pos.sy;
  P.e(1,0)=P.e(0,1)=pos.psxsy;

  tracker->reset(timestamp,x,P);
}

//----------------------------------------------------------------------------
// Propagate the kalman filter to the input timestep
void BallTracker::propagate(ulong timestamp) {
  if (nullTracker()) { return; }
  // This is only called when we have no observation of the ball
  tracker->setProcessNoise(Q_unobs);
  tracker->propagate(timestamp);

  // Prune excess hypotheses if they exist
  prune();
}

//----------------------------------------------------------------------------
// Add an observation to the tracker
void BallTracker::observe(ulong timestamp, 
			  vector2d obs, 
			  vector2d vel,
			  double confidence,
			  double distance,
			  double head_tilt, // velocity
			  double head_pan, // velocity
			  bool imp_filter) {

  if (nullTracker()) { return; }

#if 0
  if (0!=vel.length()) {
    pprintf(TextOutputStream,"BallTracker::observe() Velocity=%f\n",
	    vel.length());
  }
#endif

  double val;
  
  // The cutoff point for the tracker confidence
  val = distance/GRAB_DISTANCE;
  val = cR*(val*val); // make it a quadratic

  if (confidence < .9) {

    if (BTDebug) {
#ifdef PLATFORM_APERIOS
      pprintf(TextOutputStream,"BallTracker::observe() Confidence=%f\n",confidence);
#endif
    }

  }
  if (confidence > .9) {
  } else if (confidence > .7) {
    val=val*2;
  } else if (confidence > .4) {
    val=val*10;
  }

#if 0
  // Take the head velocity into account
  if ((head_tilt > 2.5) ||
      (head_pan > 2.5)) {

    if (BTDebug) {
#ifdef PLATFORM_APERIOS
      pprintf(TextOutputStream,"BallTracker: head velocity is too fast!\n");
#endif
    }

    //    vel=vel*2;
  }
#endif

  // We're assuming a circular covariance
  R.e(0,0)=(val*val);
  R.e(1,1)=(val*val);

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

  // Set all of the other hypotheses (if they exist) to the unobserved
  // process noise
  tracker->setProcessNoise(Q_unobs);

  // Add the observation to the tracker which makes use of the correct
  // process noise
  tracker->observe(timestamp,z,Q_obs,R,imp_filter);
  // Prune excess hypothesis (if they exist)
  prune();
}

//----------------------------------------------------------------------------
// Perturb the tracker with a new position
void BallTracker::perturb(ulong timestamp,
			  vector2d pos,
			  int idx) {
  if (nullTracker()) { return; }

  Matrix __x;
  __x.resize(nr_states,1);
  __x.e(0,0)=pos.x;
  __x.e(1,0)=pos.y;

  Matrix old_x;
  if (tracker->getState(old_x,string("state"),idx)) {
    double dist = sqrt(sq(pos.x - old_x.e(0,0)) +
		       sq(pos.y - old_x.e(1,0)));

    if (dist < minimum_kalman_distance) {

      if (BTDebug) {
#ifdef PLATFORM_APERIOS
	pprintf(TextOutputStream,"BT: Perturbing track %d with no cloning\n",idx);
#endif
      }

      tracker->perturb(timestamp,__x,idx,false);
    } else {

      if (BTDebug) {
#ifdef PLATFORM_APERIOS
	pprintf(TextOutputStream,"BT: Perturbing track %d with cloning\n",idx);
#endif
      }

      tracker->perturb(timestamp,__x,idx,true);
    }
    prune();
  }
}

//----------------------------------------------------------------------------
// How many hypotheses are we tracking?
int BallTracker::numHypotheses() {
  if (nullTracker()) { return 0; }
  return tracker->numHypotheses();
}

//----------------------------------------------------------------------------
// Return the estimated ball position
bool BallTracker::getPosition(Gaussian2 & pos, 
			      int hypothesis) {
  // Grab the position of the ball from the tracker
  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 BallTracker::getPositionFromID(Gaussian2 & pos,
				    ulong ID) {
  if (nullTracker()) { return false; }
  int hypothesis;
  if (!getHypothesisFromID(hypothesis,ID)) { return false; }
  return getPosition(pos,hypothesis);
}

bool BallTracker::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 BallTracker::getObservationTimeFromID(ulong & t,
					   ulong ID) {
  if (nullTracker()) { return false; }
  int hypothesis;
  if (!getHypothesisFromID(hypothesis,ID)) { return false; }
  return getObservationTime(t,hypothesis);
}

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

bool BallTracker::getIDFromHypothesis(ulong & ID, int hypothesis) {
  if (nullTracker()) { return false; }
  return tracker->getIDFromHypothesis(ID,hypothesis);
}

bool BallTracker::getHypothesisFromID(int & hypothesis,ulong ID) {
  if (nullTracker()) { return false; }
  return tracker->getHypothesisFromID(hypothesis,ID);
}