/* ================================================================== *//*    Implements the Jade and the Shibbs algorithms   Copyright: JF Cardoso.  cardoso@tsi.enst.fr   This is essentially my first C program.  Educated comments are more   than welcome.    version 1.2   Jun. 05, 2002.   Version 1.1   Jan. 20, 1999.   Changes wrt 1.1     o Minor fix for new versions of mex (see History)   Changes wrt 1.0     o Switched to Matlab-wise vectorization of matrices      o Merged a few subroutines into their callers     o Implemented more C tricks to make the code more unscrutable     o Changed the moment estimating routines to prepare for a       read-from-file operation (the sensor loops are nested inside       the sample loops)     o Limited facility to control verbosity levels.  Messages directed       to sterr. To do:    o Address the convergence problem of Shibbs on (e.g.) Gaussian data.   o Control of convergence may/should be based on the variation of the objective     rather than one the size of the rotations (see above item).      o Smarter use of floating types: short for the data, long when      during moment estimation (issue of error accumulation).   o An `out of memory' should return an error code rather than exiting.*//* ================================================================== */#include <stdio.h>#include <stdlib.h>#include <math.h>#include "Matutil.h"#define VERBOSITY 0#define RELATIVE_JD_THRESHOLD  1.0e-4/* A `null' Jacobi rotation for joint diagonalization is smaller than   RELATIVE_JD_THRESHOLD/sqrt(T) where T is the number of samples */#define RELATIVE_W_THRESHOLD  1.0e-12/* A null Jacobi rotation for the whitening is smaller than   RELATIVE_W_THRESHOLD/sqrt(T) where T is the number of samples */void OutOfMemory() {  printf("Out of memory, sorry...\n");  exit(EXIT_FAILURE) ;}#define SPACE_PER_LEVEL 3void Message0(int level, char *mess) {  int count ;  if (level < VERBOSITY) {    for (count=0; count<level*SPACE_PER_LEVEL; count++) fprintf(stderr," ");    fprintf(stderr, mess);  }}void MessageF(int level, char *mess, double value) {  int count ;  if (level < VERBOSITY) {    for (count=0; count<level*SPACE_PER_LEVEL; count++) fprintf(stderr," ");    fprintf(stderr, mess, value);  }}void MessageI(int level, char *mess, int value) {  int count ;  if (level < VERBOSITY) {    for (count=0; count<level*SPACE_PER_LEVEL; count++) fprintf(stderr," ");    fprintf(stderr, mess, value);  }}void Identity (double *Mat, int p){  int i;  int p2 = p*p ;  for (i=0;i<p2;i++) Mat[i]     = 0.0 ;  for (i=0;i<p ;i++) Mat[i+i*p] = 1.0 ;}/* How far from identity ? Ad hoc answer */double NonIdentity (double *Mat, int p){  int i,j;  double point ;  double sum  = 0.0 ;  for (i=0;i<p;i++)    for (j=0;j<p;j++) {      point = Mat[i*p+j] ;      if (i!=j) sum += point*point ;       else      sum += (point-1.0)*(point-1.0) ;     }  return sum ;}/* X=Trans*X : computes IN PLACE the transformation X=Trans*X.  X: nxT, Trans: nxn */void Transform (double *X, double *Trans, int n, int T)  {  double *Tx ; /* buffer for a column vector */  int i,s,t ;  int Xind, Xstart, Xstop ;  double sum ;  Tx = (double *) calloc(n, sizeof(double)) ;   if (Tx == NULL) OutOfMemory() ;  for (t=0; t<T; t++)    {      Xstart = t * n ;      Xstop  = Xstart + n ;      /* stores in Tx the t-th colum of X transformed by Trans */      for (i=0; i<n ; i++) {	sum = 0.0 ;	for (s=i, Xind=Xstart; Xind<Xstop; s+=n, Xind++)	  sum += Trans[s]*X[Xind] ;	Tx[i]=sum ;	}      /* plugs the transformed vector back in the orignal matrix */      for (i=0, Xind=Xstart; i< n; i++, Xind++) 	X[Xind]=Tx[i] ;    }  free(Tx) ;}void EstCovMat(double *R, double *A, int m, int T){  int i, j, t ;  double *x ;  double ust = 1.0 / (double) T ;  for (i=0; i<m; i++)    for (j=i; j<m; j++)      R[i+j*m] = 0.0 ;    for (t=0, x=A; t<T; t++, x+=m)    for (i=0; i<m; i++)      for (j=i; j<m; j++)	R[i+j*m] += x[i]* x[j];     for (i=0; i<m; i++)    for (j=i; j<m; j++) {      R[i+j*m] = ust * R[i+j*m] ;      R[j+i*m] = R[i+j*m] ;    }}  /* rem: does not depend on n,of course: remove the argument *//* A(mxn) --> A(mxn) x R where R=[ c s ; -s c ]  rotates the (p,q) columns of R */void RightRotSimple(double *A, int m, int n, int p, int q, double c, double s ){  double nx, ny ;  int ix = p*m ;  int iy = q*m ;  int i ;    for (i=0; i<m; i++) {    nx = A[ix] ;    ny = A[iy] ;    A[ix++] = c*nx - s*ny ;    A[iy++] = s*nx + c*ny ;  }}/* Ak(mxn) --> Ak(mxn) x R where R rotates the (p,q) columns R =[ c s ; -s c ]    and Ak is the k-th M*N matrix in the stack */void RightRotStack(double *A, int M, int N, int K, int p, int q, double c, double s ) {   int k, ix, iy, cpt, kMN ;   int pM = p*M ;  int qM = q*M ;  double nx, ny ;   for (k=0, kMN=0; k<K; k++, kMN+=M*N)    for ( cpt=0, ix=pM+kMN, iy=qM+kMN; cpt<M; cpt++) {       nx = A[ix] ;       ny = A[iy] ;       A[ix++] = c*nx - s*ny ;       A[iy++] = s*nx + c*ny ;     } }/*    A(mxn) --> R * A(mxn) where R=[ c -s ; s c ]   rotates the (p,q) rows of R */void LeftRotSimple(double *A, int m, int n, int p, int q, double c, double s ){  int ix = p ;  int iy = q ;  double nx, ny ;  int j ;    for (j=0; j<n; j++, ix+=m, iy+=m) {    nx = A[ix] ;    ny = A[iy] ;    A[ix] = c*nx - s*ny ;    A[iy] = s*nx + c*ny ;  }}/*    Ak(mxn) --> R * Ak(mxn) where R rotates the (p,q) rows R =[ c -s ; s c ]     and Ak is the k-th matrix in the stack*/void LeftRotStack(double *A, int M, int N, int K, int p, int q, double c, double s ){  int k, ix, iy, cpt ;  int MN = M*N ;  int kMN ;  double nx, ny ;  for (k=0, kMN=0; k<K; k++, kMN+=MN)    for (cpt=0, ix=p+kMN, iy=q+kMN; cpt<N; cpt++, ix+=M, iy+=M) {      nx = A[ix] ;      ny = A[iy] ;      A[ix] = c*nx - s*ny ;      A[iy] = s*nx + c*ny ;    }}/* Givens angle for the pair (p,q) of an mxm matrix A   */double Givens(double *A, int m, int p, int q){  double pp = A[p+m*p] ;  double qq = A[q+m*q] ;  double pq = A[p+m*q] ;  double qp = A[q+m*p] ;  if (pp>qq)    return 0.5 * atan2(-pq-qp, pp-qq) ;  else    return 0.5 * atan2(pq+qp, qq-pp) ;}/* Givens angle for the pair (p,q) of a stack of K M*M matrices */double GivensStack(double *A, int M, int K, int p, int q){  int k ;  double diff_on, sum_off, ton, toff ;  double *cm ; /* A cumulant matrix  */  double G11 = 0.0 ;  double G12 = 0.0 ;  double G22 = 0.0 ;    int M2 = M*M ;  int pp = p+p*M ;  int pq = p+q*M ;  int qp = q+p*M ;  int qq = q+q*M ;  for (k=0, cm=A; k<K; k++, cm+=M2) {    diff_on = cm[pp] - cm[qq] ;    sum_off = cm[pq] + cm[qp] ;        G11 += diff_on * diff_on ;    G22 += sum_off * sum_off ;    G12 += diff_on * sum_off ;  }  ton  = G11 - G22 ;  toff = 2.0 * G12  ;    return -0.5 * atan2 ( toff , ton+sqrt(ton*ton+toff*toff) );   /* there is no final minus sign in the matlab code because the     convention for c/s in the Givens rotations is the opposite ??? */}/*    Diagonalization of an mxm matrix A by a rotation R.*/int Diago (double *A, double *R, int m, double threshold) {  int encore = 1 ;  int rots = 0 ;  int p, q ;  double theta,c,s ;  Identity(R, m) ;  /* Sweeps until no pair gets updated  */  while (encore>0)  {     encore = 0 ;    for (p=0; p<m; p++)      for (q=p+1; q<m; q++) {