/*----------------------------------------------------------------------------
  Routine to compute the a-posteriori probabilities of each of the modes in
  the Mixture distribution defined by c,mean and var for the the vector
  defined by x[]. Returns the posteriors in Tau[]
  NOTE: Acutally uses INVERSE of the variance and not the variance,
        i.e 1/var, instead of var because multiplication by (1/var)
        is faster than division by var for the gauss()
------------------------------------------------------------------------------*/
#include <math.h>

#include "VTS.h"

/*------------------------------------------------------------------------------
	Function defining the log gaussian density function given mean and 
        variance.
------------------------------------------------------------------------------*/

float gauss(float *x, float *mean, float *var_inverse, float corprod,
	    int Ndim)
{
	auto int j;
	auto float power = 0, density;

	for (j = 0; j < Ndim; ++j)
		power +=
		    (x[j] - mean[j]) * (x[j] - mean[j]) * var_inverse[j];
	power *= -0.5;
	density = corprod + power;
	return (density);
}


float expectation(float *Tau,	/* Mode probablity of observation vector x */
		  float *x,	/* Observation vector x */
		  float **mean,	/* Means of the modes */
		  float **var_inverse,	/* Inverses of the variance of the modes (1/var) */
		  float *corprod,	/* Precomputed 1/sqrt(2PI**Ndim * mod(variance)) */
		  int K,	/* Number of modes */
		  int Ndim	/* Dimensionality of observation/distribution */
    )
{
	float logProbability;
	/*
   double   minTau=1.0e-30; *//* Minimum value below which the mode probablity 
   of an observation is not allow to fall */
	double sum, max, bmax, ltau[1024];
	double beam = 50.0;
	int i;

	ltau[0] = max =
	    (double) gauss(x, mean[0], var_inverse[0], corprod[0], Ndim);
	for (i = 1; i < K; ++i) {
		/*
		 * Note: prior c[i] has already been factored into corprod
		 */
		ltau[i] =
		    (double) gauss(x, mean[i], var_inverse[i], corprod[i],
				   Ndim);
		if (ltau[i] > max)
			max = ltau[i];
	}

	/* 
	 * convert Tau from log to linear 
	 * Note: The normalization by max-10 is to keep all exponents
	 * below 10
	 */
	sum = 0.0;
	bmax = max - beam;
	for (i = 0; i < K; i++) {
		if (ltau[i] > bmax)
			ltau[i] =
			    (double) exp((double) (ltau[i] - max + 10));
		else
			ltau[i] = 0;

		/*
		 * If Tau[i] becomes very small, replace it by a preset minimum to
		 * eliminate the problem of c[i] going to 0 for some i
		 if (Tau[i] < minTau) Tau[i] = minTau;
		 */
		sum += ltau[i];
	}
	for (i = 0; i < K; ++i)
		Tau[i] = (float) (ltau[i] / sum);

	logProbability = (float) (max + log(sum) - 10.0);
	return (logProbability);
}

