[valse.git] / pkg / src / EMGrank.c

#include <stdlib.h>
#include <gsl/gsl_linalg.h>
#include "utils.h"

// Compute pseudo-inverse of a square matrix
static Real* pinv(const Real* matrix, int dim)
{
  gsl_matrix* U = gsl_matrix_alloc(dim,dim);
  gsl_matrix* V = gsl_matrix_alloc(dim,dim);
  gsl_vector* S = gsl_vector_alloc(dim);
  gsl_vector* work = gsl_vector_alloc(dim);
  Real EPS = 1e-10; //threshold for singular value "== 0"
  
  //copy matrix into U
  copyArray(matrix, U->data, dim*dim);

  //U,S,V = SVD of matrix
  gsl_linalg_SV_decomp(U, V, S, work);
  gsl_vector_free(work);

  // Obtain pseudo-inverse by V*S^{-1}*t(U)
  Real* inverse = (Real*)malloc(dim*dim*sizeof(Real));
  for (int i=0; i<dim; i++)
  {
    for (int ii=0; ii<dim; ii++)
    {
      Real dotProduct = 0.0;
      for (int j=0; j<dim; j++)
        dotProduct += V->data[i*dim+j] * (S->data[j] > EPS ? 1.0/S->data[j] : 0.0) * U->data[ii*dim+j];
      inverse[i*dim+ii] = dotProduct;
    }
  }
  
  gsl_matrix_free(U);
  gsl_matrix_free(V);
  gsl_vector_free(S);
  return inverse;
}

