//-----------------------------------------------------------------------------
//
//  EM.c++
//
//  Functions for the EM Algorithm.
//
//  Tony Jebara (Copyright (C) 1997)
//
//-----------------------------------------------------------------------------

#include <stdio.h>
#include <time.h>
#include <unistd.h>
#include <stdlib.h>
#include <sys/types.h>
#include <unistd.h>
#include <sys/file.h>
#include <sys/time.h>
#include <string.h>
#include <limits.h>
#include <math.h>
#include "quickmatrix.cpp"

#define  pi         3.1415927
#define  EPSILON    0.00001
#define  FRAC       100.0
Matrix  regul;

struct  timeval time1;

void
allocateGaussianMixtureModelEM(int D, int M, double **mu, double **R, double **COV, double **scal, double **mixo)
{
  /* Allocate the Global Model */
  *mu   = (double *) malloc(M*D*sizeof(double));
  *R    = (double *) malloc(M*D*D*sizeof(double));
  *COV  = (double *) malloc(M*D*D*sizeof(double));
  *scal = (double *) malloc(M*sizeof(double));
  *mixo = (double *) malloc(M*sizeof(double));
}

void
freeGaussianMixtureModelEM(int D, int M, double **mu, double **R, double **COV, double **scal, double **mixo)
{
  /* Free up the Global Model */
  free(*mu);
  free(*R);
  free(*COV);
  free(*scal);
  free(*mixo);
}

double
kullbackLieblerDivergenceGaussians(int D, double *mu1, double *cov1, double *mu2, double *cov2)
{
  // Would also like to integrate non-symbolic version with weak law of large numbers

  int      i, j;
  double   plogp = 0.0;
  double   plogq = 0.0;
  double   detE;
  double   detK;
  Vector   u            = VectorCreate(D);
  Matrix   E            = MatrixCreate(D,D);
  Vector   v            = VectorCreate(D);
  Matrix   K            = MatrixCreate(D,D);

  memcpy(u->d, mu1,  D*sizeof(double));
  memcpy(E->d, cov1, D*D*sizeof(double));
  memcpy(v->d, mu2,  D*sizeof(double));
  memcpy(K->d, cov2, D*D*sizeof(double));

  detE = MatrixDeterminant(E);
  detK = MatrixDeterminant(K);

  MatrixInvert(K);
  
  for (i=0; i<D; i++)
    for (j=0; j<D; j++)
      {
	plogq += (E->d2[i][j] + (v->d[j] - u->d[j]) * (v->d[i] - u->d[i])) * K->d2[i][j];
      }
  plogq += log( pow(2.0*pi,D) * detK );
  plogq *= (-0.5);

  plogp  = -0.5 * D - 0.5 * log( pow(2.0*pi,D) * detE );

  return(plogp - plogq);
}

double
kullbackLieblerDivergenceInvertedGaussians(int D, double *mu1, double *cov1, double *mu2, double *cov2)
{
  int      i, j;
  double   plogp = 0.0;
  double   plogq = 0.0;
  double   detE;
  double   detK;
  Vector   u            = VectorCreate(D);
  Matrix   E            = MatrixCreate(D,D);
  Vector   v            = VectorCreate(D);
  Matrix   K            = MatrixCreate(D,D);

  memcpy(u->d, mu1,  D*sizeof(double));
  memcpy(E->d, cov1, D*D*sizeof(double));
  memcpy(v->d, mu2,  D*sizeof(double));
  memcpy(K->d, cov2, D*D*sizeof(double));

  MatrixInvert(E);
  MatrixInvert(K);

  detE = MatrixDeterminant(E);
  detK = MatrixDeterminant(K);

  MatrixInvert(K);
  
  for (i=0; i<D; i++)
    for (j=0; j<D; j++)
      {
	plogq += (E->d2[i][j] + (v->d[j] - u->d[j]) * (v->d[i] - u->d[i])) * K->d2[i][j];
      }
  plogq += log( pow(2.0*pi,D) * detK );
  plogq *= (-0.5);

  plogp  = -0.5 * D - 0.5 * log( pow(2.0*pi,D) * detE );

  return(plogp - plogq);
}

