/********************************************************************************/
/*  Program	   :  condensation.cpp						*/
/*  Version	   :  0.01A							*/
/*  Abstract       :  	*/
/*  Copyright      :  GuanFeng Ye						*/
/*  Creation Date  :  10/30/2005						*/
/*  Revision Date  :  10/30/2005						*/
/*										*/
/********************************************************************************/
#include "condensation.h"

double WINAPI uniform_random( );
double WINAPI gaussian_random( );
double WINAPI evaluate_gaussian(double val, double sigma);

/*********************************************************************************
  Function    : 
  Description : 
  Parameter   : 
  Return      : 
  Call	      :
*********************************************************************************/ProcessModel::ProcessModel()
{
	m_Mean    = ProcessMean;
	m_Scaling = ProcessScaling;
	m_Sigma   = ProcessSigma;
}

/*********************************************************************************
  Function    : first_order_arp
  Description : The process model for a first-order auto-regressive process is:
		x_{t+1} - mean = (x_t - mean)*scaling + sigma*w_t
		where w_t is unit iid Gaussian noise.
  Parameter   : previous (Input) -
  Return      : 
  Call	      :
*********************************************************************************/

double
ProcessModel::first_order_arp(double previous)
{
	return m_Mean +	((previous - m_Mean) * m_Scaling) + m_Sigma * gaussian_random();
}

/*********************************************************************************
  Function    : SceneModel/~SceneModel
  Description : 
  Parameter   : 
  Return      : 
  Call	      :
*********************************************************************************/
SceneModel::SceneModel()
{
	m_Sigma = SceneSigma;
}


/*********************************************************************************
  Function    : PriorModel/~PriorModel
  Description : 
  Parameter   : 
  Return      : 
  Call	      :
*********************************************************************************/
PriorModel::PriorModel()
{
	m_Mean  = PriorMean;
	m_Sigma = PriorSigma;
}


/*********************************************************************************
  Function    : ObservationModel/~ObservationModel
  Description : 
  Parameter   : 
  Return      : 
  Call	      :
*********************************************************************************/
ObservationModel::ObservationModel()
{
	m_Sigma = ObsSigma;
}


/*********************************************************************************
  Function    : DisplayModel/~DisplayModel
  Description : 
  Parameter   : 
  Return      : 
  Call	      :
*********************************************************************************/
DisplayModel::DisplayModel()
{
	m_HistogramWidth = DisplayWidth;
}


/*********************************************************************************
  Function    : 
  Description : 
  Parameter   : 
  Return      : 
  Call	      :
*********************************************************************************/
void
MeasData::SetTrueValue(double value)
{
	m_TrueValue = value;
}


/*********************************************************************************
  Function    : obtain_observations
  Description : In a real implementation, this routine would go and actually make
		measurements and store them in the data.meas structure. This
		simulation consists of an `object' moving around obeying a
		first-order auto-regressive process, and being observed with its
		true positions corrupted by Gaussian measurement noise.
		Accordingly, this routine calculates the new simulated true and
		measured position of the object. 
  Parameter   : 
  Return      : 
  Call	      :
*********************************************************************************/

void
MeasData::obtain_observations(GlobalData *global)
{
	m_TrueValue = global->scene.process.first_order_arp(m_TrueValue);
	m_Observed  = m_TrueValue + global->scene.sigma() * gaussian_random();
}

/*********************************************************************************
  Function    : evaluate_observation_density
  Description : This routine evaluates the observation density
		p(z_t|x_t = new_positions[new_sample])
		The observation model in this implementation is a simple mixture of
		Gaussians, where each simulated object is observed as a 1d position
		and measurement noise is represented as Gaussian. For a
		visual-tracking application, this routine would go and evaluate the
		likelihood that the object is present in the image at the position
		encoded by new_positions[new_sample].
  Parameter   : new_sample (Input) -
  Return      : 
  Call	      :
*********************************************************************************/