// TODO: comment EMGrank purpose
void EMGrank_core(
  // IN parameters
  const Real* Pi, // parametre de proportion
  const Real* Rho, // parametre initial de variance renormalise
  int mini, // nombre minimal d'iterations dans l'algorithme EM
  int maxi, // nombre maximal d'iterations dans l'algorithme EM
  const Real* X, // regresseurs
  const Real* Y, // reponse
  Real tau, // seuil pour accepter la convergence
  const int* rank, // vecteur des rangs possibles
  // OUT parameters
  Real* phi, // parametre de moyenne renormalise, calcule par l'EM
  Real* LLF, // log vraisemblance associe a cet echantillon, pour les valeurs estimees des parametres
  // additional size parameters
  int n, // taille de l'echantillon
  int p, // nombre de covariables
  int m, // taille de Y (multivarie)
  int k) // nombre de composantes
{
  // Allocations, initializations
  Real* Phi = (Real*)calloc(p*m*k,sizeof(Real));
  Real* hatBetaR = (Real*)malloc(p*m*sizeof(Real));
  int signum;
  Real invN = 1.0/n;
  int deltaPhiBufferSize = 20;
  Real* deltaPhi = (Real*)malloc(deltaPhiBufferSize*sizeof(Real));
  int ite = 0;
  Real sumDeltaPhi = 0.0;
  Real* YiRhoR = (Real*)malloc(m*sizeof(Real));
  Real* XiPhiR = (Real*)malloc(m*sizeof(Real));
  Real* Xr = (Real*)malloc(n*p*sizeof(Real));
  Real* Yr = (Real*)malloc(n*m*sizeof(Real));
  Real* tXrXr = (Real*)malloc(p*p*sizeof(Real));
  Real* tXrYr = (Real*)malloc(p*m*sizeof(Real));
  gsl_matrix* matrixM = gsl_matrix_alloc(p, m);
  gsl_matrix* matrixE = gsl_matrix_alloc(m, m);
  gsl_permutation* permutation = gsl_permutation_alloc(m);
  gsl_matrix* V = gsl_matrix_alloc(m,m);
  gsl_vector* S = gsl_vector_alloc(m);
  gsl_vector* work = gsl_vector_alloc(m);

  //Initialize class memberships (all elements in class 0; TODO: randomize ?)
  int* Z = (int*)calloc(n, sizeof(int));

  //Initialize phi to zero, because some M loops might exit before phi affectation
  zeroArray(phi, p*m*k);

  while (ite<mini || (ite<maxi && sumDeltaPhi>tau))
  {
    /////////////
    // Etape M //
    /////////////
    
    //M step: Mise a jour de Beta (et donc phi)
    for (int r=0; r<k; r++)
    {
      //Compute Xr = X(Z==r,:) and Yr = Y(Z==r,:)
      int cardClustR=0;
      for (int i=0; i<n; i++)
      {
        if (Z[i] == r)
        {
          for (int j=0; j<p; j++)
            Xr[mi(cardClustR,j,n,p)] = X[mi(i,j,n,p)];
          for (int j=0; j<m; j++)
            Yr[mi(cardClustR,j,n,m)] = Y[mi(i,j,n,m)];
          cardClustR++;
        }
      }
      if (cardClustR == 0)
        continue;

      //Compute tXrXr = t(Xr) * Xr
      for (int j=0; j<p; j++)
      {
        for (int jj=0; jj<p; jj++)
        {
          Real dotProduct = 0.0;
          for (int u=0; u<cardClustR; u++)
            dotProduct += Xr[mi(u,j,n,p)] * Xr[mi(u,jj,n,p)];
          tXrXr[mi(j,jj,p,p)] = dotProduct;
        }
      }

      //Get pseudo inverse = (t(Xr)*Xr)^{-1}
      Real* invTXrXr = pinv(tXrXr, p);

      // Compute tXrYr = t(Xr) * Yr
      for (int j=0; j<p; j++)
      {
        for (int jj=0; jj<m; jj++)
        {
          Real dotProduct = 0.0;
          for (int u=0; u<cardClustR; u++)
            dotProduct += Xr[mi(u,j,n,p)] * Yr[mi(u,jj,n,m)];
          tXrYr[mi(j,jj,p,m)] = dotProduct;
        }
      }

      //Fill matrixM with inverse * tXrYr = (t(Xr)*Xr)^{-1} * t(Xr) * Yr
      for (int j=0; j<p; j++)
      {
        for (int jj=0; jj<m; jj++)
        {
          Real dotProduct = 0.0;
          for (int u=0; u<p; u++)
            dotProduct += invTXrXr[mi(j,u,p,p)] * tXrYr[mi(u,jj,p,m)];
          matrixM->data[j*m+jj] = dotProduct;
        }
      }
      free(invTXrXr);

      //U,S,V = SVD of (t(Xr)Xr)^{-1} * t(Xr) * Yr
      if (p >= m)
        gsl_linalg_SV_decomp(matrixM, V, S, work);
      else
      {
        gsl_matrix* matrixM_ = gsl_matrix_alloc(m, p);
        for (int j=0; j<m; j++)
        {
          for (int jj=0; jj<p; jj++)
            matrixM_->data[jj*m+j] = matrixM->data[j*p +jj];
        }
        gsl_matrix* V_ = gsl_matrix_alloc(p,p);
        gsl_vector* S_ = gsl_vector_alloc(p);
        gsl_vector* work_ = gsl_vector_alloc(p);
        gsl_linalg_SV_decomp(matrixM_, V_, S_, work_);
        gsl_vector_free(work_);
        for (int j=0; j<m; j++)
        {
          if (j < p)
            S->data[j] = S_->data[j];
          else
            S->data[j] = 0.0;
          for (int jj=0; jj<m; jj++)
          {
            if (j < p && jj < p)
              V->data[jj * m + j] = V_->data[jj * m + j];
            else
              V->data[jj * m + j] = 0.0;
          }
        }
        for (int j=0; j<m; j++)
        {
          for (int jj=0; jj<p; jj++)
            matrixM->data[j*p+jj] = matrixM_->data[jj*m +j];
        }
        gsl_matrix_free(matrixM_);
        gsl_matrix_free(V_);
        gsl_vector_free(S_);
      }

      //Set m-rank(r) singular values to zero, and recompose
      //best rank(r) approximation of the initial product
      for (int j=rank[r]; j<m; j++)
        S->data[j] = 0.0;
      
      //[intermediate step] Compute hatBetaR = U * S * t(V)
      double* U = matrixM->data; //GSL require double precision
      for (int j=0; j<p; j++)
      {
        for (int jj=0; jj<m; jj++)
        {
          Real dotProduct = 0.0;
          for (int u=0; u<m; u++)
            dotProduct += U[j*m+u] * S->data[u] * V->data[jj*m+u];
          hatBetaR[mi(j,jj,p,m)] = dotProduct;
        }
      }

      //Compute phi(:,:,r) = hatBetaR * Rho(:,:,r)
      for (int j=0; j<p; j++)
      {
        for (int jj=0; jj<m; jj++)
        {
          Real dotProduct=0.0;
          for (int u=0; u<m; u++)
            dotProduct += hatBetaR[mi(j,u,p,m)] * Rho[ai(u,jj,r,m,m,k)];
          phi[ai(j,jj,r,p,m,k)] = dotProduct;
        }
      }
    }

    /////////////
    // Etape E //
    /////////////
    
    Real sumLogLLF2 = 0.0;
    for (int i=0; i<n; i++)
    {
      Real sumLLF1 = 0.0;
      Real maxLogGamIR = -INFINITY;
      for (int r=0; r<k; r++)
      {
        //Compute
        //Gam(i,r) = Pi(r) * det(Rho(:,:,r)) * exp( -1/2 * (Y(i,:)*Rho(:,:,r) - X(i,:)...
        //*phi(:,:,r)) * transpose( Y(i,:)*Rho(:,:,r) - X(i,:)*phi(:,:,r) ) );
        //split in several sub-steps
        
        //compute det(Rho(:,:,r)) [TODO: avoid re-computations]
        for (int j=0; j<m; j++)
        {
          for (int jj=0; jj<m; jj++)
            matrixE->data[j*m+jj] = Rho[ai(j,jj,r,m,m,k)];
        }
        gsl_linalg_LU_decomp(matrixE, permutation, &signum);
        Real detRhoR = gsl_linalg_LU_det(matrixE, signum);

        //compute Y(i,:)*Rho(:,:,r)
        for (int j=0; j<m; j++)
        {
          YiRhoR[j] = 0.0;
          for (int u=0; u<m; u++)
            YiRhoR[j] += Y[mi(i,u,n,m)] * Rho[ai(u,j,r,m,m,k)];
        }

        //compute X(i,:)*phi(:,:,r)
        for (int j=0; j<m; j++)
        {
          XiPhiR[j] = 0.0;
          for (int u=0; u<p; u++)
            XiPhiR[j] += X[mi(i,u,n,p)] * phi[ai(u,j,r,p,m,k)];
        }

        //compute dotProduct < Y(:,i)*rho(:,:,r)-X(i,:)*phi(:,:,r) . Y(:,i)*rho(:,:,r)-X(i,:)*phi(:,:,r) >
        Real dotProduct = 0.0;
        for (int u=0; u<m; u++)
          dotProduct += (YiRhoR[u]-XiPhiR[u]) * (YiRhoR[u]-XiPhiR[u]);
        Real logGamIR = log(Pi[r]) + log(detRhoR) - 0.5*dotProduct;

        //Z(i) = index of max (gam(i,:))
        if (logGamIR > maxLogGamIR)
        {
          Z[i] = r;
          maxLogGamIR = logGamIR;
        }
        sumLLF1 += exp(logGamIR) / pow(2*M_PI,m/2.0);
      }

      sumLogLLF2 += log(sumLLF1);
    }

    // Assign output variable LLF
    *LLF = -invN * sumLogLLF2;

    //newDeltaPhi = max(max((abs(phi-Phi))./(1+abs(phi))));
    Real newDeltaPhi = 0.0;
    for (int j=0; j<p; j++)
    {
      for (int jj=0; jj<m; jj++)
      {
        for (int r=0; r<k; r++)
        {
          Real tmpDist = fabs(phi[ai(j,jj,r,p,m,k)]-Phi[ai(j,jj,r,p,m,k)])
            / (1.0+fabs(phi[ai(j,jj,r,p,m,k)]));
          if (tmpDist > newDeltaPhi)
            newDeltaPhi = tmpDist;
        }
      }
    }

    //update distance parameter to check algorithm convergence (delta(phi, Phi))
    //TODO: deltaPhi should be a linked list for perf.
    if (ite < deltaPhiBufferSize)
      deltaPhi[ite] = newDeltaPhi;
    else
    {
      sumDeltaPhi -= deltaPhi[0];
      for (int u=0; u<deltaPhiBufferSize-1; u++)
        deltaPhi[u] = deltaPhi[u+1];
      deltaPhi[deltaPhiBufferSize-1] = newDeltaPhi;
    }
    sumDeltaPhi += newDeltaPhi;

    // update other local variables
    for (int j=0; j<m; j++)
    {
      for (int jj=0; jj<p; jj++)
      {
        for (int r=0; r<k; r++)
          Phi[ai(jj,j,r,p,m,k)] = phi[ai(jj,j,r,p,m,k)];
      }
    }
    ite++;
  }

  //free memory
  free(hatBetaR);
  free(deltaPhi);
  free(Phi);
  gsl_matrix_free(matrixE);
  gsl_matrix_free(matrixM);
  gsl_permutation_free(permutation);
  gsl_vector_free(work);
  gsl_matrix_free(V);
  gsl_vector_free(S);
  free(XiPhiR);
  free(YiRhoR);
  free(Xr);
  free(Yr);
  free(tXrXr);
  free(tXrYr);
  free(Z);
}
Commit	Line	Data
228ee602	1	#include <stdlib.h>
	2	#include <gsl/gsl_linalg.h>
	3	#include "utils.h"
	4
	5	// Compute pseudo-inverse of a square matrix
	6	static Real* pinv(const Real* matrix, int dim)
	7	{
93dcdea3 BA	8	gsl_matrix* U = gsl_matrix_alloc(dim,dim);
	9	gsl_matrix* V = gsl_matrix_alloc(dim,dim);
	10	gsl_vector* S = gsl_vector_alloc(dim);
	11	gsl_vector* work = gsl_vector_alloc(dim);
	12	Real EPS = 1e-10; //threshold for singular value "== 0"
	13
	14	//copy matrix into U
	15	copyArray(matrix, U->data, dim*dim);
228ee602	16
93dcdea3 BA	17	//U,S,V = SVD of matrix
	18	gsl_linalg_SV_decomp(U, V, S, work);
	19	gsl_vector_free(work);
228ee602	20
93dcdea3 BA	21	// Obtain pseudo-inverse by VS^{-1}t(U)
	22	Real* inverse = (Real)malloc(dimdim*sizeof(Real));
	23	for (int i=0; i<dim; i++)
	24	{
	25	for (int ii=0; ii<dim; ii++)
	26	{
	27	Real dotProduct = 0.0;
	28	for (int j=0; j<dim; j++)
	29	dotProduct += V->data[idim+j] (S->data[j] > EPS ? 1.0/S->data[j] : 0.0) * U->data[ii*dim+j];
	30	inverse[i*dim+ii] = dotProduct;
	31	}
	32	}
	33
	34	gsl_matrix_free(U);
	35	gsl_matrix_free(V);
	36	gsl_vector_free(S);
	37	return inverse;
228ee602	38	}
	39
	40	// TODO: comment EMGrank purpose
	41	void EMGrank_core(
93dcdea3 BA	42	// IN parameters
	43	const Real* Pi, // parametre de proportion
	44	const Real* Rho, // parametre initial de variance renormalise
	45	int mini, // nombre minimal d'iterations dans l'algorithme EM
	46	int maxi, // nombre maximal d'iterations dans l'algorithme EM
	47	const Real* X, // regresseurs
	48	const Real* Y, // reponse
	49	Real tau, // seuil pour accepter la convergence
	50	const int* rank, // vecteur des rangs possibles
	51	// OUT parameters
	52	Real* phi, // parametre de moyenne renormalise, calcule par l'EM
	53	Real* LLF, // log vraisemblance associe a cet echantillon, pour les valeurs estimees des parametres
	54	// additional size parameters
	55	int n, // taille de l'echantillon
	56	int p, // nombre de covariables
	57	int m, // taille de Y (multivarie)
	58	int k) // nombre de composantes