void
randomInitGaussianMixtureModelEM(double *current, int N, int D, int M, double *mu, double *R, double *scal)
{
  int      d, i, m;
  Vector   x            = VectorCreate(D);
  Matrix   xx           = MatrixCreate(D,D);
  Matrix   mxD1         = MatrixCreate(D,1);
  Matrix   mx1D         = MatrixCreate(1,D);
  Vector   firstMoment  = VectorCreate(D);
  Matrix   secondMoment = MatrixCreate(D,D);
  Vector   mean         = VectorCreate(D);
  Vector   initsd       = VectorCreate(D);
  double   sqrtDet;
  unsigned short seed16v[3];

  struct timezone tzp; 
  gettimeofday(&time1,&tzp);
  seed16v[0] = (short) time1.tv_sec;
  seed16v[1] = (short) time1.tv_usec;
  seed16v[2] = (short) (time1.tv_sec*time1.tv_usec);
  seed48((unsigned short *) seed16v);

  VectorSet(firstMoment, 0.0);
  VectorSet((Vector) secondMoment, 0.0);

  /* Get Mean and Covariance of All Data */
  for (i=0; i<N; i++)
    {
      memcpy(x->d, &current[i*D], D*sizeof(double));
      memcpy(mxD1->d, &current[i*D], D*sizeof(double));
      memcpy(mx1D->d, &current[i*D], D*sizeof(double));

      MatrixMultiply(xx, mxD1, mx1D);
      VectorAdd(firstMoment, x);
      VectorAdd((Vector) secondMoment, (Vector) xx);
    }

  VectorScale(firstMoment, 1.0/((double) N));
  VectorScale((Vector) secondMoment, 1.0/((double) N));

  memcpy(mxD1->d, firstMoment->d, D*sizeof(double));
  memcpy(mx1D->d, firstMoment->d, D*sizeof(double));
  MatrixMultiply(xx, mxD1, mx1D);
  VectorSubtract((Vector) secondMoment, (Vector) xx);


  regul = MatrixCreate(D,D);
  VectorSet((Vector) regul, 0.0);
  for (d=0; d<D; d++) regul->d2[d][d] = EPSILON;

  for (d=0; d<D; d++)
    if ((secondMoment->d2[d][d]/FRAC) > regul->d2[d][d])
      regul->d2[d][d] = secondMoment->d2[d][d]/FRAC;
  VectorAdd((Vector) secondMoment, (Vector) regul);



  VectorScale((Vector) secondMoment, 1.0/pow((double) M, 1.0/((double) D)));
  for (d=0; d<D; d++)
    initsd->d[d] = sqrt(secondMoment->d2[d][d]);

  MatrixInvert(secondMoment);

  sqrtDet = sqrt(MatrixDeterminant(secondMoment));
  
  /* Compute Some Random Models Based on Above */
  for (m=0; m<M; m++)
    {
      for (d=0; d<D; d++) mean->d[d] = 2.0*drand48()-1.0;
      VectorMultiply(mean, initsd);
      VectorAdd(mean, firstMoment);

      memcpy(&mu[m*D], mean->d, D*sizeof(double));
      memcpy(&R[m*D*D], secondMoment->d, D*D*sizeof(double));
      scal[m] = 1.0/(pow(2.0*pi,((double) D)*0.5))* sqrtDet / ((double) M);
    }
}

void
randomInitWeightedGaussianMixtureModelEM(double *current, double *weight, int N, int D, int M, double *mu, double *R, double *scal)
{
  int      d, i, m;
  Vector   x            = VectorCreate(D);
  Matrix   xx           = MatrixCreate(D,D);
  Matrix   mxD1         = MatrixCreate(D,1);
  Matrix   mx1D         = MatrixCreate(1,D);
  Vector   firstMoment  = VectorCreate(D);
  Matrix   secondMoment = MatrixCreate(D,D);
  Vector   mean         = VectorCreate(D);
  Vector   initsd       = VectorCreate(D);
  double   sqrtDet;
  double   totalN;
  unsigned short seed16v[3];

  struct timezone tzp; 
  gettimeofday(&time1,&tzp);
  seed16v[0] = (short) time1.tv_sec;
  seed16v[1] = (short) time1.tv_usec;
  seed16v[2] = (short) (time1.tv_sec*time1.tv_usec);
  seed48((unsigned short *) seed16v);

  VectorSet(firstMoment, 0.0);
  VectorSet((Vector) secondMoment, 0.0);

  /* Get Mean and Covariance of All Data */
  totalN = 0;
  for (i=0; i<N; i++)
    {
      memcpy(x->d, &current[i*D], D*sizeof(double));
      memcpy(mxD1->d, &current[i*D], D*sizeof(double));
      memcpy(mx1D->d, &current[i*D], D*sizeof(double));

      MatrixMultiply(xx, mxD1, mx1D);
      VectorScale(x, weight[i]);
      VectorAdd(firstMoment, x);
      VectorScale((Vector) xx, weight[i]);
      VectorAdd((Vector) secondMoment, (Vector) xx);
      totalN += weight[i];
    }

  VectorScale(firstMoment, 1.0/((double) totalN));
  VectorScale((Vector) secondMoment, 1.0/((double) totalN));

  memcpy(mxD1->d, firstMoment->d, D*sizeof(double));
  memcpy(mx1D->d, firstMoment->d, D*sizeof(double));
  MatrixMultiply(xx, mxD1, mx1D);
  VectorSubtract((Vector) secondMoment, (Vector) xx);

  regul = MatrixCreate(D,D);
  VectorSet((Vector) regul, 0.0);
  for (d=0; d<D; d++) regul->d2[d][d] = EPSILON;

  for (d=0; d<D; d++)
    if ((secondMoment->d2[d][d]/FRAC) > regul->d2[d][d])
      regul->d2[d][d] = secondMoment->d2[d][d]/FRAC;
  VectorAdd((Vector) secondMoment, (Vector) regul);

  VectorScale((Vector) secondMoment, 1.0/pow((double) M, 1.0/((double) D)));
  for (d=0; d<D; d++)
    initsd->d[d] = sqrt(secondMoment->d2[d][d]);

  MatrixInvert(secondMoment);

  sqrtDet = sqrt(MatrixDeterminant(secondMoment));
  
  /* Compute Some Random Models Based on Above */
  for (m=0; m<M; m++)
    {
      for (d=0; d<D; d++) mean->d[d] = 2.0*drand48()-1.0;
      VectorMultiply(mean, initsd);
      VectorAdd(mean, firstMoment);

      memcpy(&mu[m*D], mean->d, D*sizeof(double));
      memcpy(&R[m*D*D], secondMoment->d, D*D*sizeof(double));
      scal[m] = 1.0/(pow(2.0*pi,((double) D)*0.5))* sqrtDet / ((double) M);
    }
}