double
IterationData::evaluate_observation_density(int new_sample, double sigma)
{
	return evaluate_gaussian(new_positions[new_sample] - meas.observed(), sigma);
}


/*********************************************************************************
  Function    : eval_bin
  Description : The following two routines provide rudimentary graphical output in
		the form of an ASCII histogram of the 1d state density.
  Parameter   : where (Input) -
  Return      : 
  Call	      :
*********************************************************************************/
/*
int
Condensation::eval_bin(float where)
{
	return (HistBins-1)/2 + (int) (where * (HistBins-1)/2 / global.disp.histogram_width);
}
*/

/*********************************************************************************
  Function    : display_histogram
  Description : 
  Parameter   : estimated_position (Input) -
  Return      : 
  Call	      :
*********************************************************************************/
/*
static void
Condensation::display_histogram(double estimated_position)
{
  static double bins[HistBins];

  int b, n, line, which_bin, meas_bin, est_bin, true_bin;
  double lineheight;
  char outc;

  for (b=0; b<HistBins; ++b)
    bins[b] = 0.0;

  for (n=0; n<global.nsamples; ++n) {
    which_bin = eval_bin(data.new_positions[n]);

    if (which_bin >=0 && which_bin < HistBins)
      bins[which_bin] +=
	data.sample_weights[n] / data.largest_cumulative_prob;
  }

  for (b=0; b<HistBins; ++b)
    bins[b] = (bins[b] * (double) HistLines) / MaxHistHeight;

  for (line=0; line<HistLines; ++line) {
    lineheight = (double) (HistLines-1-line);
    for (b=0; b<HistBins; ++b) {
      if (line==0 && bins[b] >= lineheight+1.0)
	outc = '*';
      else if (bins[b] >= lineheight+0.5 && bins[b] < lineheight+1.0)
	outc = '-';
      else if (bins[b] >= lineheight && bins[b] < lineheight+0.5)
	outc = '_';
      else
	outc = ' ';
      printf("%c", outc);
    }
    printf("\n");
  }

  true_bin = eval_bin(data.meas.true);
  meas_bin = eval_bin(data.meas.observed);
  est_bin = eval_bin(estimated_position);

  for (b=0; b<HistBins; ++b) {
    if ((b == meas_bin && b == est_bin) ||
	(b == meas_bin && b == true_bin) ||
	(b == true_bin && b == est_bin))
      outc = '*';
    else if (b == true_bin)
      outc = '.';
    else if (b == meas_bin)
      outc = '+';
    else if (b == est_bin)
      outc = 'x';
    else
      outc = ' ';
    printf("%c", outc);
  }
  printf("\n");
}
*/

/*********************************************************************************
  Function    : display_data
  Description : This routine computes the estimated position of the object by
		estimating the mean of the state-distribution as a weighted mean of
		the sample positions, then displays a histogram of the state
		distribution along with a character denoting the estimated position
		of the object.
  Parameter   : iteration (Input) -
  Return      : 
  Call	      :
*********************************************************************************/
/*
void
Condensation::display_data(int iteration)
{
  int n;
  double aggregate;

  aggregate = 0.0;
*/
  /* Compute the unnormalised weighted mean of the sample
     positions. */
/*
  for (n=0; n<global.nsamples; ++n)
    aggregate += data.new_positions[n] * data.sample_weights[n];

  aggregate /= data.largest_cumulative_prob;

  display_histogram(aggregate);

  printf("%04d: Measured pos. % 3.4lf True pos. % 3.4lf Est. position % 3.4lf\n",
	 iteration, data.meas.observed, data.meas.true, aggregate);

#ifdef ANSI_TERM_SEQUENCES
*/
  /* If possible, run the cursor back up the screen so the histogram
     stays in the same place instead of scrolling down the display. */
/*
  printf("\033[%dA", HistLines+2);
  sleep(1);
#endif
}