228ee602	59	{
93dcdea3 BA	60	// Allocations, initializations
	61	Real* Phi = (Real)calloc(pm*k,sizeof(Real));
	62	Real* hatBetaR = (Real)malloc(pm*sizeof(Real));
	63	int signum;
	64	Real invN = 1.0/n;
	65	int deltaPhiBufferSize = 20;
	66	Real* deltaPhi = (Real)malloc(deltaPhiBufferSizesizeof(Real));
	67	int ite = 0;
	68	Real sumDeltaPhi = 0.0;
	69	Real* YiRhoR = (Real)malloc(msizeof(Real));
	70	Real* XiPhiR = (Real)malloc(msizeof(Real));
	71	Real* Xr = (Real)malloc(np*sizeof(Real));
	72	Real* Yr = (Real)malloc(nm*sizeof(Real));
	73	Real* tXrXr = (Real)malloc(pp*sizeof(Real));
	74	Real* tXrYr = (Real)malloc(pm*sizeof(Real));
	75	gsl_matrix* matrixM = gsl_matrix_alloc(p, m);
	76	gsl_matrix* matrixE = gsl_matrix_alloc(m, m);
	77	gsl_permutation* permutation = gsl_permutation_alloc(m);
	78	gsl_matrix* V = gsl_matrix_alloc(m,m);
	79	gsl_vector* S = gsl_vector_alloc(m);
	80	gsl_vector* work = gsl_vector_alloc(m);
228ee602	81
93dcdea3 BA	82	//Initialize class memberships (all elements in class 0; TODO: randomize ?)
93dcdea3 BA	83	int* Z = (int*)calloc(n, sizeof(int));
228ee602	84
93dcdea3 BA	85	//Initialize phi to zero, because some M loops might exit before phi affectation
93dcdea3 BA	86	zeroArray(phi, pmk);
228ee602	87
93dcdea3 BA	88	while (ite<mini \|\| (ite<maxi && sumDeltaPhi>tau))
	89	{
	90	/////////////
	91	// Etape M //
	92	/////////////
	93
	94	//M step: Mise a jour de Beta (et donc phi)
	95	for (int r=0; r<k; r++)
	96	{
	97	//Compute Xr = X(Z==r,:) and Yr = Y(Z==r,:)
	98	int cardClustR=0;
	99	for (int i=0; i<n; i++)
	100	{
	101	if (Z[i] == r)
	102	{
	103	for (int j=0; j<p; j++)
	104	Xr[mi(cardClustR,j,n,p)] = X[mi(i,j,n,p)];
	105	for (int j=0; j<m; j++)
	106	Yr[mi(cardClustR,j,n,m)] = Y[mi(i,j,n,m)];
	107	cardClustR++;
	108	}
	109	}
	110	if (cardClustR == 0)
	111	continue;
228ee602	112
93dcdea3 BA	113	//Compute tXrXr = t(Xr) * Xr
	114	for (int j=0; j<p; j++)
	115	{
	116	for (int jj=0; jj<p; jj++)
	117	{
	118	Real dotProduct = 0.0;
	119	for (int u=0; u<cardClustR; u++)
	120	dotProduct += Xr[mi(u,j,n,p)] * Xr[mi(u,jj,n,p)];
	121	tXrXr[mi(j,jj,p,p)] = dotProduct;
	122	}
	123	}
228ee602	124
93dcdea3 BA	125	//Get pseudo inverse = (t(Xr)*Xr)^{-1}
93dcdea3 BA	126	Real* invTXrXr = pinv(tXrXr, p);
228ee602	127
93dcdea3 BA	128	// Compute tXrYr = t(Xr) * Yr
	129	for (int j=0; j<p; j++)
	130	{
	131	for (int jj=0; jj<m; jj++)
	132	{
	133	Real dotProduct = 0.0;
	134	for (int u=0; u<cardClustR; u++)
	135	dotProduct += Xr[mi(u,j,n,p)] * Yr[mi(u,jj,n,m)];
	136	tXrYr[mi(j,jj,p,m)] = dotProduct;
	137	}
	138	}
228ee602	139
93dcdea3 BA	140	//Fill matrixM with inverse * tXrYr = (t(Xr)Xr)^{-1} t(Xr) * Yr
	141	for (int j=0; j<p; j++)
	142	{
	143	for (int jj=0; jj<m; jj++)
	144	{
	145	Real dotProduct = 0.0;
	146	for (int u=0; u<p; u++)
	147	dotProduct += invTXrXr[mi(j,u,p,p)] * tXrYr[mi(u,jj,p,m)];
	148	matrixM->data[j*m+jj] = dotProduct;
	149	}
	150	}
	151	free(invTXrXr);
228ee602	152
93dcdea3 BA	153	//U,S,V = SVD of (t(Xr)Xr)^{-1} * t(Xr) * Yr
	154	if (p >= m)
	155	gsl_linalg_SV_decomp(matrixM, V, S, work);
	156	else
	157	{
	158	gsl_matrix* matrixM_ = gsl_matrix_alloc(m, p);
	159	for (int j=0; j<m; j++)
	160	{
	161	for (int jj=0; jj<p; jj++)
	162	matrixM_->data[jjm+j] = matrixM->data[jp +jj];
	163	}
	164	gsl_matrix* V_ = gsl_matrix_alloc(p,p);
	165	gsl_vector* S_ = gsl_vector_alloc(p);
	166	gsl_vector* work_ = gsl_vector_alloc(p);
	167	gsl_linalg_SV_decomp(matrixM_, V_, S_, work_);
	168	gsl_vector_free(work_);
	169	for (int j=0; j<m; j++)
	170	{
	171	if (j < p)
	172	S->data[j] = S_->data[j];
	173	else
	174	S->data[j] = 0.0;
	175	for (int jj=0; jj<m; jj++)
	176	{
	177	if (j < p && jj < p)
	178	V->data[jj * m + j] = V_->data[jj * m + j];
	179	else
	180	V->data[jj * m + j] = 0.0;
	181	}
	182	}
	183	for (int j=0; j<m; j++)
	184	{
	185	for (int jj=0; jj<p; jj++)
	186	matrixM->data[jp+jj] = matrixM_->data[jjm +j];
	187	}
	188	gsl_matrix_free(matrixM_);
	189	gsl_matrix_free(V_);
	190	gsl_vector_free(S_);
	191	}
228ee602	192
93dcdea3 BA	193	//Set m-rank(r) singular values to zero, and recompose
	194	//best rank(r) approximation of the initial product
	195	for (int j=rank[r]; j<m; j++)
	196	S->data[j] = 0.0;
	197
	198	//[intermediate step] Compute hatBetaR = U * S * t(V)
	199	double* U = matrixM->data; //GSL require double precision
	200	for (int j=0; j<p; j++)
	201	{
	202	for (int jj=0; jj<m; jj++)
	203	{
	204	Real dotProduct = 0.0;
	205	for (int u=0; u<m; u++)
	206	dotProduct += U[jm+u] S->data[u] * V->data[jj*m+u];
	207	hatBetaR[mi(j,jj,p,m)] = dotProduct;
	208	}
	209	}
228ee602	210
93dcdea3 BA	211	//Compute phi(:,:,r) = hatBetaR * Rho(:,:,r)
	212	for (int j=0; j<p; j++)
	213	{
	214	for (int jj=0; jj<m; jj++)
	215	{
	216	Real dotProduct=0.0;
	217	for (int u=0; u<m; u++)
	218	dotProduct += hatBetaR[mi(j,u,p,m)] * Rho[ai(u,jj,r,m,m,k)];
	219	phi[ai(j,jj,r,p,m,k)] = dotProduct;
	220	}
	221	}
	222	}
228ee602	223
93dcdea3 BA	224	/////////////
	225	// Etape E //
	226	/////////////
	227
	228	Real sumLogLLF2 = 0.0;
	229	for (int i=0; i<n; i++)
	230	{
	231	Real sumLLF1 = 0.0;
	232	Real maxLogGamIR = -INFINITY;
	233	for (int r=0; r<k; r++)
	234	{
	235	//Compute
	236	//Gam(i,r) = Pi(r) * det(Rho(:,:,r)) * exp( -1/2 * (Y(i,:)*Rho(:,:,r) - X(i,:)...
	237	//phi(:,:,r)) transpose( Y(i,:)Rho(:,:,r) - X(i,:)phi(:,:,r) ) );
	238	//split in several sub-steps
	239
	240	//compute det(Rho(:,:,r)) [TODO: avoid re-computations]
	241	for (int j=0; j<m; j++)
	242	{
	243	for (int jj=0; jj<m; jj++)
	244	matrixE->data[j*m+jj] = Rho[ai(j,jj,r,m,m,k)];
	245	}
	246	gsl_linalg_LU_decomp(matrixE, permutation, &signum);
	247	Real detRhoR = gsl_linalg_LU_det(matrixE, signum);
228ee602	248
93dcdea3 BA	249	//compute Y(i,:)*Rho(:,:,r)
	250	for (int j=0; j<m; j++)
	251	{
	252	YiRhoR[j] = 0.0;
	253	for (int u=0; u<m; u++)
	254	YiRhoR[j] += Y[mi(i,u,n,m)] * Rho[ai(u,j,r,m,m,k)];
	255	}
228ee602	256
93dcdea3 BA	257	//compute X(i,:)*phi(:,:,r)
	258	for (int j=0; j<m; j++)
	259	{
	260	XiPhiR[j] = 0.0;
	261	for (int u=0; u<p; u++)
	262	XiPhiR[j] += X[mi(i,u,n,p)] * phi[ai(u,j,r,p,m,k)];
	263	}
228ee602	264
93dcdea3 BA	265	//compute dotProduct < Y(:,i)rho(:,:,r)-X(i,:)phi(:,:,r) . Y(:,i)rho(:,:,r)-X(i,:)phi(:,:,r) >
	266	Real dotProduct = 0.0;
	267	for (int u=0; u<m; u++)
	268	dotProduct += (YiRhoR[u]-XiPhiR[u]) * (YiRhoR[u]-XiPhiR[u]);
	269	Real logGamIR = log(Pi[r]) + log(detRhoR) - 0.5*dotProduct;
228ee602	270
93dcdea3 BA	271	//Z(i) = index of max (gam(i,:))
	272	if (logGamIR > maxLogGamIR)
	273	{
	274	Z[i] = r;
	275	maxLogGamIR = logGamIR;
	276	}
	277	sumLLF1 += exp(logGamIR) / pow(2*M_PI,m/2.0);
	278	}
228ee602	279
93dcdea3 BA	280	sumLogLLF2 += log(sumLLF1);
93dcdea3 BA	281	}
228ee602	282
93dcdea3 BA	283	// Assign output variable LLF
93dcdea3 BA	284	LLF = -invN sumLogLLF2;
228ee602	285
93dcdea3 BA	286	//newDeltaPhi = max(max((abs(phi-Phi))./(1+abs(phi))));
	287	Real newDeltaPhi = 0.0;
	288	for (int j=0; j<p; j++)
	289	{
	290	for (int jj=0; jj<m; jj++)
	291	{
	292	for (int r=0; r<k; r++)
	293	{
	294	Real tmpDist = fabs(phi[ai(j,jj,r,p,m,k)]-Phi[ai(j,jj,r,p,m,k)])
	295	/ (1.0+fabs(phi[ai(j,jj,r,p,m,k)]));
	296	if (tmpDist > newDeltaPhi)
	297	newDeltaPhi = tmpDist;
	298	}
	299	}
	300	}
228ee602	301
93dcdea3 BA	302	//update distance parameter to check algorithm convergence (delta(phi, Phi))
	303	//TODO: deltaPhi should be a linked list for perf.
	304	if (ite < deltaPhiBufferSize)
	305	deltaPhi[ite] = newDeltaPhi;
	306	else
	307	{
	308	sumDeltaPhi -= deltaPhi[0];
	309	for (int u=0; u<deltaPhiBufferSize-1; u++)
	310	deltaPhi[u] = deltaPhi[u+1];
	311	deltaPhi[deltaPhiBufferSize-1] = newDeltaPhi;
	312	}
	313	sumDeltaPhi += newDeltaPhi;
228ee602	314
93dcdea3 BA	315	// update other local variables
	316	for (int j=0; j<m; j++)
	317	{
	318	for (int jj=0; jj<p; jj++)
	319	{
	320	for (int r=0; r<k; r++)
	321	Phi[ai(jj,j,r,p,m,k)] = phi[ai(jj,j,r,p,m,k)];
	322	}
	323	}
	324	ite++;
	325	}
	326
	327	//free memory
	328	free(hatBetaR);
	329	free(deltaPhi);
	330	free(Phi);
	331	gsl_matrix_free(matrixE);
	332	gsl_matrix_free(matrixM);
	333	gsl_permutation_free(permutation);
	334	gsl_vector_free(work);
	335	gsl_matrix_free(V);
	336	gsl_vector_free(S);
	337	free(XiPhiR);
	338	free(YiRhoR);
	339	free(Xr);
	340	free(Yr);
	341	free(tXrXr);
	342	free(tXrYr);
	343	free(Z);
228ee602	344	}