double
updateParamsGaussianMixtureModelEM(double *current, int N, int D, int M, double *mu, double *R, double *COV, double *scal, double *mixo)
{
  FILE    *plotfp;
  int     i, d, d2, m;
  Vector  x     = VectorCreate(D);
  Vector  y     = VectorCreate(D);
  Vector  z     = VectorCreate(D);
  Vector  resp  = VectorCreate(M);
  Matrix  xx    = MatrixCreate(D,D);
  Matrix  yy    = MatrixCreate(D,D);
  Matrix  mxD1  = MatrixCreate(D,1);
  Matrix  mx1D  = MatrixCreate(1,D);
  Vector  *means;
  Matrix  *invcovs;
  double  sum;
  double  likelihood = 0;
  Vector  *firstMoments;
  Matrix  *secondMoments;
  double  *mix;

  /* Allocate Model and Moments */
  means         = (Vector *) malloc(M*sizeof(Vector));
  invcovs       = (Matrix *) malloc(M*sizeof(Matrix));
  firstMoments  = (Vector *) malloc(M*sizeof(Vector));
  secondMoments = (Matrix *) malloc(M*sizeof(Matrix));
  mix           = (double *) malloc(M*sizeof(double));

    

  for (m=0; m<M; m++)
    {
      means[m]   = VectorCreate(D);
      invcovs[m] = MatrixCreate(D,D);
      
      memcpy(means[m]->d, &mu[m*D], D*sizeof(double));
      memcpy(invcovs[m]->d, &R[m*D*D], D*D*sizeof(double));

      firstMoments[m]  = VectorCreate(D);
      VectorSet(firstMoments[m], 0.0);
      secondMoments[m] = MatrixCreate(D,D);
      VectorSet((Vector) secondMoments[m], 0.0);
      mix[m] = 0.0;
    }
 
  /* 1 EM Iteration */
  for (i=0; i<N; i++)
    {
      memcpy(x->d, &current[i*D], D*sizeof(double));
      memcpy(mxD1->d, &current[i*D], D*sizeof(double));
      memcpy(mx1D->d, &current[i*D], D*sizeof(double));

      MatrixMultiply(xx, mxD1, mx1D);

      // Might be better to just do x*x in a for loop
      // instead of doing this extensive memory access
      // also, only need upper (or lower) triangular
      // half of the matrix until we need to invert it
      // after the i=1:N iteration (i.e. in the m=1:M iteration)

      /* Expectation */
      for (m=0; m<M; m++)
	{
	  VectorMove(y, x);
	  VectorSubtract(y, means[m]);
	  MatrixVectorMultiply(z, invcovs[m], y);
	  resp->d[m] = scal[m]*exp(-0.5*VectorDot(z,y));
	}

      /* Normalization */
      sum   = VectorSum(resp);
      if (sum != 0)
	{
	  VectorScale(resp, 1.0/sum);
	  likelihood += log(sum);
	}

      /* Maximization */
      for (m=0; m<M; m++)
	{
	  VectorMove(y, x);
	  VectorScale(y, resp->d[m]);
	  VectorAdd(firstMoments[m], y);

	  VectorMove((Vector) yy, (Vector) xx);
	  VectorScale((Vector) yy, resp->d[m]);
	  VectorAdd((Vector) secondMoments[m], (Vector) yy);
	  
	  mix[m] += resp->d[m];
	}
    }

  /* Copy Model Out */
  sum = 0;
  for (m=0; m<M; m++)
    {
      VectorScale((Vector) firstMoments[m], 1.0/mix[m]);
      VectorScale((Vector) secondMoments[m], 1.0/mix[m]);

      memcpy(mxD1->d, firstMoments[m]->d, D*sizeof(double));
      memcpy(mx1D->d, firstMoments[m]->d, D*sizeof(double));
      MatrixMultiply(xx, mxD1, mx1D);
      VectorSubtract((Vector) secondMoments[m], (Vector) xx);
      VectorAdd((Vector) secondMoments[m], (Vector) regul);

      sum += mix[m];
    }


#ifdef DEBUG_MATLAB
  plotfp = fopen("plotme.m","w");
  fprintf(plotfp,"function [mu, c, mix] = plotme()\n");
  fprintf(plotfp, "mu=[");
  for (m=0; m<M; m++)
    {
      for (d=0; d<D; d++)
	fprintf(plotfp,"%f ",firstMoments[m]->d[d]);
      fprintf(plotfp,";");
    }
  fprintf(plotfp, "];\n");
  fprintf(plotfp, "c=[");
  for (m=0; m<M; m++)
    for (d=0; d<D; d++)
      {
	for (d2=0; d2<D; d2++)
	  fprintf(plotfp,"%f ",secondMoments[m]->d2[d][d2]);
	fprintf(plotfp,";");
      }
  fprintf(plotfp, "];\n");
  fprintf(plotfp, "mix=[");
  for (m=0; m<M; m++) fprintf(plotfp,"%f ",mix[m]/sum);
  fprintf(plotfp, "];\n");

  if (D==3)
    fprintf(plotfp,"plotClust3D(mu,c,1,2,3);\nview([30 50]);\n");
  if (D==2)
    fprintf(plotfp,"plotClust(mu,c,1,2);\n");
    
  fclose(plotfp);
#endif



  for (m=0; m<M; m++)
    {
      mix[m] /= sum;

      memcpy(&mu[m*D], firstMoments[m]->d, D*sizeof(double));
      memcpy(&COV[m*D*D], secondMoments[m]->d, D*D*sizeof(double));
      MatrixInvert(secondMoments[m]);
      memcpy(&R[m*D*D], secondMoments[m]->d, D*D*sizeof(double));

      scal[m] = 1.0/(pow(2.0*pi,((double) D)*0.5))*sqrt(MatrixDeterminant(secondMoments[m]))*mix[m];
      mixo[m] = mix[m];
    }



  /* Free Model and Moments */
  VectorFree(x);
  VectorFree(y);
  VectorFree(z);
  VectorFree(resp);
  MatrixFree(xx);
  MatrixFree(yy);
  MatrixFree(mxD1);
  MatrixFree(mx1D);
  for (m=0; m<M; m++)
    {
      VectorFree(means[m]);
      MatrixFree(invcovs[m]);
      VectorFree(firstMoments[m]);
      MatrixFree(secondMoments[m]);
    }

  free(means);
  free(invcovs);
  free(firstMoments);
  free(secondMoments);
  free(mix);


  /* Return Average Likelihood Before the EM Step */
  likelihood /= N;
  return(likelihood);
}


