theta = Givens(A,m,p,q)  ;	if ( fabs(theta) > threshold ) {	  c = cos(theta);	  s = sin(theta);	  LeftRotSimple (A, m, m, p, q, c, s) ;  	  RightRotSimple(A, m, m, p, q, c, s) ;  	  LeftRotSimple (R, m, m, p, q, c, s) ;  	  encore = 1 ;	  rots++ ;	}      }  }  return rots ;}/* Joint diagonalization of a stack K of symmetric M*M matrices by Jacobi *//* returns the number of plane rotations executed */int JointDiago (double *A, double *R, int M, int K, double threshold) {  int rots = 0 ;  int more = 1 ;  int p, q ;  double theta, c, s ;    Identity(R, M) ;  while ( more > 0 )   /* One sweep through a stack of K symmetric M*M matrices.  */    {      more = 0 ;      for (p=0; p<M; p++)	for (q=p+1; q<M; q++) {	  theta = GivensStack(A,M,K,p,q)  ; 	  if (fabs(theta)>threshold) {	    c = cos(theta);	    s = sin(theta);	    LeftRotStack (A, M, M, K, p, q, c, s) ;  	    RightRotStack(A, M, M, K, p, q, c, s) ;  	    LeftRotSimple(R, M, M,    p, q, c, s) ;  	    rots++ ; 	    more = 1  ; /* any pair rotation triggers a new sweep */	  }	}    }  return rots ;}/* W = sqrt(inv(cov(X)))  */void ComputeWhitener (double *W, double *X, int n, int T)  {  double threshold_W  = RELATIVE_W_THRESHOLD /  sqrt((double) T) ;  double *Cov  = (double *) calloc(n*n,     sizeof(double)) ;   double rescale ;  int i,j ;  if (Cov == NULL) OutOfMemory() ;  EstCovMat (Cov, X, n, T) ;   Diago (Cov, W, n, threshold_W)  ;  for (i=0; i<n; i++) {    rescale= 1.0 / sqrt (Cov[i+i*n]) ;    for (j=0; j< n ; j++)       W[i+j*n] = rescale * W[i+j*n] ;  }  free(Cov);}/* X: nxT, C: nxnxn.  Computes a stack of n cumulant matrices.  */void EstCumMats ( double *C, double *X, int n, int T) {  double *x  ; /* pointer to a data vector in the data matrix  */  double *tm ; /* temp matrix */  double *R  ; /* EXX' : WE DO NOT ASSUME WHITE DATA */  double xk2, xijkk, xij ;  double ust = 1.0 / (float) T ;   int n2 = n*n ;  int n3 = n*n*n ;  int i,j,k,t, kdec, index ;  Message0(3, "Memory allocation and reset...\n");  tm = (double *) calloc(n*n, sizeof(double)) ;   R  = (double *) calloc(n*n, sizeof(double)) ;   if (tm == NULL || R == NULL) OutOfMemory() ;  for (i=0; i<n3; i++) C[i] = 0.0 ;  for (i=0; i<n2; i++) R[i] = 0.0 ;  Message0(3, "Computing some moments...\n");  for (t=0, x=X; t<T; t++, x+=n)    {      for (i=0; i<n; i++)   /* External product (and accumulate for the covariance)  */	for (j=i; j<n; j++) /* We do not set the symmetric parts yet */	  {	    xij        = x[i]*x[j] ;	    tm[i+j*n]  = xij  ;	    R[i+j*n]  += xij  ;	  }      /* Accumulate */      for (k=0; k<n; k++)	{	  xk2  = tm[k+k*n] ;       /* x_k^2 */	  kdec = k*n2 ;            /* pre_computed shift to address the k-th matrx */	  for (i=0; i<n; i++)	    for (j=i, index=i+i*n; j<n; j++, index+=n)	      C[index+kdec] += xk2 * tm[index] ; /* filling the lower part is postponed  */	}    }      Message0(3, "From moments to cumulants...\n");    /* Normalize and symmetrize the 2th-order moments*/  for (i=0; i<n; i++)    for (j=i; j<n; j++)      {	xij      = ust * R[i+j*n] ;	R[i+j*n] = xij  ;	R[j+i*n] = xij  ;      }    /* from moments to cumulants and symmetrization */  for (k=0, kdec=0; k<n; k++, kdec+=n2)    for (i=0; i<n; i++)      for (j=i; j<n; j++) {	xijkk	  = ust * C[i+j*n+kdec]       /* normalization */	  -       R[i+j*n]*R[k+k*n]   /* cumulant correction */	  - 2.0 * R[i+k*n]*R[j+k*n] ;	C[i+j*n+kdec] = xijkk ;	C[j+i*n+kdec] = xijkk ;      }  free(tm) ;  free(R) ;}#define MC(ii,jj,kk,ll)   C[ ii + jj*n + kk*n2 + ll*n3 ]/* X: nxT, C: nxnxnxn.  Computes the cumulant tensor.  */void EstCumTens ( double *C, double *X, int n, int T) {  int n2 = n*n ;  int n3 = n*n*n ;  int n4 = n*n*n*n ;  int i,j,k,l,t ;  double Cijkl, xi, xij, xijk, *x ;  double ust = 1.0 / (float) T ;   double *R = (double *) calloc(n*n, sizeof(double)) ;   /* To store Cov(x).  Recomputed: no whiteness assumption here*/  if (R == NULL) OutOfMemory() ;  for (i=0; i<n4; i++) C[i] = 0.0 ;  for (i=0; i<n2; i++) R[i] = 0.0 ;  Message0(3, "Computing 2nd order cumulants...\n");   /* accumulation */  for(t=0, x=X; t<T; t++, x+=n)    for (i=0; i<n; i++)      for (j=i; j<n; j++)	  R[i+j*n] += x[i] * x[j] ;  /* normalization and symmetrization */  for (i=0; i<n; i++)    for (j=i; j<n; j++) {      R[i+j*n] = ust * R[i+j*n] ;      R[j+i*n] = R[i+j*n] ;    }    Message0(3, "Computing 4th order cumulants...\n");  /* accumulation */  for(t=0, x=X; t<T; t++, x+=n)    for (i=0; i<n; i++) {      xi = x[i] ;      for (j=i; j<n; j++) {	xij = xi *x[j] ;	for (k=j; k<n; k++) {	  xijk = xij*x[k] ;	  for (l=k; l<n; l++) 	    MC(i,j,k,l) += xijk*x[l];	}      }    }  /* normalization, mom2cum, and symmetrization */  for (i=0; i<n; i++)     for (j=i; j<n; j++)       for (k=j; k<n; k++) 	for (l=k; l<n; l++) {	  Cijkl = ust *  MC(i,j,k,l) 	    - R[i+j*n] * R[k+l*n]	    - R[i+k*n] * R[j+l*n]	    - R[i+l*n] * R[j+k*n] ;		  MC(i,j,k,l)=Cijkl;MC(i,j,l,k)=Cijkl;MC(j,i,k,l)=Cijkl;MC(j,i,l,k)=Cijkl; /* ijxx  */	  MC(i,k,j,l)=Cijkl;MC(i,k,l,j)=Cijkl;MC(k,i,j,l)=Cijkl;MC(k,i,l,j)=Cijkl; /* ikxx  */	  MC(i,l,j,k)=Cijkl;MC(i,l,k,j)=Cijkl;MC(l,i,j,k)=Cijkl;MC(l,i,k,j)=Cijkl; /* ilxx  */	  MC(j,k,i,l)=Cijkl;MC(j,k,l,i)=Cijkl;MC(k,j,i,l)=Cijkl;MC(k,j,l,i)=Cijkl; /* jkxx  */	  MC(j,l,i,k)=Cijkl;MC(j,l,k,i)=Cijkl;MC(l,j,i,k)=Cijkl;MC(l,j,k,i)=Cijkl; /* jlxx  */	  MC(k,l,i,j)=Cijkl;MC(k,l,j,i)=Cijkl;MC(l,k,i,j)=Cijkl;MC(l,k,j,i)=Cijkl; /* klxx  */	}  free(R) ;}void MeanRemoval(double *X, int n, int T)  {  double sum ;  double ust = 1.0 / (double)T ;  int i, t, tstart, tstop ;    for (i=0; i<n; i++) {      tstart = i ;      tstop  = i + n*T ;      sum = 0.0 ;       for (t=tstart; t<tstop ; t+=n) sum  += X[t] ;      sum = ust * sum ; for (t=tstart; t<tstop ; t+=n) X[t] -= sum ;  }}/* _________________________________________________________________  */void Shibbs ( double *B, /* Output. Separating matrix. nbc*nbc */	      double *X, /* Input.  Data set nbc x nbs */	      int nbc,   /* Input.  Number of sensors  */	      int nbs    /* Input.  Number of samples  */	      ){  double threshold_JD = RELATIVE_JD_THRESHOLD / sqrt((double)nbs) ;    int rots = 1 ;  double *Transf  = (double *) calloc(nbc*nbc,         sizeof(double)) ;   double *CumMats = (double *) calloc(nbc*nbc*nbc,     sizeof(double)) ;   if ( Transf == NULL || CumMats == NULL ) OutOfMemory() ;  /* Init */  Message0(2, "Init...\n") ;  Identity(B, nbc);   MeanRemoval(X, nbc, nbs)  ;   Message0(2, "Whitening...\n") ;  ComputeWhitener(Transf, X, nbc, nbs)  ;   Transform (X, Transf,          nbc, nbs) ;  Transform (B, Transf,          nbc, nbc) ;  while (rots>0)    {      Message0(2, "Computing cumulant matrices...\n") ;      EstCumMats (CumMats, X,      nbc, nbs) ;      Message0(2, "Joint diagonalization...\n") ;      rots = JointDiago (CumMats, Transf,  nbc, nbc, threshold_JD) ;      MessageI(3, "Total number of plane rotations: %6i.\n", rots) ;      MessageF(3, "Size of the total rotation: %10.7e\n", NonIdentity(Transf,nbc) );      Message0(2, "Updating...\n") ;      Transform  (X, Transf,        nbc, nbs ) ;      Transform  (B, Transf,        nbc, nbc ) ;    }  free(Transf) ;   free(CumMats) ;}/* _________________________________________________________________  */void Jade (	     double *B, /* Output. Separating matrix. nbc*nbc */	     double *X, /* Input.  Data set nbc x nbs */	     int nbc,   /* Input.  Number of sensors  */	     int nbs    /* Input.  Number of samples  */	     ){  double threshold_JD = RELATIVE_JD_THRESHOLD / sqrt((double)nbs) ;  int rots = 1 ;  double *Transf  = (double *) calloc(nbc*nbc,         sizeof(double)) ;   double *CumTens = (double *) calloc(nbc*nbc*nbc*nbc, sizeof(double)) ;   if ( Transf == NULL || CumTens == NULL ) OutOfMemory() ;  /* Init */  Message0(2, "Init...\n") ;  Identity(B, nbc);   MeanRemoval(X, nbc, nbs)  ;   Message0(2, "Whitening...\n") ;  ComputeWhitener(Transf, X, nbc, nbs)  ;   Transform (X, Transf,          nbc, nbs) ;  Transform (B, Transf,          nbc, nbc) ;  Message0(2, "Estimating the cumulant tensor...\n") ;  EstCumTens (CumTens, X,      nbc, nbs) ;  Message0(2, "Joint diagonalization...\n") ;  rots = JointDiago (CumTens, Transf,  nbc, nbc*nbc, threshold_JD) ;  MessageI(3, "Total number of plane rotations: %6i.\n", rots) ;  MessageF(3, "Size of the total rotation: %10.7e\n", NonIdentity(Transf,nbc) )  ;  Message0(2, "Updating...\n") ;  Transform  (X, Transf,        nbc, nbs ) ;  Transform  (B, Transf,        nbc, nbc ) ;  free(Transf) ;   free(CumTens) ;}