double
updateParamsGaussianMixtureModelEMANNEALED(double *current, int N, int D, int M, double *mu, double *R, double *scal, double T)
{
  FILE    *plotfp;
  int     i, d, d2, m;
  Vector  x     = VectorCreate(D);
  Vector  y     = VectorCreate(D);
  Vector  z     = VectorCreate(D);
  Vector  resp  = VectorCreate(M);
  Matrix  xx    = MatrixCreate(D,D);
  Matrix  yy    = MatrixCreate(D,D);
  Matrix  mxD1  = MatrixCreate(D,1);
  Matrix  mx1D  = MatrixCreate(1,D);
  Vector  *means;
  Matrix  *invcovs;
  double  sum;
  double  likelihood = 0;
  Vector  *firstMoments;
  Matrix  *secondMoments;
  double  *mix;

  /* Allocate Model and Moments */
  means         = (Vector *) malloc(M*sizeof(Vector));
  invcovs       = (Matrix *) malloc(M*sizeof(Matrix));
  firstMoments  = (Vector *) malloc(M*sizeof(Vector));
  secondMoments = (Matrix *) malloc(M*sizeof(Matrix));
  mix           = (double *) malloc(M*sizeof(double));

    

  for (m=0; m<M; m++)
    {
      means[m]   = VectorCreate(D);
      invcovs[m] = MatrixCreate(D,D);
      
      memcpy(means[m]->d, &mu[m*D], D*sizeof(double));
      memcpy(invcovs[m]->d, &R[m*D*D], D*D*sizeof(double));

      firstMoments[m]  = VectorCreate(D);
      VectorSet(firstMoments[m], 0.0);
      secondMoments[m] = MatrixCreate(D,D);
      VectorSet((Vector) secondMoments[m], 0.0);
      mix[m] = 0.0;
    }
 
  /* 1 EM Iteration */
  for (i=0; i<N; i++)
    {
      memcpy(x->d, &current[i*D], D*sizeof(double));
      memcpy(mxD1->d, &current[i*D], D*sizeof(double));
      memcpy(mx1D->d, &current[i*D], D*sizeof(double));

      MatrixMultiply(xx, mxD1, mx1D);

      // Might be better to just do x*x in a for loop
      // instead of doing this extensive memory access
      // also, only need upper (or lower) triangular
      // half of the matrix until we need to invert it
      // after the i=1:N iteration (i.e. in the m=1:M iteration)

      /* Expectation */
      for (m=0; m<M; m++)
	{
	  VectorMove(y, x);
	  VectorSubtract(y, means[m]);
	  MatrixVectorMultiply(z, invcovs[m], y);
	  resp->d[m] = scal[m]*exp(-0.5*VectorDot(z,y));
	  resp->d[m] = powf(resp->d[m],1.0/T);
	}

      /* Normalization */
      sum   = VectorSum(resp);
      if (sum != 0)
	{
	  VectorScale(resp, 1.0/sum);
	  likelihood += log(sum);
	}

      /* Maximization */
      for (m=0; m<M; m++)
	{
	  VectorMove(y, x);
	  VectorScale(y, resp->d[m]);
	  VectorAdd(firstMoments[m], y);

	  VectorMove((Vector) yy, (Vector) xx);
	  VectorScale((Vector) yy, resp->d[m]);
	  VectorAdd((Vector) secondMoments[m], (Vector) yy);
	  
	  mix[m] += resp->d[m];
	}
    }

  /* Copy Model Out */
  sum = 0;
  for (m=0; m<M; m++)
    {
      VectorScale((Vector) firstMoments[m], 1.0/mix[m]);
      VectorScale((Vector) secondMoments[m], 1.0/mix[m]);

      memcpy(mxD1->d, firstMoments[m]->d, D*sizeof(double));
      memcpy(mx1D->d, firstMoments[m]->d, D*sizeof(double));
      MatrixMultiply(xx, mxD1, mx1D);
      VectorSubtract((Vector) secondMoments[m], (Vector) xx);
      VectorAdd((Vector) secondMoments[m], (Vector) regul);

      sum += mix[m];
    }


#ifdef DEBUG_MATLAB
  plotfp = fopen("plotme.m","w");
  fprintf(plotfp,"function [mu, c, mix] = plotme()\n");
  fprintf(plotfp, "mu=[");
  for (m=0; m<M; m++)
    {
      for (d=0; d<D; d++)
	fprintf(plotfp,"%f ",firstMoments[m]->d[d]);
      fprintf(plotfp,";");
    }
  fprintf(plotfp, "];\n");
  fprintf(plotfp, "c=[");
  for (m=0; m<M; m++)
    for (d=0; d<D; d++)
      {
	for (d2=0; d2<D; d2++)
	  fprintf(plotfp,"%f ",secondMoments[m]->d2[d][d2]);
	fprintf(plotfp,";");
      }
  fprintf(plotfp, "];\n");
  fprintf(plotfp, "mix=[");
  for (m=0; m<M; m++) fprintf(plotfp,"%f ",mix[m]/sum);
  fprintf(plotfp, "];\n");

  if (D==3)
    fprintf(plotfp,"plotClust3D(mu,c,1,2,3);\nview([30 50]);\n");
  if (D==2)
    fprintf(plotfp,"plotClust(mu,c,1,2);\n");
    
  fclose(plotfp);
#endif



  for (m=0; m<M; m++)
    {
      mix[m] /= sum;

      memcpy(&mu[m*D], firstMoments[m]->d, D*sizeof(double));
      MatrixInvert(secondMoments[m]);
      memcpy(&R[m*D*D], secondMoments[m]->d, D*D*sizeof(double));

      scal[m] = 1.0/(pow(2.0*pi,((double) D)*0.5))*sqrt(MatrixDeterminant(secondMoments[m]))*mix[m];
    }



  /* Free Model and Moments */
  VectorFree(x);
  VectorFree(y);
  VectorFree(z);
  VectorFree(resp);
  MatrixFree(xx);
  MatrixFree(yy);
  MatrixFree(mxD1);
  MatrixFree(mx1D);
  for (m=0; m<M; m++)
    {
      VectorFree(means[m]);
      MatrixFree(invcovs[m]);
      VectorFree(firstMoments[m]);
      MatrixFree(secondMoments[m]);
    }

  free(means);
  free(invcovs);
  free(firstMoments);
  free(secondMoments);
  free(mix);


  /* Return Average Likelihood Before the EM Step */
  likelihood /= N;
  return(likelihood);
}



double
updateParamsGaussianMixtureModelNarrowEM(double *current, int N, int D, int M, double *mu, double *R, double *scal, double narrow)
{
  FILE    *plotfp;
  int     i, d, d2, m;
  Vector  x     = VectorCreate(D);
  Vector  y     = VectorCreate(D);
  Vector  z     = VectorCreate(D);
  Vector  resp  = VectorCreate(M);
  Matrix  xx    = MatrixCreate(D,D);
  Matrix  yy    = MatrixCreate(D,D);
  Matrix  mxD1  = MatrixCreate(D,1);
  Matrix  mx1D  = MatrixCreate(1,D);
  Vector  *means;
  Matrix  *invcovs;
  double  sum;
  double  likelihood = 0;
  Vector  *firstMoments;
  Matrix  *secondMoments;
  double  *mix;

  /* Allocate Model and Moments */
  means         = (Vector *) malloc(M*sizeof(Vector));
  invcovs       = (Matrix *) malloc(M*sizeof(Matrix));
  firstMoments  = (Vector *) malloc(M*sizeof(Vector));
  secondMoments = (Matrix *) malloc(M*sizeof(Matrix));
  mix           = (double *) malloc(M*sizeof(double));

    

  for (m=0; m<M; m++)
    {
      means[m]   = VectorCreate(D);
      invcovs[m] = MatrixCreate(D,D);
      
      memcpy(means[m]->d, &mu[m*D], D*sizeof(double));
      memcpy(invcovs[m]->d, &R[m*D*D], D*D*sizeof(double));


      firstMoments[m]  = VectorCreate(D);
      VectorSet(firstMoments[m], 0.0);
      secondMoments[m] = MatrixCreate(D,D);
      VectorSet((Vector) secondMoments[m], 0.0);
      mix[m] = 0.0;
    }
 
  /* 1 EM Iteration */
  for (i=0; i<N; i++)
    {
      memcpy(x->d, &current[i*D], D*sizeof(double));
      memcpy(mxD1->d, &current[i*D], D*sizeof(double));
      memcpy(mx1D->d, &current[i*D], D*sizeof(double));

      MatrixMultiply(xx, mxD1, mx1D);

      /* Expectation */
      for (m=0; m<M; m++)
	{
	  VectorMove(y, x);
	  VectorSubtract(y, means[m]);
	  MatrixVectorMultiply(z, invcovs[m], y);
	  resp->d[m] = scal[m]*exp(-0.5*VectorDot(z,y));
	}

      /* Normalization */
      sum   = VectorSum(resp);
      if (sum != 0)
	{
	  VectorScale(resp, 1.0/sum);
	  likelihood += log(sum);
	}

      /* Maximization */
      for (m=0; m<M; m++)
	{
	  VectorMove(y, x);
	  VectorScale(y, resp->d[m]);
	  VectorAdd(firstMoments[m], y);

	  VectorMove((Vector) yy, (Vector) xx);
	  VectorScale((Vector) yy, resp->d[m]);
	  VectorAdd((Vector) secondMoments[m], (Vector) yy);
	  
	  mix[m] += resp->d[m];
	}
    }

  /* Copy Model Out */
  sum = 0;
  for (m=0; m<M; m++)
    {
      VectorScale((Vector) firstMoments[m], 1.0/mix[m]);
      VectorScale((Vector) secondMoments[m], 1.0/mix[m]);

      memcpy(mxD1->d, firstMoments[m]->d, D*sizeof(double));
      memcpy(mx1D->d, firstMoments[m]->d, D*sizeof(double));
      MatrixMultiply(xx, mxD1, mx1D);
      VectorSubtract((Vector) secondMoments[m], (Vector) xx);
      VectorAdd((Vector) secondMoments[m], (Vector) regul);

      sum += mix[m];
    }


/*
   Make the Gaussians narrow by setting everything except
   the largest eigenvalue to a value of 'narrow'

  Matrix EVecs = MatrixCreate(D,D);
  Vector EVals = VectorCreate(D);
  for (m=0; m<M; m++)
    {
      VectorCopy((Vector) EVecs, (Vector) secondMoments);
      MatrixEigenvectorsSym(EVecs, EVals->d);
      for (d=0; d<(D-1); d++)
	EVals->d[d] = narrow;
      MatrixVectorMultiply(Vector dest, Matrix a, Vector v);
     Modifies dest to be the product Av.  dest may be the same as v.
     Requires: dest->len == a->rows, a->cols == v->len
      
    }
  VectorFree(EVals);
  MatrixFree(EVecs);
*/

  for (m=0; m<M; m++)
    {
      mix[m] /= sum;

      memcpy(&mu[m*D], firstMoments[m]->d, D*sizeof(double));
      MatrixInvert(secondMoments[m]);
      memcpy(&R[m*D*D], secondMoments[m]->d, D*D*sizeof(double));

      scal[m] = 1.0/(pow(2.0*pi,((double) D)*0.5))*sqrt(MatrixDeterminant(secondMoments[m]))*mix[m];
    }



  /* Free Model and Moments */
  VectorFree(x);
  VectorFree(y);
  VectorFree(z);
  VectorFree(resp);
  MatrixFree(xx);
  MatrixFree(yy);
  MatrixFree(mxD1);
  MatrixFree(mx1D);
  for (m=0; m<M; m++)
    {
      VectorFree(means[m]);
      MatrixFree(invcovs[m]);
      VectorFree(firstMoments[m]);
      MatrixFree(secondMoments[m]);
    }

  free(means);
  free(invcovs);
  free(firstMoments);
  free(secondMoments);
  free(mix);


  /* Return Average Likelihood Before the EM Step */
  likelihood /= N;
  return(likelihood);
}


double
updateParamsWeightedGaussianMixtureModelEM(double *current, double *weight, int N, int D, int M, double *mu, double *R, double *scal)
{
  FILE    *plotfp;
  int     i, d, d2, m;
  Vector  x     = VectorCreate(D);
  Vector  y     = VectorCreate(D);
  Vector  z     = VectorCreate(D);
  Vector  resp  = VectorCreate(M);
  Matrix  xx    = MatrixCreate(D,D);
  Matrix  yy    = MatrixCreate(D,D);
  Matrix  mxD1  = MatrixCreate(D,1);
  Matrix  mx1D  = MatrixCreate(1,D);
  Vector  *means;
  Matrix  *invcovs;
  double  sum;
  double  likelihood = 0;
  Vector  *firstMoments;
  Matrix  *secondMoments;
  double  *mix;

  /* Allocate Model and Moments */
  means         = (Vector *) malloc(M*sizeof(Vector));
  invcovs       = (Matrix *) malloc(M*sizeof(Matrix));
  firstMoments  = (Vector *) malloc(M*sizeof(Vector));
  secondMoments = (Matrix *) malloc(M*sizeof(Matrix));
  mix           = (double *) malloc(M*sizeof(double));


  for (m=0; m<M; m++)
    {
      means[m]   = VectorCreate(D);
      invcovs[m] = MatrixCreate(D,D);
      
      memcpy(means[m]->d, &mu[m*D], D*sizeof(double));
      memcpy(invcovs[m]->d, &R[m*D*D], D*D*sizeof(double));


      firstMoments[m]  = VectorCreate(D);
      VectorSet(firstMoments[m], 0.0);
      secondMoments[m] = MatrixCreate(D,D);
      VectorSet((Vector) secondMoments[m], 0.0);
      mix[m] = 0.0;
    }
 
  /* 1 EM Iteration */
  for (i=0; i<N; i++)
    {
      memcpy(x->d, &current[i*D], D*sizeof(double));
      memcpy(mxD1->d, &current[i*D], D*sizeof(double));
      memcpy(mx1D->d, &current[i*D], D*sizeof(double));

      MatrixMultiply(xx, mxD1, mx1D);

      /* Expectation */
      for (m=0; m<M; m++)
	{
	  VectorMove(y, x);
	  VectorSubtract(y, means[m]);
	  MatrixVectorMultiply(z, invcovs[m], y);
	  resp->d[m] = scal[m]*exp(-0.5*VectorDot(z,y));
	}

      /* Normalization */
      sum   = VectorSum(resp);
      if (sum != 0)
	{
	  VectorScale(resp, 1.0/sum);
	  likelihood += weight[i]*log(sum);
	}

      VectorScale(resp, weight[i]);

      /* Maximization */
      for (m=0; m<M; m++)
	{
	  VectorMove(y, x);
	  VectorScale(y, resp->d[m]);
	  VectorAdd(firstMoments[m], y);

	  VectorMove((Vector) yy, (Vector) xx);
	  VectorScale((Vector) yy, resp->d[m]);
	  VectorAdd((Vector) secondMoments[m], (Vector) yy);
	  
	  mix[m] += resp->d[m];
	}
    }

  /* Copy Model Out */
  sum = 0;
  for (m=0; m<M; m++)
    {
      VectorScale((Vector) firstMoments[m], 1.0/mix[m]);
      VectorScale((Vector) secondMoments[m], 1.0/mix[m]);

      memcpy(mxD1->d, firstMoments[m]->d, D*sizeof(double));
      memcpy(mx1D->d, firstMoments[m]->d, D*sizeof(double));
      MatrixMultiply(xx, mxD1, mx1D);
      VectorSubtract((Vector) secondMoments[m], (Vector) xx);
      VectorAdd((Vector) secondMoments[m], (Vector) regul);

      sum += mix[m];
    }


#ifdef DEBUG_MATLAB
  plotfp = fopen("plotme.m","w");
  fprintf(plotfp,"function [mu, c, mix] = plotme()\n");
  fprintf(plotfp, "mu=[");
  for (m=0; m<M; m++)
    {
      for (d=0; d<D; d++)
	fprintf(plotfp,"%f ",firstMoments[m]->d[d]);
      fprintf(plotfp,";");
    }
  fprintf(plotfp, "];\n");
  fprintf(plotfp, "c=[");
  for (m=0; m<M; m++)
    for (d=0; d<D; d++)
      {
	for (d2=0; d2<D; d2++)
	  fprintf(plotfp,"%f ",secondMoments[m]->d2[d][d2]);
	fprintf(plotfp,";");
      }
  fprintf(plotfp, "];\n");
  fprintf(plotfp, "mix=[");
  for (m=0; m<M; m++) fprintf(plotfp,"%f ",mix[m]/sum);
  fprintf(plotfp, "];\n");

  if (D==3)
    fprintf(plotfp,"plotClust3D(mu,c,1,2,3);\nview([30 50]);\n");
  if (D==2)
    fprintf(plotfp,"plotClust(mu,c,1,2);\n");
    
  fclose(plotfp);
#endif



  for (m=0; m<M; m++)
    {
      mix[m] /= sum;

      memcpy(&mu[m*D], firstMoments[m]->d, D*sizeof(double));
      MatrixInvert(secondMoments[m]);
      memcpy(&R[m*D*D], secondMoments[m]->d, D*D*sizeof(double));

      scal[m] = 1.0/(pow(2.0*pi,((double) D)*0.5))*sqrt(MatrixDeterminant(secondMoments[m]))*mix[m];
    }



  /* Free Model and Moments */
  VectorFree(x);
  VectorFree(y);
  VectorFree(z);
  VectorFree(resp);
  MatrixFree(xx);
  MatrixFree(yy);
  MatrixFree(mxD1);
  MatrixFree(mx1D);
  for (m=0; m<M; m++)
    {
      VectorFree(means[m]);
      MatrixFree(invcovs[m]);
      VectorFree(firstMoments[m]);
      MatrixFree(secondMoments[m]);
    }

  free(means);
  free(invcovs);
  free(firstMoments);
  free(secondMoments);
  free(mix);
  

  /* Return Average Likelihood Before the EM Step */
  likelihood /= N;
  return(likelihood);
}

double
updateParamsWeightedGaussianMixtureModelEMANNEALED(double *current, double *weight, int N, int D, int M, double *mu, double *R, double *scal, double T)
{
  FILE    *plotfp;
  int     i, d, d2, m;
  Vector  x     = VectorCreate(D);
  Vector  y     = VectorCreate(D);
  Vector  z     = VectorCreate(D);
  Vector  resp  = VectorCreate(M);
  Matrix  xx    = MatrixCreate(D,D);
  Matrix  yy    = MatrixCreate(D,D);
  Matrix  mxD1  = MatrixCreate(D,1);
  Matrix  mx1D  = MatrixCreate(1,D);
  Vector  *means;
  Matrix  *invcovs;
  double  sum;
  double  likelihood = 0;
  Vector  *firstMoments;
  Matrix  *secondMoments;
  double  *mix;

  /* Allocate Model and Moments */
  means         = (Vector *) malloc(M*sizeof(Vector));
  invcovs       = (Matrix *) malloc(M*sizeof(Matrix));
  firstMoments  = (Vector *) malloc(M*sizeof(Vector));
  secondMoments = (Matrix *) malloc(M*sizeof(Matrix));
  mix           = (double *) malloc(M*sizeof(double));


  for (m=0; m<M; m++)
    {
      means[m]   = VectorCreate(D);
      invcovs[m] = MatrixCreate(D,D);
      
      memcpy(means[m]->d, &mu[m*D], D*sizeof(double));
      memcpy(invcovs[m]->d, &R[m*D*D], D*D*sizeof(double));


      firstMoments[m]  = VectorCreate(D);
      VectorSet(firstMoments[m], 0.0);
      secondMoments[m] = MatrixCreate(D,D);
      VectorSet((Vector) secondMoments[m], 0.0);
      mix[m] = 0.0;
    }
 
  /* 1 EM Iteration */
  for (i=0; i<N; i++)
    {
      memcpy(x->d, &current[i*D], D*sizeof(double));
      memcpy(mxD1->d, &current[i*D], D*sizeof(double));
      memcpy(mx1D->d, &current[i*D], D*sizeof(double));

      MatrixMultiply(xx, mxD1, mx1D);

      /* Expectation */
      for (m=0; m<M; m++)
	{
	  VectorMove(y, x);
	  VectorSubtract(y, means[m]);
	  MatrixVectorMultiply(z, invcovs[m], y);
	  resp->d[m] = scal[m]*exp(-0.5*VectorDot(z,y));
	  resp->d[m] = powf(resp->d[m],1.0/T);
	}

      /* Normalization */
      sum   = VectorSum(resp);
      if (sum != 0)
	{
	  VectorScale(resp, 1.0/sum);
	  likelihood += weight[i]*log(sum);
	}

      VectorScale(resp, weight[i]);

      /* Maximization */
      for (m=0; m<M; m++)
	{
	  VectorMove(y, x);
	  VectorScale(y, resp->d[m]);
	  VectorAdd(firstMoments[m], y);

	  VectorMove((Vector) yy, (Vector) xx);
	  VectorScale((Vector) yy, resp->d[m]);
	  VectorAdd((Vector) secondMoments[m], (Vector) yy);
	  
	  mix[m] += resp->d[m];
	}
    }

  /* Copy Model Out */
  sum = 0;
  for (m=0; m<M; m++)
    {
      VectorScale((Vector) firstMoments[m], 1.0/mix[m]);
      VectorScale((Vector) secondMoments[m], 1.0/mix[m]);

      memcpy(mxD1->d, firstMoments[m]->d, D*sizeof(double));
      memcpy(mx1D->d, firstMoments[m]->d, D*sizeof(double));
      MatrixMultiply(xx, mxD1, mx1D);
      VectorSubtract((Vector) secondMoments[m], (Vector) xx);
      VectorAdd((Vector) secondMoments[m], (Vector) regul);

      sum += mix[m];
    }


#ifdef DEBUG_MATLAB
  plotfp = fopen("plotme.m","w");
  fprintf(plotfp,"function [mu, c, mix] = plotme()\n");
  fprintf(plotfp, "mu=[");
  for (m=0; m<M; m++)
    {
      for (d=0; d<D; d++)
	fprintf(plotfp,"%f ",firstMoments[m]->d[d]);
      fprintf(plotfp,";");
    }
  fprintf(plotfp, "];\n");
  fprintf(plotfp, "c=[");
  for (m=0; m<M; m++)
    for (d=0; d<D; d++)
      {
	for (d2=0; d2<D; d2++)
	  fprintf(plotfp,"%f ",secondMoments[m]->d2[d][d2]);
	fprintf(plotfp,";");
      }
  fprintf(plotfp, "];\n");
  fprintf(plotfp, "mix=[");
  for (m=0; m<M; m++) fprintf(plotfp,"%f ",mix[m]/sum);
  fprintf(plotfp, "];\n");

  if (D==3)
    fprintf(plotfp,"plotClust3D(mu,c,1,2,3);\nview([30 50]);\n");
  if (D==2)
    fprintf(plotfp,"plotClust(mu,c,1,2);\n");
    
  fclose(plotfp);
#endif



  for (m=0; m<M; m++)
    {
      mix[m] /= sum;

      memcpy(&mu[m*D], firstMoments[m]->d, D*sizeof(double));
      MatrixInvert(secondMoments[m]);
      memcpy(&R[m*D*D], secondMoments[m]->d, D*D*sizeof(double));

      scal[m] = 1.0/(pow(2.0*pi,((double) D)*0.5))*sqrt(MatrixDeterminant(secondMoments[m]))*mix[m];
    }



  /* Free Model and Moments */
  VectorFree(x);
  VectorFree(y);
  VectorFree(z);
  VectorFree(resp);
  MatrixFree(xx);
  MatrixFree(yy);
  MatrixFree(mxD1);
  MatrixFree(mx1D);
  for (m=0; m<M; m++)
    {
      VectorFree(means[m]);
      MatrixFree(invcovs[m]);
      VectorFree(firstMoments[m]);
      MatrixFree(secondMoments[m]);
    }

  free(means);
  free(invcovs);
  free(firstMoments);
  free(secondMoments);
  free(mix);
  

  /* Return Average Likelihood Before the EM Step */
  likelihood /= N;
  return(likelihood);
}