/* ============================================================ *
 * decomp_eb.c							*
 * Martin Kilbinger, Liping Fu 2008, 2009			*
 * ============================================================ */
#include "decomp_eb.h"
#include <gsl/gsl_sf_gamma.h>

/* === CFHTLS Wide 3rd data release, Fu&Kilbinger (2010) === */

/* S/N, Psi=19' eta=1/50 */
const double a_FK10_SN[N_FK10]        = {0.1197730890, -0.3881211865, 0.5212557875, -0.3440507036, 0.2761305382, -0.07286690971};

/* FoM, Psi=222', eta=1/10 */
const double a_FK10_FoM_eta10[N_FK10] = {0.009877788826, 0.1061397843, -0.4300211814, 0.5451016406, -0.3372272549, 0.1716983151};

/* FoM, Psi=222', eta=1/50 */
const double a_FK10_FoM_eta50[N_FK10] = {0.1239456383, -0.3881431858, 0.5579593467, -0.3679282338, 0.1540941993, 0.01293361618};

/* First-kind Chebyshev polynomial of order n */
#define EPS 1.0e-10
double Cheby(double x, int n, error **err)
{
   double Cn;

   testErrorRetVA(x<-1-EPS || x>1+EPS, mr_range, "x = %g out of range", *err, __LINE__, -1, x);

   if (x<-1) x = -1.0;
   if (x>1) x = 1.0;

   Cn = cos (n * acos (x));
   return Cn;
}
#undef EPS

/* Second-kind Chebyshev polynomial of order n */
#define EPS 1.0e-10
double Cheby2(double x, int n, error **err)
{
   double Un;

   testErrorRetVA(x<-1-EPS || x>1+EPS, mr_range, "x = %g out of range", *err, __LINE__, -1, x);

   if (x<-1) x = -1.0;
   if (x>1) x = 1.0;

   if (x == 1) {
      Un = n + 1;
   } else if (x == -1) {
      Un = pow(-1.0, n) * (n + 1.0);
   } else {
      Un = sin((n+1.0)*acos(x))/sin(acos (x));
   }

   return Un;
}
#undef EPS

/* Legendre polynomial of order n */
double Legen(double x, int n)
{

   switch (n) {
      case 0:
	 return 1.0;
      case 1:
	 return x;
      default:
	 return (2.0*n-1.0)/(double)n*x*Legen(x,n-1)-(n-1.0)/(double)n*Legen(x,n-2);
   }
}

/* General basis function of order n */
#define EPS 1.0e-10
double C(double x, int n, poly_t poly, error **err)
{
   double c;

   testErrorRetVA(x<-1-EPS || x>1+EPS, mr_range, "x = %g out of range", *err, __LINE__, -1, x);

   if (x<-1) x = -1.0;
   if (x>1) x = 1.0;

   switch (poly) {
      case cheby :
	 c = Cheby(x, n, err);
	 forwardError(*err, __LINE__, -1.0);
	 break;
      case cheby2 :
	 c = Cheby2(x, n, err);
	 forwardError(*err, __LINE__, -1.0);
	 break;
      case legen :
	 c = Legen(x, n);
	 break;
      default :
	 *err = addErrorVA(mr_poly, "Unknown polynomial type %d", *err, __LINE__, poly);
	 return -1.0;
   }

   return c;
}
#undef EPS

/* ============================================================ *
 * The filter function for the generalised ring statistics.	*
 * FK09 (11).							*
 * ============================================================ */
#define	 EPS 1.0e-6
double Tp(double x, const double *a, int N, poly_t poly, error **err)
{
   int n;
   double res, Cn, Cnm1=0.0, Cnm2;

   if (x<-1-EPS || x>+1+EPS) return 0;

   /* NEW! */
   //if (x<-1) x = -1;
   //if (x>+1) x = +1;

   testErrorRetVA(N<=0, mr_range, "N has to be larger than zero but is %d", *err, __LINE__, 0.0, N);

   testErrorRetVA(x>+1+EPS, mr_range, "x=%.10f out of range", *err, __LINE__, 0.0, x);
   testErrorRetVA(x<-1-EPS, mr_range, "x=%.10f out of range", *err, __LINE__, 0.0, x);

   if (poly==cheby || poly==cheby2) {

      Cnm2 = 1.0;
      if (poly==cheby)       Cnm1 = x;
      else if (poly==cheby2) Cnm1 = 2.0*x;

      res  = 0.0;
      res += a[0] * Cnm2;
      if (N==1) return res;
      res += a[1] * Cnm1;
      if (N==2) return res;

      for (n=2; n<N; n++) {
	 /* Chebyshev (both T,U) recurrence relation *
	  * C_n(x) = 2 x C_{n-1}(x) - C_{n-2}(x)     */
	 Cn   = 2.0*x*Cnm1 - Cnm2;
	 res += a[n] * Cn;
	 Cnm2 = Cnm1;
	 Cnm1 = Cn;
      }

   } else {

      for (n=0,res=0.0; n<N; n++) {
	 Cn   = C(x, n, poly, err);
	 forwardError(*err, __LINE__, -1);
	 res += a[n] * Cn;
      }

   }

   return res;
}
#undef EPS

/* ============================================================ *
 * FK09 (24).							*
 * ============================================================ */
double Fn0(double x, int n, poly_t poly, error **err)
{
   double fn0, denom=0.0;
   int nn;

   if (n<0) nn = -n;
   else nn = n;

   if (poly==cheby) {
      if (nn==0) return x + 1.0;
      if (nn==1) return 0.5*(x*x - 1.0);
   } else if (poly==cheby2) {
      if (n==-1) return 0.0;  /* n, not nn: no symmetry for cheby2! */
   } else {
      *err = addErrorVA(mr_poly, "Tm for polynomial type %d not defined", *err, __LINE__, poly);
      return 0.0;
   }

   fn0 = 0.0;

   if (nn%2==0) fn0 += 1.0;   /* n even */
   else fn0 -= 1.0;           /* n odd  */

   //printf("# ** 1 %g\n", fn0);

   fn0 += x*Cheby(x, nn, err);
   forwardError(*err, __LINE__, 0.0);

   //printf("# ** 2 %g\n", fn0);

   if (poly==cheby) fn0 += n*(1 - x*x)*Cheby2(x, n-1, err);
   else if (poly==cheby2) fn0 -= (1 - x*x)*Cheby2(x, n-1, err);
   forwardError(*err, __LINE__, 0.0);

   //printf("# ** 3 %g\n", fn0);

   if (poly==cheby) denom = 1.0 - (double)(n*n);
   else if (poly==cheby2) denom = 1.0+(double)n;         /* n, not nn! */

   fn0 /= denom;

   return fn0;
}

/* ============================================================ *
 * FK09 (26)							*
 * ============================================================ */
void Fnnu(double x, int n, poly_t poly, double Fn[], error **err)
{
   double f0, fp1, fp2, fp3, fm1, fm2, fm3;

   f0  = Fn0(x, n, poly, err);      forwardError(*err, __LINE__,);
   fp1 = Fn0(x, n+1, poly, err);    forwardError(*err, __LINE__,);
   fp2 = Fn0(x, n+2, poly, err);    forwardError(*err, __LINE__,);
   fp3 = Fn0(x, n+3, poly, err);    forwardError(*err, __LINE__,);
   fm1 = Fn0(x, n-1, poly, err);    forwardError(*err, __LINE__,);
   fm2 = Fn0(x, n-2, poly, err);    forwardError(*err, __LINE__,);
   fm3 = Fn0(x, n-3, poly, err);    forwardError(*err, __LINE__,);

   Fn[0] = f0;
   Fn[1] = 0.5*(fp1 + fm1);
   Fn[2] = 0.25*(fp2 + 2.0*f0 + fm2);
   Fn[3] = 0.125*(fp3 + 3.0*fp1 + 3.0*fm1 + fm3);
}

/* ============================================================ *
 * FK09 (23)							*
 * ============================================================ */
#define EPS 1.0e-10
double alpha_explicit(double x, int n, double R, poly_t poly, error **err)
{
   double r, Fn[4], alpha;

   testErrorRet(poly!=cheby && poly!=cheby2, mr_poly,
		"Explicit expression only obtained so far for cheby1 and cheby2",
		*err, __LINE__, 0.0);

   if (x<-1.0+EPS) return 0.0;
   assert(x>-1.0);

   r = 3.0/dsqr(x + R);
   Fnnu(x, n, poly, Fn, err);
   forwardError(*err, __LINE__, 0.0);

   alpha  = R*(1.0-r*R*R)*Fn[0];
   alpha += (1.0 - 3.0*r*R*R)*Fn[1];
   alpha += -3.0*r*R*Fn[2];
   alpha += -r*Fn[3];
   alpha *= 4.0/3.0*r;

   return alpha;
}

/* ============================================================ *
 * The filter function T_- obtained from T_+, FK09 (20).	*
 * ============================================================ */
#define EPS 1.0e-10
double Tm(double x, const double *a, int N, poly_t poly, double R, error **err)
{
   double tm, c, alph;
   int n;

   testErrorRetVA(x<-1-EPS, mr_range, "x=%.20f smaller than -1", *err, __LINE__, 0.0, x);

   for (n=0,tm=0.0; n<N; n++) {
      c    = C(x, n, poly, err);                forwardError(*err, __LINE__, 0.0);
      alph = alpha_explicit(x, n, R, poly, err);
      forwardError(*err, __LINE__, 0.0);

      tm  += a[n]*(c + alph);
   }
   return tm;
}
#undef EPS

/* ================================================================== *
 * Returns the generalised ring statistic using a as the coefficients *
 * of the T_+ decomposition, see FK09 (4, 8, 11).		      *
 * The correlation functions xi+ and xi- are read from the            *
 * Nxi-dimensional arrays xip and xim, on angular scales th.          *
 * N is the size of the coefficient-array a.                          *
 * ================================================================== */
double RR_data(const double *xip, const double *xim, const double *th, const int Nxi,
	       double THETA_MIN, double THETA_MAX, const double *a, int N,
	       poly_t poly,  int pm, error **err)
{
   double theta, dtheta, res, A, B, x, summand;
   double dt1, dt2;
   int i;

   testErrorRetVA(abs(pm)!=1, mr_incompatible, "pm=%d not valid, has to be +1 or -1",
		  *err, __LINE__, 0.0, pm);

   testErrorRetVA(THETA_MIN<th[0], mr_range,
		  "THETA_MIN=%g' is smaller than minimum angular scale for xi+-, %g'",
		  *err, __LINE__, 0.0, THETA_MIN/arcmin, th[0]/arcmin);

   testErrorRetVA(THETA_MAX>th[Nxi-1], mr_range,
		  "THETA_MAX=%g' is larger than maximum angular scale for xi+-, %g'",
		  *err, __LINE__, 0.0, THETA_MAX/arcmin, th[Nxi-1]/arcmin);

   if (a==NULL) {
      *err = addError(mr_null, "Coefficients a=NULL", *err, __LINE__);
      return 0.0;
   }

   /* FK09 (8) */
   A = (THETA_MAX - THETA_MIN)/2.0;
   B = (THETA_MAX + THETA_MIN)/2.0;

   for (i=0,res=0.0; i<Nxi; i++) {
      theta  = th[i];
      if (theta < THETA_MIN) continue;
      else if (theta > THETA_MAX) break;

      /* theta[i] is the bin center */

      if (i==0) {
	 dt1 = (th[i+1] - th[i])/2.;
	 dtheta = 2*dt1;
      } else if (i==Nxi-1) {
	 dt2 = (th[i] - th[i-1])/2.;
	 dtheta = 2*dt2;
      } else {
	 dt1 = (th[i+1] - th[i])/2.;
	 dt2 = (th[i] - th[i-1])/2.;
	 dtheta = dt2 + dt1;
      }
      x = (theta-B)/A;

      if (pm==+1)  {
	 summand  = xip[i]*theta/dsqr(THETA_MAX);
	 summand *= Tp(x, a, N, poly, err);
	 forwardError(*err, __LINE__, -1.0);
      } else if (pm==-1) {
	 summand  = xim[i]*theta/dsqr(THETA_MAX);
	 summand *= Tm(x, a, N, poly, B/A, err);
	 forwardError(*err, __LINE__, -1.0);
      } else {
	 summand = 0.0;
      }

      res += summand*dtheta;
   }

   testErrorRet(!finite(res), math_infnan, "R is not finite", *err, __LINE__,
		  0.0);

   return res;
}

/* If cov_mode=outer, n, m are not used. For cov_mode=inner, a, N are not used.  *
 * If cov_mode=fromZ, the covariance using Z+ is returned and a, N are not used. */
#define EPS 1.0e-5
double cov_RR(const double *THETA_MIN, const double *THETA_MAX, const double *a, int N, poly_t poly,
	      const double *theta, const double *cov_xi, int Ntheta,
	      cov_mode_t cov_mode, int n, int m, double fac, error **err)
{
   double sum, dlogtheta, tmp, A[2], B[2], xi, xj, yi, yj, thetai, thetaj;
   int i, j;
   static double  **Cov_xi = NULL;

   /* Testing for a==NULL not possible because cov_RR is also called from get_cov_RR_inner_array */
   if (cov_mode==fromZ) {
#ifdef __MRING_H
      sum = cov_RR_Z(THETA_MIN, THETA_MAX, theta, cov_xi, Ntheta, err);
      forwardError(*err, __LINE__, 0.0);
      return sum;
#else
      *err = addError(mr_type, "cov_mode 'fromZ' not supported here", *err, __LINE__);
      return 0.0;
#endif
   }

   for (i=0; i<2; i++) {
      A[i] = (THETA_MAX[i] - THETA_MIN[i])/2.0;
      B[i] = (THETA_MAX[i] + THETA_MIN[i])/2.0;
   }

   dlogtheta = log(theta[1]) - log(theta[0]);

   for (i=0; i<2; i++) {
      testErrorRetVA(theta[0]>THETA_MIN[i], mr_range,
		     "Minumum of xi-covariance (%g=%g') larger than THETA_MIN (%g=%g')",
		     *err, __LINE__, -1, theta[0], theta[0]/arcmin, THETA_MIN[i], THETA_MIN[i]/arcmin);
      testErrorRetVA(theta[Ntheta-1]<THETA_MAX[i], mr_range,
		     "Maximum of xi-covariance scale (%g=%g') smaller than THETA_MAX (%g=%g')",
		     *err, __LINE__, -1, theta[Ntheta-1], theta[Ntheta-1]/arcmin, THETA_MAX[i],
		     THETA_MAX[i]/arcmin);
   }

   if (Cov_xi==NULL) {
      Cov_xi = sm2_matrix(0, Ntheta-1, 0, Ntheta-1, err);
      forwardError(*err, __LINE__, -1.0);
      for (i=0; i<Ntheta; i++) {
	 for (j=0; j<Ntheta; j++) {

	    Cov_xi[i][j] = cov_xi[i*Ntheta+j];

	 }

      }

      //if (fac<1.12)
	//fprintf(stderr, "WARNING: Cov-xi-factor = %g, too small for stable inversion of covariance!!!\n", fac);
   }

   sum = 0.0;
   //for (i=0; i<Ntheta; i++) {
   //thetai = theta[i];
   for (thetai=theta[0]; thetai<=theta[Ntheta-1]; thetai*=fac) {
      if (thetai<THETA_MIN[0] || thetai>THETA_MAX[0]) continue;
      xi = (thetai-B[0])/A[0];
      yi = thetai/THETA_MAX[0];

      //for (j=0; j<Ntheta; j++) {
      //thetaj = theta[j];
      for (thetaj=theta[0]; thetaj<=theta[Ntheta-1]; thetaj*=fac) {
	 if (thetaj<THETA_MIN[1] || thetaj>THETA_MAX[1]) continue;
	 xj = (thetaj-B[1])/A[1];
	 yj = thetaj/THETA_MAX[1];

	 tmp  = dsqr(dlogtheta);

	 if (cov_mode==outer) {
	    tmp *= Tp(xi, a, N, poly, err);
	    forwardError(*err, __LINE__, -1);
	    tmp *= Tp(xj, a, N, poly, err);
	    forwardError(*err, __LINE__, -1);
	 } else {
	    tmp *= C(xi, n, poly, err);  	    forwardError(*err, __LINE__, -1);
	    tmp *= C(xj, m, poly, err);  	    forwardError(*err, __LINE__, -1);
	 }

	 tmp *= dsqr(yi*yj);

	 //tmp *= cov_xi[i*Ntheta+j];
	 tmp *= sm2_interpol2d(Cov_xi, Ntheta, log(theta[0]), log(theta[Ntheta-1]), dlogtheta, log(thetai),
	 		       Ntheta, log(theta[0]), log(theta[Ntheta-1]), dlogtheta, log(thetaj), 0.0, 0.0, err);
	 forwardError(*err, __LINE__, -1.0);

	 sum += tmp;

      }

  }

   /* Comment the following two lines for faster code, but be aware that no check is done
    * whether the input covariance changes! */
   sm2_free_matrix(Cov_xi, 0, Ntheta-1, 0, Ntheta-1);
   Cov_xi = NULL;

   return sum;
}
#undef EPS

/* RR-Covariance using a diagonal xi-covariance.				 *
 * If cov_mode=outer, n, m are not used. For cov_mode=inner, a, N are not used.  *
 * If cov_mode=fromZ, the covariance using Z+ is returned and a, N are not used. */
#define EPS 1.0e-5
double cov_RR_diag_xi(const double *THETA_MIN, const double *THETA_MAX, const double *a, int N, poly_t poly,
		      const double *theta, const double *var_xi, int Ntheta,
		      int islog, error **err)
{
   double sum, dlogtheta, dtheta, tmp, A[2], B[2], xi, xj, yi, yj, thetai, thetaj;
   int i;

   for (i=0; i<2; i++) {
      A[i] = (THETA_MAX[i] - THETA_MIN[i])/2.0;
      B[i] = (THETA_MAX[i] + THETA_MIN[i])/2.0;
   }

   if (islog==0) {
      dtheta = theta[1] - theta[0];
      dlogtheta = 0.0;
   } else if (islog==1) {
      dlogtheta = log(theta[1]) - log(theta[0]);
      dtheta = 0.0;
   } else {
      *err = addErrorVA(mr_type, "Invalid flag islog = %d\n", *err, __LINE__, islog);
      return 0.0;
   }

   for (i=0; i<2; i++) {
      testErrorRetVA(theta[0]>THETA_MIN[i], mr_range,
		     "Minumum of xi-covariance (%g=%g') larger than THETA_MIN (%g=%g')",
		     *err, __LINE__, -1, theta[0], theta[0]/arcmin, THETA_MIN[i], THETA_MIN[i]/arcmin);
      testErrorRetVA(theta[Ntheta-1]<THETA_MAX[i], mr_range,
		     "Maximum of xi-covariance scale (%g=%g') smaller than THETA_MAX (%g=%g')",
		     *err, __LINE__, -1, theta[Ntheta-1], theta[Ntheta-1]/arcmin, THETA_MAX[i],
		     THETA_MAX[i]/arcmin);
   }

   sum = 0.0;
   for (i=0; i<Ntheta; i++) {
      thetai = theta[i];
      thetaj = thetai;

      if (thetai<THETA_MIN[0] || thetai>THETA_MAX[0]) continue;
      xi = (thetai-B[0])/A[0];
      yi = thetai/THETA_MAX[0];

      xj = (thetaj-B[1])/A[1];
      yj = thetaj/THETA_MAX[1];

      if (islog==0) {
	 //	 tmp = dsqr(dtheta) /thetai/thetai;
	 tmp = dsqr(dtheta)*yi*yj/THETA_MAX[0]/THETA_MAX[1];

      } else {

	 tmp  = dsqr(dlogtheta);
	 tmp *= dsqr(yi*yj);
      }

      tmp *= Tp(xi, a, N, poly, err);
      forwardError(*err, __LINE__, -1);
      tmp *= Tp(xj, a, N, poly, err);
      forwardError(*err, __LINE__, -1);

      tmp *= var_xi[i];
      //      tmp *= dsqr(yi*yj);

      sum += tmp;
  }

   return sum;
}
#undef EPS

/* ============================================================ *
 * Returns the chi^2 for a null test (RR=0).			*
 * ============================================================ */
double chi2_RB_null(const double *RB, const double *covRB, int NRB)
{
   int i, j;
   double c2;

   for (i=0,c2=0.0; i<NRB; i++) {
      for (j=0; j<NRB; j++) {
	 c2 += RB[i] * covRB[i*NRB+j] * RB[j];
      }
   }

   return c2;
}

/* ============================================================ *
 * Reads COSEBIs zeros and normalisation coefficients from file *
 * and returns polynomial coefficients.	File name is automatic, *
 * scale checks are done.					*
 * ============================================================ */
double *read_zeros_norm_cosebi_auto_check(double Psimin, double Psimax, const char *path, error **err)
{
   double *c_cosebi, psimin, psimax;
   char rname[1024];

   sprintf(rname, "%s/cosebi_tplog_rN_%d_%.3f_%.3f",
	   path == NULL ? "." : path, NMAX_COSEBI, Psimin/arcmin, Psimax/arcmin);
   c_cosebi = read_zeros_norm_cosebi(rname, &psimin, &psimax, err);
   forwardError(*err, __LINE__, NULL);

   testErrorRetVA(fabs(psimin-Psimin/arcmin)>EPSILON, mr_file,
		  "Inconsistent file %s, psimin=%g should be %g according to the file name",
		  *err, __LINE__, NULL, rname, psimin, Psimin/arcmin);
   testErrorRetVA(fabs(psimax-Psimax/arcmin)>EPSILON, mr_file,
		  "Inconsistent file %s, psimax=%g should be %g according to the file name",
		  *err, __LINE__, NULL, rname, psimax, Psimax/arcmin);

   return c_cosebi;
}

/* ============================================================ *
 * Reads COSEBIs zeros and normalisation coefficients from file *
 * and sets corresponding min and max scales.			*
 * Calculates polynomial coefficients c and returns them.       *
 * ============================================================ */
double *read_zeros_norm_cosebi(const char *rname, double *psimin, double *psimax, error **err)
{
   FILE *F;
   int Nzeros, Ncoeff, k, n, off_c, off_R, nmax;
   ssize_t nread;
   double *Rn, *Norm, *c;
   char *str, *line=NULL;

   F = fopen_err(rname, "r", err);                  forwardError(*err, __LINE__, NULL);

   /* Read two header lines */
   line = malloc_err(1024*sizeof(char), err);       forwardError(*err, __LINE__, NULL);
   str = fgets(line, 1024, F);
   str = fgets(line, 1024, F);
   free(line);

   nread = fscanf(F, "%d  %lg %lg\n", &nmax, psimin, psimax);
   testErrorRet(nread != 3, mr_file, "File has wrong format.", *err, __LINE__, NULL);

   testErrorRetVA(nmax > NMAX_COSEBI, mr_range,
		  "COSEBI number of modes n=%d read from file %s cannot be larger than NMAX_COSEBI=%d",
		  *err, __LINE__, NULL, nmax, rname, NMAX_COSEBI);

   /* Number of zeros for nmax polynomials,               *
    * sum_{i=1}^{nmax+1} (i+1) = sum_{i=0}^{nmax+1} i - 1 */
   Nzeros = (nmax+1) * (nmax+2) / 2 - 1;

   //fprintf(stderr, "nmax = %d , Psi = [%g, %g]\n", nmax, *psimin, *psimax);
   //fprintf(stderr, "Reading %d Rn, %d Norm, from %s\n", Nzeros, nmax, rname);

   Rn   = malloc_err(sizeof(double)*Nzeros, err);   forwardError(*err, __LINE__, NULL);
   Norm = malloc_err(sizeof(double)*nmax, err);     forwardError(*err, __LINE__, NULL);

   /* Read zeros */
   for (k=0; k<Nzeros; k++) {
      fscanf(F, "%lg ", Rn+k);
   }
   /* Read normalisations */
   for (k=0; k<nmax; k++) {
      fscanf(F, "%lg ", Norm+k);
   }

   fclose(F);

   /* Calculate polynomial coefficients */
   Ncoeff = NMAX_COSEBI * (NMAX_COSEBI + 5) / 2;
   c =  malloc_err(sizeof(double)*Ncoeff, err);        forwardError(*err, __LINE__, NULL);

   /* Polynomial T_i of order i+1 */
   for (n=1,off_R=0; n<=nmax; n++) {

      off_c = n*(n + 3)/2 - 2;   // Offset for coefficients for polynomial #n

      for (k=0; k<n+2; k++) {
	 c[k + off_c] = sum_combinations(n+1-k, n+1, Rn+off_R, err);
	 forwardError(*err, __LINE__, NULL);
	 c[k + off_c] *= Norm[n-1];
	 if (k % 2 == 1) c[k + off_c] = -c[k + off_c];
	 if (n % 2 == 0) c[k + off_c] = -c[k + off_c];
      }

      off_R = (n+1)*(n+2)/2 - 1; // Ncoeff for index i, degree i+1
   }

   free(Rn);
   free(Norm);

   return c;
}

/* ============================================================ *
 * COSEBI logarithmis filter function, SEK10 (28). The          *
 * coefficients have to be precalculated from the zeros, output *
 * by Mathematica.						*
 * Not normalised. 						*
 * ============================================================ */
double Tplog_c(double z, const double *c, int n, error **err)
{
   int i, off_c;
   double res, zpowerm;
   const double *cn;

   off_c = n*(n + 3)/2 - 2; // Offset for coefficients for polynomial #n
   cn    = c + off_c;

   for (i=0,res=0.0,zpowerm=1.0; i<=n+1; i++) {
      res     += cn[i] * zpowerm;
      zpowerm *= z;
   }

   testErrorRet(!finite(res), math_infnan, "Tplog is not finite", *err, __LINE__, 0.0);

   return res;
}

/* ============================================================ *
 * Filter function, SEK10 (38, 39). Normalised.                 *
 * ============================================================ */
double Tmlog(double z, const double *c, int n, error **err)
{
   int m, off_c;
   double res, zpowerm;
   const double *cn;

   off_c = n*(n + 3)/2 - 2; // Offset for coefficients for polynomial #n
   cn    = c + off_c;

   for (m=0,res=0.0,zpowerm=1; m<=n; m++) { // n+1 MKDEBUG ??
      res += dnm(n, m ,cn) * zpowerm;
      zpowerm *= z;
   }

   res += an2(n, cn) * exp(-2.0*z) - an4(n, cn) * exp(-4.0*z);
   testErrorRet(!finite(res), math_infnan, "Tmlog is not finite", *err, __LINE__, 0.0);

   return res;
}

double an2(int n, const double *c)
{
   int j;
   double res, jfac, m2powerjp1;

   for (j=0,res=0.0,jfac=1.0,m2powerjp1=-2.0; j<=n+1; j++) {
      res        += c[j]*jfac / m2powerjp1;
      jfac       *= j + 1.0;      /* factorial(j) */
      m2powerjp1 *= -2.0;        /* (-2)^(j+1)   */
   }

   res *= 4.0;

   return res;
}

double an4(int n, const double *c)
{
   int j;
   double res, jfac, m4powerjp1;

   for (j=0,res=0.0,jfac=1.0,m4powerjp1=-4.0; j<=n+1; j++) {
      res        += c[j]*jfac / m4powerjp1;
      jfac       *= j + 1.0;
      m4powerjp1 *= -4.0;        /* (-4)^(j+1) */
   }

   res *= 12.0;

   return res;
}

/* dnm(n+1, n, c) = 0 */
double dnm(int n, const int m, const double *c)
{
   int j;
   double res, jfac, m2powermjm1, p2powermjm1, mfac;

   p2powermjm1 = 0.5;
   m2powermjm1 = -0.5;
   mfac = gsl_sf_fact(m);
   for (j=m,res=0.0,jfac=mfac; j<=n+1; j++) {
      res         += c[j] * jfac * m2powermjm1 * (3.0 * p2powermjm1 - 1.0);
      jfac        *= j + 1.0;        /* j! */
      m2powermjm1 /= -2.0;    /* (-2)^(m-j-1) */
      p2powermjm1 /= 2.0;     /* 2^(m-k-1)    */
   }

   res *= 4.0/mfac;
   res += c[m];

   return res;
}

double sum_combinations(int j, int n, const double *r, error **err)
{
   gsl_combination *c;
   size_t i, c_i;
   double sum, prod;

   if (n==0) return 1.0;

   c = gsl_combination_calloc(n, j);

   sum = 0.0;
   do {
      for (i=0,prod=1.0; i<j; i++) {
	 c_i =  gsl_combination_get(c, i);
	 prod *= r[c_i];
	 //printf(" %zu", c_i);
      }
      sum += prod;
      //printf("{"); gsl_combination_fprintf(stdout, c, " %u"); printf (" }\n");
   } while (gsl_combination_next(c) == GSL_SUCCESS);

   gsl_combination_free(c);

   return sum;
}

/* ============================================================ *
 * E-/B-mode correlation functions from COSEBIs, SEK10 (40).    *
 * The results are stored in xi_pm_EB = (E+, E-, B+, B-).       *
 * ============================================================ */
void xipmEB(double theta, double THETA_MIN, double THETA_MAX, const double *c,
	    const double *E, const double *B, int N, double xi_pm_EB[4], error **err)
{
   double Tp, Tm, z;
   int n;

   for (n=0; n<4; n++) {
      xi_pm_EB[n] = 0.0;
   }

   for (n=1; n<=N; n++) {
      z  = log(theta/THETA_MIN);
      Tp = Tplog_c(z, c, n, err);
      Tm = Tmlog(z, c, n, err);

      xi_pm_EB[0] += E[n] * Tp;   // E+
      xi_pm_EB[1] += E[n] * Tm;   // E-
      xi_pm_EB[2] += B[n] * Tp;   // B+
      xi_pm_EB[3] += B[n] * Tm;   // B-
   }

   for (n=0; n<4; n++) {
      xi_pm_EB[n] *= 2.0 / theta / (THETA_MAX - THETA_MIN);
   }
}


#include <math.h>
#include <float.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdbool.h>

#include <cblas.h>

#include "funcsfa.h"

#define min(x, y) ((x)>(y)?(y):(x))
#define max(x, y) ((x)>(y)?(x):(y))

#define DEBUG 0
#define debug_print(fmt, ...) \
        do { if (DEBUG) fprintf(stderr, "%s:%4d:%25s(): " fmt, __FILE__, \
                                __LINE__, __func__, __VA_ARGS__); } while (0)

double eps_ev = 1e-20;

// **************************************
// Lapack
// **************************************

// Lapack Fortran DGESV interface
void
dgesv_(int *n, int *nrhs, double *a, int *lda, int *ipiv, double *b, int *ldb,
       int *info);
// DGESV wrapper, for solving linear equations.
static int
dgesv(int n, int nrhs, double *a, int lda, int *ipiv, double *b, int ldb)
{
  int info = -1;
  dgesv_(&n, &nrhs, a, &lda, ipiv, b, &ldb, &info);
  return info;
}

// Lapack Fortran DGESVD interface
void
dgesvd_(char *jobu, char *jobvt, int *m, int *n, double *a, int *lda,
        double *s, double *u, int *ldu, double *vt, int *ldvt, double *work,
        int *lwork, int *info);
// DGESVD wrapper, for Singular Value Decomposition.
static int
dgesvd(char jobu, char jobvt, int m, int n, double *a, int lda, double *s,
       double *u, int ldu, double *vt, int ldvt)
{
  int info = -1;
  int lwork = -1;
  double optimal_lwork;
  double *work;
  // Query optimal work size
  dgesvd_(&jobu, &jobvt, &m, &n, a, &lda,
      s, u, &ldu, vt, &ldvt, &optimal_lwork,
      &lwork, &info);
  if (info != 0) {
    return info;
  }
  lwork = (int) optimal_lwork;

  work = malloc(sizeof(double)*lwork);

  dgesvd_(&jobu, &jobvt, &m, &n, a, &lda,
      s, u, &ldu, vt, &ldvt, work,
      &lwork, &info);

  free(work);
  return info;
}

// **************************************
// Objects
// **************************************

funcsfa_Factorization*
funcsfa_Factorization_alloc(size_t n_features, size_t n_factors,
                            size_t n_samples, size_t n_datatypes,
                            size_t *n_features_split, double *data)
{
  size_t i;
  funcsfa_Factorization *m;
  size_t sum_n_features_split = 0;

  debug_print("%s\n", "Allocating SFA object");

  m = malloc(sizeof(funcsfa_Factorization));
  m->n_features = n_features;
  m->n_factors = n_factors;
  m->n_samples = n_samples;
  m->n_datatypes = n_datatypes;
  m->n_features_split = malloc(sizeof(size_t)*n_datatypes);
  for(i=0; i<n_datatypes; i++)
  {
    m->n_features_split[i] = n_features_split[i];
    sum_n_features_split += n_features_split[i];
  }
  if (sum_n_features_split != n_features)
  {
    debug_print("Total number of features (%zd) does not match sum (%zd)\n",
        n_features, sum_n_features_split);
    free(m->n_features_split);
    free(m);
    return NULL;
  }
  m->data = data;
  m->coefficients = malloc(sizeof(double)*n_features*n_factors);
  m->factors = malloc(sizeof(double)*n_factors*n_samples);
  m->factor_cov = malloc(sizeof(double)*n_factors*n_factors);
  m->residual_var = malloc(sizeof(double)*n_features);

  return(m);
}

void
funcsfa_Factorization_free(funcsfa_Factorization *m)
{
  debug_print("%s\n", "Freeing SFA object");

  if (m != NULL)
  {
    free(m->n_features_split);
    free(m->coefficients);
    free(m->factors);
    free(m->factor_cov);
    free(m->residual_var);
  }
  free(m);
}

funcsfa_Monitor*
funcsfa_Monitor_alloc(int max_iter)
{
  funcsfa_Monitor *mon;
  int i;

  debug_print("%s\n", "Allocating Monitor object");

  mon = malloc(sizeof(funcsfa_Monitor));
  if (mon == NULL) {
    return NULL;
  }
  mon->n_iter = 0;
  mon->reconstruction_error = malloc(sizeof(double)*(max_iter+1));
  mon->max_diff_factors = malloc(sizeof(double)*(max_iter+1));
  mon->max_diff_coefficients = malloc(sizeof(double)*(max_iter+1));
  if ((mon->reconstruction_error == NULL) ||
      (mon->max_diff_factors == NULL) ||
      (mon->max_diff_coefficients == NULL))
  {
    funcsfa_Monitor_free(mon);
    return NULL;
  }
  for (i = 0; i < max_iter+1; i++)
  {
    mon->reconstruction_error[i] = NAN;
    mon->max_diff_factors[i] = NAN;
    mon->max_diff_coefficients[i] = NAN;
  }

  return mon;
}

void
funcsfa_Monitor_free(funcsfa_Monitor *mon)
{
  debug_print("%s\n", "Freeing Monitor object");

  if (mon != NULL)
  {
    free(mon->reconstruction_error);
    free(mon->max_diff_factors);
    free(mon->max_diff_coefficients);
  }
  free(mon);
}

/** Bunch of memory for temporary matrices.
 * @private
 */
typedef struct
{
  /** Integer vector n_factors long */
  int *factors_int;
  /** Double vector n_features long */
  double *features_double;
  /** Double matrix n_features x n_factors 1 */
  double *features_factors_double_a;
  /** Double matrix n_features x n_factors 2 */
  double *features_factors_double_b;
  /** Double matrix n_factors x n_factors 1 */
  double *factors_factors_double_a;
  /** Double matrix n_factors x n_factors 2 */
  double *factors_factors_double_b;

  /** Precomputed value of diag(data * t(data)) */
  double *diag_data_square;
  /** Precomputed value of sum(diag(data * t(data))) per data type */
  double *dt_var;
  /** Factors of previous iteration */
  double *prev_factors;
  /** Coefficients of previous iteration */
  double *prev_coefficients;
} Workspace;

static Workspace*
ws_alloc(funcsfa_Factorization *m)
{
  Workspace *ws;

  debug_print("%s\n", "Allocating Workspace");

  ws = malloc(sizeof(Workspace));
  ws->factors_int = malloc(sizeof(int)*m->n_factors);
  ws->features_double = malloc(sizeof(double)*m->n_features);

  ws->features_factors_double_a = malloc(sizeof(double)*m->n_features*m->n_factors);
  ws->features_factors_double_b = malloc(sizeof(double)*m->n_features*m->n_factors);
  ws->factors_factors_double_a = malloc(sizeof(double)*m->n_factors*m->n_factors);
  ws->factors_factors_double_b = malloc(sizeof(double)*m->n_factors*m->n_factors);

  ws->diag_data_square = malloc(sizeof(double)*m->n_features);
  ws->dt_var = malloc(sizeof(double)*m->n_factors);
  ws->prev_factors = malloc(sizeof(double)*m->n_factors*m->n_samples);
  ws->prev_coefficients = malloc(sizeof(double)*m->n_features*m->n_factors);

  return ws;
}

static void
ws_init(Workspace *ws, funcsfa_Factorization *m)
{
  double *d_d;
  size_t dt_i, feature_start, n_features, i;

  debug_print("%s\n", "Initializing Workspace");

  d_d = malloc(sizeof(double)*m->n_features*m->n_features);

  cblas_dgemm(CblasColMajor, CblasTrans, CblasNoTrans,
      m->n_features, m->n_features, m->n_samples, 1,
      m->data, m->n_samples,
      m->data, m->n_samples,
      0, d_d, m->n_features);

  for (i = 0; i < m->n_features; i++)
  {
    ws->diag_data_square[i] = d_d[i*m->n_features + i];
  }
  feature_start = 0;
  for(dt_i = 0; dt_i < m->n_datatypes; dt_i++)
  {
    ws->dt_var[dt_i] = 0;
    n_features = m->n_features_split[dt_i];
    for(i = feature_start; i < (feature_start + n_features); i++)
    {
      ws->dt_var[dt_i] += ws->diag_data_square[i];
    }
    ws->dt_var[dt_i] /= n_features;
    feature_start = feature_start + n_features;
  }

  free(d_d);
}

static void
ws_free(Workspace *ws)
{
  debug_print("%s\n", "Freeing Workspace");

  free(ws->factors_int);
  free(ws->features_double);
  free(ws->features_factors_double_a);
  free(ws->features_factors_double_b);
  free(ws->factors_factors_double_a);
  free(ws->factors_factors_double_b);

  free(ws->diag_data_square);
  free(ws->dt_var);
  free(ws->prev_factors);
  free(ws->prev_coefficients);

  free(ws);
}

// **************************************
// Utility Functions
// **************************************

static double
max_diff(double *a, double *b, size_t length)
{
  size_t i;
  double md, d;

  md = 0;
  for (i = 0; i < length; i++)
  {
    d = fabs(a[i] - b[i]);
    if (d > md)
    {
      md = d;
    }
  }
  return md;
}

static bool
finite_array(double *a, size_t length)
{
  for (size_t i=0; i<length; i++)
  {
    if (!isfinite(a[i]))
    {
      return false;
    }
  }
  return true;
}

static bool
positive_array(double *a, size_t length)
{
  for (size_t i=0; i<length; i++)
  {
    if (!(a[i] > 0.0))
    {
      return false;
    }
  }
  return true;
}

#define check_finite(a, length) \
    if (DEBUG && !finite_array(a, length)) {\
      debug_print("%s\n", "Error, non-finite values in matrix");}

#define check_positive(a, length) \
    if (DEBUG && !positive_array(a, length)) {\
      debug_print("%s\n", "Error, non-positive values in matrix");}

// **************************************
// Algorithm
// **************************************

static int
expectation_factors(funcsfa_Factorization *m, Workspace *ws)
{
  size_t i, j;
  double *r_c, *c_r_c, *c_r_c_i, *o, *c_o;
  double factor_mean, factor_var, factor_sd, deviance;
  int *ipiv;
  int info;

  debug_print("%s\n", "Computing Expectation and Covariance of Factors");
  check_finite(m->coefficients, m->n_features*m->n_factors);
  check_finite(m->residual_var, m->n_features);
  check_positive(m->residual_var, m->n_features);

  /* Compute $\Psi^{-1} B$
   * r_c = diag(1/residual_var) * coefficients */
  r_c = ws->features_factors_double_a;
  for (i=0; i < m->n_features; i++)
  {
    for (j=0; j < m->n_factors; j++)
    {
      r_c[j * m->n_features + i] =
          m->coefficients[j * m->n_features + i] / m->residual_var[i];
    }
  }

  /* Compute $B^T \Psi^{-1} B + I$
   * c_r_c = t(coefficients) * r_c + I */
  c_r_c = ws->factors_factors_double_a;
  cblas_dgemm(CblasColMajor, CblasTrans, CblasNoTrans,
      m->n_factors, m->n_factors, m->n_features, 1,
      m->coefficients, m->n_features,
      r_c, m->n_features,
      0, c_r_c, m->n_factors);
  for (i = 0; i < m->n_factors; i++)
  {
    c_r_c[i*m->n_factors + i] += 1;
  }

  /* Compute $O = \Psi^{-1}B(B^T \Psi^{-1} B + I)^{-1}$
   * o = r_c*solve(c_r_c) */
  c_r_c_i = ws->factors_factors_double_b;
  ipiv = ws->factors_int;
  for (i = 0; i < m->n_factors; i++)
  {
    for (j = 0; j < m->n_factors; j++)
    {
      c_r_c_i[j * m->n_factors + i] = (i == j);
    }
  }

  info = dgesv(m->n_factors, m->n_factors,
      c_r_c, m->n_factors, ipiv,
      c_r_c_i, m->n_factors);
  if (info != 0) {
    debug_print("Lapack error: %d\n", info);
    return -1;
  }
  c_r_c = NULL;
  o = ws->features_factors_double_b;
  cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans,
     m->n_features, m->n_factors, m->n_factors, 1,
     r_c, m->n_features,
     c_r_c_i, m->n_factors,
     0, o, m->n_features);

  /* Compute $E[Z|X] = (\Psi^{-1}B(B^T\Psi^{-1}B+I)^{-1})^TX$
   * factors = t(o) * t(X)
   */
  cblas_dgemm(CblasColMajor, CblasTrans, CblasTrans,
      m->n_factors, m->n_samples, m->n_features, 1,
      o, m->n_features,
      m->data, m->n_samples,
      0, m->factors, m->n_factors);

  /* Compute $B^T\Psi^{-1}B(B^T\Psi^{-1}B+I)^{-1}$
   * c_o = t(coefficients) * o
   */
  c_o = ws->factors_factors_double_a;
  cblas_dgemm(CblasColMajor, CblasTrans, CblasNoTrans,
      m->n_factors, m->n_factors, m->n_features, 1,
      m->coefficients, m->n_features,
      o, m->n_features,
      0, c_o, m->n_factors);

  /*
   * Compute $E[ZZ^T|X] = I - (B^T\Psi^{-1}B(B^T\Psi^{-1}B+I)^{-1})^T + E[Z|X]E[Z|X]^T$
   * factor_cov = factors * t(factors) + I - c_o
   */
  cblas_dgemm(CblasColMajor, CblasNoTrans, CblasTrans,
      m->n_factors, m->n_factors, m->n_samples, 1,
      m->factors, m->n_factors,
      m->factors, m->n_factors,
      0, m->factor_cov, m->n_factors);

  for (i = 0; i < m->n_factors; i++)
  {
    for (j = 0; j < m->n_factors; j++)
    {
      m->factor_cov[j * m->n_factors + i] += (i==j) - c_o[j * m->n_factors + i];
    }
  }

  debug_print("Factor value: %g\n", m->factors[0]);

  // Rescale factors
  for (i = 0; i < m->n_factors; i++)
  {
    factor_mean = 0;
    for (j = 0; j < m->n_samples; j++)
    {
      factor_mean += m->factors[i + j*m->n_factors];
    }
    factor_mean /= m->n_samples;
    debug_print("  Factor mean %zd: %g\n", i, factor_mean);
    factor_var = 0;
    for (j = 0; j < m->n_samples; j++)
    {
      deviance = m->factors[i + j*m->n_factors] - factor_mean;
      factor_var += deviance*deviance;
    }
    factor_var /= m->n_samples;
    debug_print("  Factor variation %zd: %g\n", i, factor_var);
    for (j = 0; j < m->n_factors; j++)
    {
      m->factor_cov[i + j*m->n_factors] /= factor_var;
    }
    factor_sd = sqrt(factor_var);
    for (j = 0; j < m->n_samples; j++)
    {
      m->factors[i + j * m->n_factors] /= factor_sd;
    }
  }

  return 0;
}

static int
maximize(funcsfa_Factorization *m, double *lambdas, Workspace *ws)
{
  const size_t n_lambda = 2;
  size_t dt_i, f_i, lambda_i, k;
  double *ZXt, *BZZt;
  double unpenalized_coeff, l1, l2, dt_var;
  size_t n_features, feature_start;

  debug_print("%s\n", "Estimating Coefficients");

  check_finite(m->factor_cov, m->n_factors*m->n_factors);
  check_finite(m->factors, m->n_factors*m->n_samples);

  ZXt = ws->features_factors_double_a;
  BZZt = ws->features_double;
  // Compute E[Z|X]X^T
  cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans,
      m->n_factors, m->n_features, m->n_samples, 1,
      m->factors, m->n_factors,
      m->data, m->n_samples,
      0, ZXt, m->n_factors);

  // Coordinate Descent over factors
  for(k = 0; k < m->n_factors; k++)
  {
    debug_print("\tCoordinate descent for factor: %zd\n", k);
    // Compute $BE[ZZ^T|X]_(\cdot k)$
    cblas_dgemv(CblasColMajor, CblasNoTrans,
      m->n_features, m->n_factors, 1.0,
      m->coefficients, m->n_features,
      m->factor_cov+(k*m->n_factors), 1,
      0.0, BZZt, 1);

    lambda_i = 0;
    feature_start = 0;
    for(dt_i = 0; dt_i < m->n_datatypes; dt_i++)
    {
      n_features = m->n_features_split[dt_i];
      l1 = lambdas[lambda_i];
      l2 = lambdas[lambda_i+1];
      for(f_i = feature_start; f_i < (feature_start + n_features); f_i++)
      {
        // Compute: $$
        //  \hat B_{(ij)} \leftarrow \frac
        //    {\operatorname{S}(
        //      \hat B_{(ij)} + E[Z|X]_{(j \cdot)} X_{(i \cdot)}^T -
        //      \hat B_{(i\cdot)} \operatorname{E}[ZZ^T|X]_{(\cdot j)},
        //      l_1)}
        //    {1+l_2}
        // $$
        unpenalized_coeff = m->coefficients[f_i + (k * m->n_features)] +
                            ((ZXt[k + (f_i * m->n_factors)] - BZZt[f_i])
                             / m->n_samples);
        m->coefficients[f_i + (k * m->n_features)] =
          copysign(fmax(0.0, fabs(unpenalized_coeff) - l1),
                   unpenalized_coeff) /
          (1 + l2);
      }
      feature_start = feature_start + n_features;
      lambda_i = lambda_i + n_lambda;
    }
  }

  debug_print("%s\n", "Estimating Residual Variance");

  // Residual variance per data type
  feature_start = 0;
  for(dt_i = 0; dt_i < m->n_datatypes; dt_i++)
  {
    dt_var = 0;
    n_features = m->n_features_split[dt_i];
    for(f_i = feature_start; f_i < (feature_start + n_features); f_i++)
    {
      for(k = 0; k < m->n_factors; k++)
      {
        dt_var += (ZXt[k + (f_i * m->n_factors)] *
                   m->coefficients[f_i + (k * m->n_features)]);
      }
    }
    dt_var = (ws->dt_var[dt_i] - (dt_var / n_features)) / m->n_samples;
    for(f_i = feature_start; f_i < (feature_start + n_features); f_i++)
    {
      if (dt_var > eps_ev) {
        m->residual_var[f_i] = dt_var;
      } else {
        m->residual_var[f_i] = eps_ev;
      }
    }
    debug_print("\tResidual Variance %zd: %g\n", dt_i, dt_var);
    feature_start = feature_start + n_features;
  }
  return 0;
}

static int
init_svd(funcsfa_Factorization *m, Workspace *ws)
{
  int info;
  size_t i, j;
  double *x, *s, *vt, *u;
  double scale, f, explained_var, data_var;
  size_t total_n_factors;

  debug_print("%s\n", "Initializing with SVD");

  total_n_factors = min(m->n_features, m->n_samples);

  x = malloc(sizeof(double) * m->n_features * m->n_samples);
  s = malloc(sizeof(double) * total_n_factors);
  vt = malloc(sizeof(double) * total_n_factors * m->n_features);
  u = malloc(sizeof(double) * total_n_factors * m->n_samples);
  if (x == NULL || s == NULL || vt == NULL)
  {
    free(x);
    free(vt);
    free(s);
    free(u);
    return -1;
  }

  for (i=0; i < m->n_features * m->n_samples; i++)
  {
    x[i] = m->data[i];
  }

  /* vt = svd(X).V.t */
  debug_print("%s\n", "\tStarting Lapack SVD");
  info = dgesvd('S', 'S',
      m->n_samples, m->n_features,
      x, m->n_samples,
      s, u, m->n_samples,
      vt, total_n_factors);
  if (info != 0) {
    debug_print("Lapack error: %d\n", info);
    free(x);
    free(vt);
    free(u);
    free(s);
    return -1;
  }
  debug_print("%s\n", "\tDone Lapack SVD");

  scale = 0;
  for (i=0; i < m->n_factors; i++)
  {
    for (j=0; j < m->n_samples; j++)
    {
      f = u[j + i*m->n_samples] * s[i];
      m->factors[i + j*m->n_factors] = f;
      scale += f*f;
    }
  }
  scale /= m->n_factors * m->n_samples;
  scale = sqrt(scale);
  for (i=0; i < m->n_factors; i++)
  {
    for (j=0; j < m->n_features; j++)
    {
      m->coefficients[j + i*m->n_features] = vt[i + j*total_n_factors] * scale;
    }
  }
  for (i=0; i < m->n_factors; i++)
  {
    for (j=0; j < m->n_samples; j++)
    {
      m->factors[i + j*m->n_factors] /= scale;
    }
  }

  explained_var = 0;
  for (i=0; i < m->n_factors; i++)
  {
    explained_var += s[i]*s[i];
  }
  explained_var /= m->n_samples * m->n_features;
  data_var = 0;
  for (i=0; i < m->n_features; i++)
  {
    data_var += ws->diag_data_square[i];
  }
  data_var /= m->n_samples * m->n_features;
  debug_print("\tData Var: %g\n", data_var);
  explained_var = data_var - explained_var;
  explained_var = fmax(0, explained_var) + eps_ev;
  for (i = 0; i < m->n_features; i++) {
    m->residual_var[i] = explained_var;
  }
  debug_print("\tResidual Variance: %g\n", explained_var);

  free(x);
  free(vt);
  free(u);
  free(s);
  return 0;
}

double
calc_reconstruction_error(funcsfa_Factorization *m, Workspace *ws)
{
  int i, j;
  double error, e;
  double *prediction;

  error = 0.0;
  for (i = 0; i < m->n_samples; i++)
  {
    prediction = ws->features_double;
    cblas_dgemv(CblasColMajor, CblasNoTrans,
        m->n_features, m->n_factors,
        1.0, m->coefficients, m->n_features,
        m->factors + (i*m->n_factors), 1,
        0.0, prediction, 1);
    for (j = 0; j < m->n_features; j++)
    {
      e = m->data[i + (j * m->n_samples)] - prediction[j];
      error += e*e;
    }
  }
  return error;
}

int
funcsfa(funcsfa_Factorization *m, double eps, int max_iter,
        char regularizations[], double *lambdas, funcsfa_Monitor *mon)
{
  int iter = 0;
  int err = 0;
  size_t i;
  double diff = DBL_MAX;
  double diff_f, diff_c;
  double rec_error;
  int const LEN_PREV_REC_ERROR = 10;
  double prev_rec_error[LEN_PREV_REC_ERROR];
  int prev_rec_error_i=0;

  Workspace *ws;

  for (i=0; i<m->n_datatypes; i++)
  {
    if (regularizations[i] != 'e')
    {
      debug_print("%s\n", "Only elastic net regularization is supported");
      return -4;
    }
  }

  ws = ws_alloc(m);
  ws_init(ws, m);

  debug_print("%s\n", "Starting Factorization");

  err = init_svd(m, ws);
  if (err != 0)
  {
    return(1);
  }

  rec_error = calc_reconstruction_error(m, ws);
  if (mon != NULL)
  {
    mon->reconstruction_error[0] = rec_error;
    mon->max_diff_factors[0] = 0;
    mon->max_diff_coefficients[0] = 0;
  }
  for (i=0; i<LEN_PREV_REC_ERROR; i++) {
    prev_rec_error[i] = 0.0;
  }

  /* EM Loop */
  while (iter < max_iter && diff > eps) {
    debug_print("Iteration %d\n", iter);
    memcpy(ws->prev_factors, m->factors,
        sizeof(double)*m->n_factors*m->n_samples);
    memcpy(ws->prev_coefficients, m->coefficients,
        sizeof(double)*m->n_features*m->n_factors);
    err = expectation_factors(m, ws);
    if (err != 0)
    {
      debug_print("Error in computing expectation of factors: %d\n", err);
      ws_free(ws);
      return(2);
    }
    err = maximize(m, lambdas, ws);
    if (err != 0)
    {
      debug_print("Error in maximization: %d\n", err);
      ws_free(ws);
      return(3);
    }

    diff_f = max_diff(m->factors,
        ws->prev_factors, m->n_factors*m->n_samples);
    diff_c = max_diff(m->coefficients, ws->prev_coefficients,
        m->n_features*m->n_factors);
    debug_print("Diff (factor/coefficients): %g; %g\n", diff_f, diff_c);
    prev_rec_error[prev_rec_error_i] = rec_error;
    prev_rec_error_i += 1;
    if (prev_rec_error_i > LEN_PREV_REC_ERROR) {
      prev_rec_error_i = 0;
    }
    rec_error = calc_reconstruction_error(m, ws);
    debug_print("Reconstruction Error: %g\n", rec_error);
    diff = 0.0;
    for (i=0; i<LEN_PREV_REC_ERROR; i++) {
      diff += fabs(prev_rec_error[i] - rec_error);
    }
    diff /= LEN_PREV_REC_ERROR;
    debug_print("Diff (Reconstruction Error): %g\n", diff);

    if (mon != NULL)
    {
      mon->reconstruction_error[iter+1] = rec_error;
      mon->max_diff_factors[iter+1] = diff_f;
      mon->max_diff_coefficients[iter+1] = diff_c;
      mon->n_iter = iter;
    }
    iter = iter + 1;
  }
  ws_free(ws);

  return 0;
}


/* ///////////////////////////////////////////////////////////////////// */
/*!
  \file
  \brief Contains basic functions for problem initialization.

  The init.c file collects most of the user-supplied functions useful
  for problem configuration.
  It is automatically searched for by the makefile.

  \author A. Mignone (mignone@ph.unito.it)
  \date   March 5, 2017
*/
/* ///////////////////////////////////////////////////////////////////// */
#include "pluto.h"
#include "local_pluto.h"
#include <math.h>
#include "coolit.h"
#include "init_interp.h"
#include <stdbool.h>
//#include <gsl.h>

double mu, mue, mui, Rcl;
double rho0, P0, cs0, v0, s0, cool_0, tcool0, r_crit;
double rhobnd[2], pbnd[2], vbnd[2]; //boundary conditions at inner and outer radius

int q = 1, counter = 0;
double T0 , Mach0, n0, chi, singular;

/* ********************************************************************* */
void Init (double *v, double x1, double x2, double x3)
/*!
 * The Init() function can be used to assign initial conditions as
 * as a function of spatial position.
 *
 * \param [out] v   a pointer to a vector of primitive variables
 * \param [in] x1   coordinate point in the 1st dimension
 * \param [in] x2   coordinate point in the 2nd dimension
 * \param [in] x3   coordinate point in the 3rdt dimension
 *
 * The meaning of x1, x2 and x3 depends on the geometry:
 * \f[ \begin{array}{cccl}
 *    x_1  & x_2    & x_3  & \mathrm{Geometry}    \\ \noalign{\medskip}
 *     \hline
 *    x    &   y    &  z   & \mathrm{Cartesian}   \\ \noalign{\medskip}
 *    R    &   z    &  -   & \mathrm{cylindrical} \\ \noalign{\medskip}
 *    R    & \phi   &  z   & \mathrm{polar}       \\ \noalign{\medskip}
 *    r    & \theta & \phi & \mathrm{spherical}
 *    \end{array}
 *  \f]
 *
 * Variable names are accessed by means of an index v[nv], where
 * nv = RHO is density, nv = PRS is pressure, nv = (iVR, VX2, VX3) are
 * the three components of velocity, and so forth.
 *
 *********************************************************************** */
{

  #if PHYSICS == MHD || PHYSICS == RMHD
  v[BX1] = 0.0;
  v[BX2] = 0.0;
  v[BX3] = 0.0;

  v[AX1] = 0.0;
  v[AX2] = 0.0;
  v[AX3] = 0.0;
  #endif
}

/* ********************************************************************* */
void InitDomain (Data *d, Grid *grid)
/*!
 * Assign initial condition by looping over the computational domain.
 * Called after the usual Init() function to assign initial conditions
 * on primitive variables.
 * Value assigned here will overwrite those prescribed during Init().
 *
 *
 *********************************************************************** */
{
	int i, j, k;
	int id;
	double *x1 = grid->x[IDIR];
	double *x2 = grid->x[JDIR];
	double *x3 = grid->x[KDIR];
	double ne0, ni0;
	double* muemui = MeanElIonWeight((double*)d->Vc);
    mue = muemui[0];
	mui = muemui[1];
	mu = MeanMolecularWeight((double*)d->Vc);

	T0 = g_inputParam[T_CRIT]; //K
	Mach0 = g_inputParam[MACH_CRIT];
	n0 = g_inputParam[N_CRIT]; //cm^-3
	chi = g_inputParam[CHI];
	singular = g_inputParam[SINGULAR];

	rho0 = n0*mu*CONST_mp; //g cm^-3
	ne0 = n0*mu/mue;
	ni0 = n0*mu/mui;
	P0 = n0*CONST_kB*T0; //dyne cm^-2
	cs0 = sqrt(g_gamma*P0/rho0); //cm s^-1
	v0 = -Mach0*cs0; //cm s^-1
	s0 = P0/pow(rho0,g_gamma); //CGS
	cool_0 = lambda(T0); //CGS
	tcool0 = (1./(g_gamma-1))*n0*CONST_kB*T0/(ne0*ni0*cool_0); //s

	r_crit = -q*v0*g_gamma*tcool0; //cm
	Rcl = singular*r_crit*pow(UNIT_LENGTH,-1); // code units (pc usually)

	//print("Test: %.3e\t%.3e\t%.3e\n",r_crit,UNIT_LENGTH,r_crit*pow(UNIT_LENGTH,-1));
	print("T0=%.1e\tM0=%.1f\tn0=%.2e\t\tchi=%d\tsingular=%.2f\n",T0,Mach0, n0, (int)chi, singular);
	print("rho0=%.1e\t\tP0=%.1e\t\tv0=%.2e\t\tr_crit=%.2e kpc\tRcl=%.2e kpc\t\ttcool0=%.2e Myr\n",rho0/UNIT_DENSITY,P0/(UNIT_DENSITY*pow(UNIT_VELOCITY,2)), v0/UNIT_VELOCITY, r_crit*pow(UNIT_LENGTH,-1), Rcl, tcool0*pow(CONST_PI*1e13,-1.));
	print("Starting from %.2f kpc (r/ro=%.2e) to %.2f kpc (r/r0=%.2e)\n",g_domBeg[IDIR],g_domBeg[IDIR]/r_crit*UNIT_LENGTH,g_domEnd[IDIR],g_domEnd[IDIR]/r_crit*UNIT_LENGTH);
	print("Using Solar metallicity\n");
	print("Useful info: mu=%.2f mu_e=%.2f mu_i=%.2f\n",mu,mue,mui);
	print("Mass flux: %.2e Msun/yr\n",-4*CONST_PI*r_crit*r_crit*rho0*v0/(CONST_Msun/(365*24*60*60)));

	#if SUBSONIC_INI != NO
	readICfile("subsonic_rdpv.txt");
	#else
	readICfile("transonic_rdpv.txt");
	#endif

	TOT_LOOP(k,j,i){
		//if (prank==0 && (x1[i]<g_domBeg[IDIR] || x1[i]>g_domEnd[IDIR])) printf("%.2f pc\n",x1[i]);
		double* dpv = initdpv(x1[i]*UNIT_LENGTH/r_crit);
		if (i==0){
			rhobnd[0] = dpv[0]; pbnd[0] = dpv[1]; vbnd[0] = dpv[2];
		}
		else if(i==NX1-10){
			rhobnd[1] = dpv[0]; pbnd[1] = dpv[1]; vbnd[1] = dpv[2];
		}
		//print("%.2e\t\t%.2e\t\t%.2e\t\t%.2e\n",x1[i]*UNIT_LENGTH/r_crit,dpv[0],dpv[1],dpv[2]);
		d->Vc[RHO][k][j][i] = dpv[0]*rho0/UNIT_DENSITY; //(x1[i]/Rcl<=1.)?chi*n0*mu*CONST_mp/UNIT_DENSITY:n0*mu*CONST_mp/UNIT_DENSITY;
		d->Vc[PRS][k][j][i] = dpv[1]*P0/(UNIT_DENSITY*pow(UNIT_VELOCITY,2));
		EXPAND(d->Vc[iVR][k][j][i] = dpv[2]*abs(v0)/UNIT_VELOCITY; ,d->Vc[iVZ][k][j][i] = 0.;, d->Vc[VX3][k][j][i] = 0.;)
		d->Vc[TRC][k][j][i] = 1.0;
	}
	print("Done reading IC from file!\n");
	print("Left fixed boundary: rho/rho0=%.2e\t\tp/p0=%.2e\t\tv/v0=%.2e\n",rhobnd[0],pbnd[0],vbnd[0]);
	print("Right fixed boundary: rho/rho0=%.2e\t\tp/p0=%.2e\t\tv/v0=%.2e\n",rhobnd[1],pbnd[1],vbnd[1]);
	#if VX1RHO_OUTFLOW ==YES
	print("iVR and Density are outflowing.\n");
	#endif
}

/* ********************************************************************* */
void Analysis (const Data *d, Grid *grid)
/*!
 *  Perform runtime data analysis.
 *
 * \param [in] d the PLUTO Data structure
 * \param [in] grid   pointer to array of Grid structures
 *
 *********************************************************************** */
{
	/*
	int i, j, k;
	double *dx, *dy, *dz;*/
	/* ---- Parallel data reduction ---- */
	/*
	#ifdef PARALLEL
	int transfer_size = 3;
	int transfer = 0;
	double sendArray [transfer_size], recvArray[transfer_size];*/

	/* ---- Set pointer shortcuts ---- */
	/*
	dx = grid->dx[IDIR];
	dy = grid->dx[JDIR];
	dz = grid->dx[KDIR];*/

}
#if PHYSICS == MHD
/* ********************************************************************* */
void BackgroundField (double x1, double x2, double x3, double *B0)
/*!
 * Define the component of a static, curl-free background
 * magnetic field.
 *
 * \param [in] x1  position in the 1st coordinate direction \f$x_1\f$
 * \param [in] x2  position in the 2nd coordinate direction \f$x_2\f$
 * \param [in] x3  position in the 3rd coordinate direction \f$x_3\f$
 * \param [out] B0 array containing the vector componens of the background
 *                 magnetic field
 *********************************************************************** */
{
   B0[0] = 0.0;
   B0[1] = 0.0;
   B0[2] = 0.0;
}
#endif

/* ********************************************************************* */
void UserDefBoundary (const Data *d, RBox *box, int side, Grid *grid)
/*!
 *  Assign user-defined boundary conditions.
 *
 * \param [in,out] d  pointer to the PLUTO data structure containing
 *                    cell-centered primitive quantities (d->Vc) and
 *                    staggered magnetic fields (d->Vs, when used) to
 *                    be filled.
 * \param [in] box    pointer to a RBox structure containing the lower
 *                    and upper indices of the ghost zone-centers/nodes
 *                    or edges at which data values should be assigned.
 * \param [in] side   specifies the boundary side where ghost zones need
 *                    to be filled. It can assume the following
 *                    pre-definite values: X1_BEG, X1_END,
 *                                         X2_BEG, X2_END,
 *                                         X3_BEG, X3_END.
 *                    The special value side == 0 is used to control
 *                    a region inside the computational domain.
 * \param [in] grid  pointer to an array of Grid structures.
 *
 *********************************************************************** */
{
  int   i, j, k, nv;
  double  *x1, *x2, *x3;

  x1 = grid->x[IDIR];
  x2 = grid->x[JDIR];
  x3 = grid->x[KDIR];

  if (side == X1_BEG) {

    BOX_LOOP(box,k,j,i) {
		d->Vc[PRS][k][j][i] = pbnd[0]*P0/(UNIT_DENSITY*pow(UNIT_VELOCITY,2)); //only fix the pressure
		#if VX1RHO_OUTFLOW ==YES
		d->Vc[RHO][k][j][i] = d->Vc[RHO][k][j][IBEG];   //outflow density
		d->Vc[iVR][k][j][i] = d->Vc[iVR][k][j][IBEG];   //outflow velocity
		#else
		d->Vc[RHO][k][j][i] = rhobnd[0]*rho0/UNIT_DENSITY;
		d->Vc[iVR][k][j][i] = vbnd[0]*abs(v0)/UNIT_VELOCITY;
		#endif
	}

  }else if (side == X1_END){

    BOX_LOOP(box,k,j,i) {
		d->Vc[PRS][k][j][i] = pbnd[1]*P0/(UNIT_DENSITY*pow(UNIT_VELOCITY,2));
		#if VX1RHO_OUTFLOW ==YES
		d->Vc[RHO][k][j][i] = d->Vc[RHO][k][j][IEND];   //outflow density
		d->Vc[iVR][k][j][i] = d->Vc[iVR][k][j][IEND]; //outflow velocity
		#else
		d->Vc[RHO][k][j][i] = rhobnd[1]*rho0/UNIT_DENSITY;
		d->Vc[iVR][k][j][i] = vbnd[1]*abs(v0)/UNIT_VELOCITY;
		#endif
	}

  }

  /*
  if (side == 0) {  */  /* -- check solution inside domain -- */
  /* TOT_LOOP(k,j,i){

    if ((x1[i]*UNIT_LENGTH/r_crit>=(1.-1e-1)) && (x1[i]*UNIT_LENGTH/r_crit<=(1.+1e-1))) {
		d->Vc[RHO][k][j][i] = rho0/UNIT_DENSITY;
		d->Vc[iVR][k][j][i] = v0/UNIT_VELOCITY;
		d->Vc[PRS][k][j][i] = P0/(UNIT_DENSITY*pow(UNIT_VELOCITY,2));
	}/*
	if (x1[i]/Rcl<1.)
		d->Vc[iVR][k][j][i] = 0.; *//*
   }
  }*/
}

#if BODY_FORCE != NO
/* ********************************************************************* */
void BodyForceVector(double *v, double *g, double x1, double x2, double x3)
/*!
 * Prescribe the acceleration vector as a function of the coordinates
 * and the vector of primitive variables *v.
 *
 * \param [in] v  pointer to a cell-centered vector of primitive
 *                variables
 * \param [out] g acceleration vector
 * \param [in] x1  position in the 1st coordinate direction \f$x_1\f$
 * \param [in] x2  position in the 2nd coordinate direction \f$x_2\f$
 * \param [in] x3  position in the 3rd coordinate direction \f$x_3\f$
 *
 *********************************************************************** */
{
  g[IDIR] = 0.0;
  g[JDIR] = 0.0;
  g[KDIR] = 0.0;
}
/* ********************************************************************* */
double BodyForcePotential(double x1, double x2, double x3)
/*!
 * Return the gravitational potential as function of the coordinates.
 *
 * \param [in] x1  position in the 1st coordinate direction \f$x_1\f$
 * \param [in] x2  position in the 2nd coordinate direction \f$x_2\f$
 * \param [in] x3  position in the 3rd coordinate direction \f$x_3\f$
 *
 * \return The body force potential \f$ \Phi(x_1,x_2,x_3) \f$.
 *
 *********************************************************************** */
{
  return 0.0;
}
#endif


/*
** Implementation of LISA algorithm
** for statistical inference of fMRI images
**
** 2nd level inference (one-sample test)
**
** G.Lohmann, 2017
*/
#include <viaio/Vlib.h>
#include <viaio/file.h>
#include <viaio/mu.h>
#include <viaio/option.h>
#include <viaio/os.h>
#include <viaio/VImage.h>

#include <gsl/gsl_cdf.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_histogram.h>

#include <math.h>
#include <stdio.h>
#include <string.h>

#ifdef _OPENMP
#include <omp.h>
#endif /*_OPENMP*/

#define ABS(x) ((x) > 0 ? (x) : -(x))

extern void   VIsolatedVoxels(VImage src,float threshold);
extern void   VHistogram(gsl_histogram *histogram,VString filename);
extern void   VCheckImage(VImage src);
extern void   FDR(VImage src,VImage dest,gsl_histogram *nullhist,gsl_histogram *realhist,double);
extern double ttest1(double *data1,int n);
extern void   ImageStats(VImage src,double *,double *,double *hmin,double *hmax);
extern void   VBilateralFilter(VImage src,VImage dest,int radius,double var1,double var2,int);
extern double VImageVar(VImage src);
extern void   VImageCount(VImage src);
extern void   VGetHistRange(VImage src,double *hmin,double *hmax);
extern void   VZScale(VImage src,float mode,float stddev);
extern float  VGetMode(VImage src);
extern double t2z(double,double);
extern void   HistoUpdate(VImage,gsl_histogram *);

/* generate permutation table */
int **genperm(gsl_rng *rx,int n,int numperm)
{
  int i,j;
  int **table = (int **) VCalloc(numperm,sizeof(int *));
  for (i = 0; i < numperm; i++) {
    table[i] = (int *) VCalloc(n,sizeof(int));
    for (j=0; j<n; j++) {
      table[i][j] = 0;
      if (gsl_ran_bernoulli (rx,(double)0.5) == 1) table[i][j] = 1;
    }
  }
  return table;
}

/* onesample t-test */
void OnesampleTest(VImage *src1,VImage dest,int *permtable,int n)
{
  int i,k,b,r,c,nslices,nrows,ncols;
  double nx,ave,var,u,t,z,df;
  double tiny=1.0e-8;
  extern void avevar(double *data,int n,double *a,double *v);

  nslices = VImageNBands(src1[0]);
  nrows   = VImageNRows(src1[0]);
  ncols   = VImageNColumns(src1[0]);
  VFillImage(dest,VAllBands,0);
  double *data = (double *) VCalloc(n,sizeof(double));

  for (b=0; b<nslices; b++) {
    for (r=0; r<nrows; r++) {
      for (c=0; c<ncols; c++) {
	k = 0;
	for (i=0; i<n; i++) {
	  u = (double)VPixel(src1[i],b,r,c,VFloat);
	  if (ABS(u) > tiny) {
	    if (permtable[i] > 0) u = -u;
	    data[k] = u;
	    k++;
	  }
	}
	if (k < n-2) continue;
	avevar(data,k,&ave,&var);
	if (var < tiny) continue;
	nx   = (double)k;
	t    = sqrt(nx) * ave/sqrt(var);
	df   = nx - 1.0;
	z    = t2z(t,df);
	if (t < 0) z = -z;
	VPixel(dest,b,r,c,VFloat) = z;
      }
    }
  }
  VFree(data);
}

int main (int argc, char *argv[])
{
  static VArgVector in_files1;
  static VString  out_filename="";
  static VString  mask_filename="";
  static VFloat   alpha = 0.05;
  static VShort   radius = 2;
  static VFloat   rvar = 2.0;
  static VFloat   svar = 2.0;
  static VShort   numiter = 2;
  static VShort   numperm = 5000;
  static VLong    seed = 99402622;
  static VBoolean centering = FALSE;
  static VBoolean cleanup = TRUE;
  static VShort   nproc = 0;
  static VOptionDescRec options[] = {
    {"in", VStringRepn, 0, & in_files1, VRequiredOpt, NULL,"Input files" },
    {"out", VStringRepn, 1, & out_filename, VRequiredOpt, NULL,"Output file" },
    {"alpha",VFloatRepn,1,(VPointer) &alpha,VOptionalOpt,NULL,"FDR significance level"},
    {"perm",VShortRepn,1,(VPointer) &numperm,VOptionalOpt,NULL,"Number of permutations"},
    {"mask", VStringRepn, 1, (VPointer) &mask_filename, VRequiredOpt, NULL, "Mask"},
    {"seed",VLongRepn,1,(VPointer) &seed,VOptionalOpt,NULL,"Seed for random number generation"},
    {"radius",VShortRepn,1,(VPointer) &radius,VOptionalOpt,NULL,"Bilateral parameter (radius in voxels)"},
    {"rvar",VFloatRepn,1,(VPointer) &rvar,VOptionalOpt,NULL,"Bilateral parameter (radiometric)"},
    {"svar",VFloatRepn,1,(VPointer) &svar,VOptionalOpt,NULL,"Bilateral parameter (spatial)"},
    {"filteriterations",VShortRepn,1,(VPointer) &numiter,VOptionalOpt,NULL,"Bilateral parameter (number of iterations)"},
    {"cleanup",VBooleanRepn,1,(VPointer) &cleanup,VOptionalOpt,NULL,"Whether to remove isloated voxels"},
    {"j",VShortRepn,1,(VPointer) &nproc,VOptionalOpt,NULL,"Number of processors to use, '0' to use all"},
  };

  FILE *fp=NULL;
  VString in_filename;
  VAttrList list1=NULL,out_list=NULL,geolist=NULL;
  VImage *src1;
  int i,nimages,npix=0;
  char *prg_name=GetLipsiaName("vlisa_onesample");
  fprintf (stderr, "%s\n", prg_name);

  /* parse command line */
  if (! VParseCommand (VNumber (options), options, & argc, argv)) {
    VReportUsage (argv[0], VNumber (options), options, NULL);
    exit (EXIT_FAILURE);
  }
  if (argc > 1) {
    VReportBadArgs (argc, argv);
    exit (EXIT_FAILURE);
  }

  /* omp-stuff */
#ifdef _OPENMP
  int num_procs=omp_get_num_procs();
  if (nproc > 0 && nproc < num_procs) num_procs = nproc;
  fprintf(stderr," using %d cores\n",(int)num_procs);
  omp_set_num_threads(num_procs);
#endif /* _OPENMP */

  /* images  */
  VImage mask = VReadImageFile(mask_filename);
  if (mask==NULL) VError("Error reading mask file %s",mask_filename);

  nimages = in_files1.number;
  src1 = (VImage *) VCalloc(nimages,sizeof(VImage));
  for (i = 0; i < nimages; i++) {
    in_filename = ((VString *) in_files1.vector)[i];
    list1   = VReadAttrList(in_filename,0L,TRUE,FALSE);
    VMaskMinval(list1,mask,0.0);

    src1[i] = VReadImage(list1);
    if (src1[i] == NULL) VError(" no input image found");
    if (VPixelRepn(src1[i]) != VFloatRepn) VError(" input pixel repn must be float");
    if (i == 0) npix = VImageNPixels(src1[i]);
    else if (npix != VImageNPixels(src1[i])) VError(" inconsistent image dimensions");

    /* use geometry info from 1st file */
    if (geolist == NULL) {
      geolist = VGetGeoInfo(list1);
      double *D = VGetGeoPixdim(geolist,NULL);
      if (fabs(D[0]-4.0) < TINY) VError(" The input must be a list of separate 3D images, not one 4D file");
    }
  }

  /* ini random permutations */
  size_t n = nimages;
  gsl_rng_env_setup();
  const gsl_rng_type *T = gsl_rng_default;
  gsl_rng *rx = gsl_rng_alloc(T);
  gsl_rng_set(rx,(unsigned long int)seed);
  int nperm=0;
  int **permtable = genperm(rx,(int)n,(int)numperm);

  /* estimate null variance based on first 30 permutations */
  double hmin=0,hmax=0;
  float stddev=1.0;
  if (numperm > 0) {
    int tstperm = 30;
    if (tstperm > numperm) tstperm = numperm;
    double varsum=0,nx=0;

#pragma omp parallel for shared(src1,permtable) schedule(dynamic)
    for (nperm = 0; nperm < tstperm; nperm++) {
      VImage zmap = VCreateImageLike(src1[0]);
      OnesampleTest(src1,zmap,permtable[nperm],nimages);
#pragma omp critical
      {
	varsum += VImageVar(zmap);
	nx++;
      }
      VDestroyImage(zmap);
    }
    double meanvar = varsum/nx;
    stddev = sqrt(meanvar);
  }

  /* no permutation */
  int *nopermtable = (int *) VCalloc(n,sizeof(int));
  VImage dst1  = VCreateImageLike (src1[0]);
  VImage zmap1 = VCreateImageLike(src1[0]);
  OnesampleTest(src1,zmap1,nopermtable,nimages);

  if (numperm == 0) {
    double z = VImageVar(zmap1);
    stddev = (float)(sqrt(z)); /* update stddev */
  }
  float mode=0;
  if (centering) mode = VGetMode(zmap1);
  if (numperm > 0) VZScale(zmap1,mode,stddev);
  VBilateralFilter(zmap1,dst1,(int)radius,(double)(rvar),(double)svar,(int)numiter);

  /* ini histograms */
  VGetHistRange(dst1,&hmin,&hmax);
  size_t nbins = 20000;
  gsl_histogram *hist0 = gsl_histogram_alloc (nbins);
  gsl_histogram_set_ranges_uniform (hist0,hmin,hmax);
  gsl_histogram *histz = gsl_histogram_alloc (nbins);
  gsl_histogram_set_ranges_uniform (histz,hmin,hmax);
  HistoUpdate(dst1,histz);

  /* random permutations */
#pragma omp parallel for shared(src1,permtable) schedule(dynamic)
  for (nperm = 0; nperm < numperm; nperm++) {
    if (nperm%20 == 0) fprintf(stderr," perm  %4d  of  %d\r",nperm,(int)numperm);

    VImage zmap = VCreateImageLike(src1[0]);
    VImage dst  = VCreateImageLike (zmap);
    OnesampleTest(src1,zmap,permtable[nperm],nimages);
    float mode=0;
    if (centering) mode = VGetMode(zmap);
    VZScale(zmap,mode,stddev);
    VBilateralFilter(zmap,dst,(int)radius,(double)(rvar),(double)svar,(int)numiter);

#pragma omp critical
    {
      HistoUpdate(dst,hist0);
    }
    VDestroyImage(dst);
    VDestroyImage(zmap);
  }

  /* apply fdr */
  VImage fdrimage = VCopyImage (dst1,NULL,VAllBands);
  if (numperm > 0) {
    FDR(dst1,fdrimage,hist0,histz,(double)alpha);

    if (cleanup && alpha < 1.0) {
      VIsolatedVoxels(fdrimage,(float)(1.0-alpha));
    }
    if (alpha < 1.0) VImageCount(fdrimage);
  }

  /* write output to disk */
  out_list = VCreateAttrList ();
  VHistory(VNumber(options),options,prg_name,&list1,&out_list);
  VSetGeoInfo(geolist,out_list);
  VAppendAttr (out_list,"image",NULL,VImageRepn,fdrimage);
  fp = VOpenOutputFile (out_filename, TRUE);
  if (! VWriteFile (fp, out_list)) exit (1);
  fclose(fp);
  fprintf (stderr, "\n%s: done.\n", argv[0]);
  exit(0);
}


/*
 *  maths_base.h
 *  ecosstat_project
 *
 *  Created by Karim Benabed on 23/06/09.
 *  Copyright 2009 Institut d'Astrophysique de Paris. All rights reserved.
 *
 */

#ifndef __MATHS_BASE_H
#define __MATHS_BASE_H

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#if 0
#include <gsl/gsl_vector.h>
#include <gsl/gsl_vector_int.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>
#endif

#ifdef __PLANCK__
#include "HL2_likely/tools/errorlist.h"
#include "HL2_likely/tools/io.h"
#else
#include "errorlist.h"
#include "io.h"
#endif

#define math_base                -300
#define math_negative            -1 + math_base
#define math_singularValue       -2 + math_base
#define math_tooManySteps        -3 + math_base
#define math_underflow	         -4 + math_base
#define math_infnan	         -5 + math_base
#define math_wrongValue	         -6 + math_base
#define math_alloc	         -7 + math_base
#define math_interpoloutofrange	 -8 + math_base
#define math_interpol2small      -9 + math_base
#define math_interpol2big       -10 + math_base
#define math_stackTooSmall      -11 + math_base
#define math_overflow	        -12 + math_base
#define math_unknown            -13 + math_base

typedef double my_complex[2];

/* Mathematical constants */
#define pi     3.14159265358979323846
#define pi_sqr 9.86960440108935861883
#define twopi  6.28318530717958647693
#define ln2    0.69314718
#define ln2pi  1.837877066409
#define arcmin 2.90888208665721580e-4
#define arcsec 4.84813681e-6

/* Confidence levels, = erf({1,2,3}/sqrt(2)) */
#define conf_68 0.6827
#define conf_90 0.9000
#define conf_95 0.9545
#define conf_99 0.9973

/* Small numbers */
#define EPSILON  1.0e-5
#define EPSILON1 1.0e-8
#define EPSILON2 1.0e-18

/* Square of the absolute value of x (=|x|^2) */
#define ABSSQR(x) ((x)[0]*(x)[0] + (x)[1]*(x)[1])

/* Scalar product (x,y) */
#define SP(x,y) ((x)[0]*(y)[0]+(x)[1]*(y)[1])

#endif


/* Copyright (c) 2014, Giuseppe Argentieri <giuseppe.argentieri@ts.infn.it>

 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 */
/*
 *
 *
 *       Filename:  hamil.c
 *
 *    Description:  Create an hamiltonian generator in Bloch form, from a given
 *    			vector omega
 *
 *        Version:  1.0
 *        Created:  03/05/2014 14:16:00
 *       Revision:  none
 *        License:  BSD
 *
 *         Author:  Giuseppe Argentieri (ga), giuseppe.argentieri@ts.infn.it
 *   Organization:  Università degli Studi di Trieste
 *
 *
 */

#include "funcs.h"
#include <gsl/gsl_matrix.h>

/*
 *      FUNCTION
 *         Name:  ham_gen
 *  Description:
 *
 */
int ham_gen ( gsl_matrix* h, const double* o )
{
	gsl_matrix_set (h, 1, 2, o[3] ) ;
	gsl_matrix_set (h, 1, 3, -o[2] ) ;
	gsl_matrix_set (h, 2, 1, -o[3] ) ;
	gsl_matrix_set (h, 2, 3, o[1] ) ;
	gsl_matrix_set (h, 3, 1, o[2] ) ;
	gsl_matrix_set (h, 3, 2, -o[1] ) ;

	/*  Multiplies times -4 to obtain the Bloch matrix */
	gsl_matrix_scale ( h, -4 ) ;

	return 0;
}		/* -----  end of function ham_gen  ----- */


/*
 * Copyright 2016 Maikel Nadolski
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef HIDDEN_MARKOV_MODEL_H_
#define HIDDEN_MARKOV_MODEL_H_

#include <Eigen/Dense>
#include <gsl_util.h>

#include "maikel/hmm/stochastical_conditions.h"

namespace maikel { namespace hmm {

  struct hmm_errors: public std::runtime_error {
    hmm_errors(std::string s): std::runtime_error(s) {}
  };

  struct dimensions_not_consistent: public hmm_errors {
    dimensions_not_consistent(const std::string& a): hmm_errors(a) {}
  };

  template <class T>
    class hidden_markov_model
      {
        public:
          using matrix     = typename Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
          using row_vector = typename Eigen::Matrix<T, 1, Eigen::Dynamic>;
          using size_type  = typename matrix::Index;

          struct arguments_not_probability_arrays: public hmm_errors {
              matrix A;
              matrix B;
              row_vector pi;
              arguments_not_probability_arrays(
                  const matrix& A_,
                  const matrix& B_,
                  const row_vector& pi_,
                  const std::string& a): hmm_errors(a), A{A_}, B{B_}, pi{pi_} {}
          };

          hidden_markov_model(
              const matrix&     transition_matrix,
              const matrix&     symbol_matrix,
              const row_vector& initial_dist )
           : A { transition_matrix },
             B { symbol_matrix     },
             pi{ initial_dist      },
             num_states  { B.rows() },
             num_symbols { B.cols() }
          {
            if (!rows_are_probability_arrays(A) || !rows_are_probability_arrays(B)
                || !is_probability_array(pi.array()))
              throw arguments_not_probability_arrays
                { A, B, pi, "Some inputs in constructor do not have the stochastical property." };
            if (A.rows() != A.cols() || A.rows() != B.rows() || A.rows() != pi.cols())
              throw dimensions_not_consistent
                { "Dimensions of input matrices are not consistent with each other." };
          }

          inline size_type states() const noexcept  { return num_states;  }
          inline size_type symbols() const noexcept { return num_symbols; }

          inline const matrix&     transition_matrix()    const noexcept { return A; }
          inline const matrix&     symbol_probabilities() const noexcept { return B; }
          inline const row_vector& initial_distribution() const noexcept { return pi; }

        private:
          matrix A;
          matrix B;
          row_vector pi;
          size_type num_states;
          size_type num_symbols;
      };

} // namespace hmm
} // namespace maikel

#endif /* HIDDEN_MARKOV_MODEL_H_ */


#include <cblas.h>

#include "tasks.h"
#include "utils.h"

void gemm_task_par_reconfigure(int nth)
{
    // empty
}

void gemm_task_par_finalize(void)
{
    // empty
}

void gemm_task_par(void *ptr, int nth, int me)
{
    struct gemm_task_arg *arg = (struct gemm_task_arg*) ptr;

    int m        = arg->m;
    int n        = arg->n;
    int k        = arg->k;
    double *A21  = arg->A21;
    double *A21T = arg->A21T;
    double *A22  = arg->A22;
    int ldA      = arg->ldA;

    // Cut A21T into panels of size m-by-blksz. Compute nominal block size.
    int blksz = iceil(n, nth);

    // Determine my share of A21T.
    int my_first_col = blksz * me;
    int my_num_cols  = min(blksz, n - my_first_col);

    // Compute A22 := A22 - A21 * A21T', using my share of A22 and A21T.
    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasTrans,
                m, my_num_cols, k,
                -1.0, A21,                      ldA,
                      A21T + my_first_col,      ldA,
                 1.0, A22 + my_first_col * ldA, ldA);
}


/* rng/ranmar.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 James Theiler, Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_rng.h>

/* This is the RANMAR lagged fibonacci generator of Marsaglia, Zaman
   and Tsang.  The sequence is a series of 24-bit integers, x_n,

   x_n = (y_n - c_n + 2^24) mod 2^24

   where,

   y_n = (y_{n-97) - y_{n-33} + 2^24) mod 2^24
   c_n = (c_{n-1} - 7654321 + 2^24 - 3) mod (2^24 - 3)

   The period of this generator is 2^144.

   The generator provides about 900 million different subsequences
   each of length O(10^30). Thus each seed up to 900,000,000 gives an
   independent sequence.

   Although it was good in its day this generator now has known
   statistical defects and has been superseded by RANLUX.

   From: F. James, "A Review of Pseudorandom number generators",
   Computer Physics Communications 60, 329 (1990).

   G. Marsaglia, A. Zaman and W.W. Tsang, Stat. Prob. Lett. 9, 35 (1990)  */

static inline unsigned long int ranmar_get (void *vstate);
static double ranmar_get_double (void *vstate);
static void ranmar_set (void *state, unsigned long int s);

static const unsigned long int two24 = 16777216;	/* 2^24 */

typedef struct
  {
    unsigned int i;
    unsigned int j;
    long int carry;
    unsigned long int u[97];
  }
ranmar_state_t;

static inline unsigned long int
ranmar_get (void *vstate)
{
  ranmar_state_t *state = (ranmar_state_t *) vstate;

  unsigned int i = state->i;
  unsigned int j = state->j;
  long int carry = state->carry;

  long int delta = state->u[i] - state->u[j];

  if (delta < 0)
    delta += two24 ;

  state->u[i] = delta;

  if (i == 0)
    {
      i = 96;
    }
  else
    {
      i--;
    }

  state->i = i;

  if (j == 0)
    {
      j = 96;
    }
  else
    {
      j--;
    }

  state->j = j;

  carry += - 7654321 ;

  if (carry < 0)
    carry += two24 - 3;

  state->carry = carry ;

  delta += - carry ;

  if (delta < 0)
    delta += two24 ;

  return delta;
}

static double
ranmar_get_double (void *vstate)
{
  return ranmar_get (vstate) / 16777216.0 ;
}

static void
ranmar_set (void *vstate, unsigned long int s)
{
  ranmar_state_t *state = (ranmar_state_t *) vstate;

  unsigned long int ij = s / 30082 ;
  unsigned long int kl = s % 30082 ;

  int i = (ij / 177) % 177 + 2 ;
  int j = (ij % 177) + 2 ;
  int k = (kl / 169) % 178 + 1 ;
  int l = (kl % 169) ;

  int a, b;

  for (a = 0; a < 97; a++)
    {
      unsigned long int sum = 0 ;
      unsigned long int t = two24 ;

      for (b = 0; b < 24; b++)
	{
	  unsigned long int m = (((i * j) % 179) * k) % 179 ;
	  i = j ;
	  j = k ;
	  k = m ;
	  l = (53 * l + 1) % 169 ;
	  t >>= 1 ;

	  if ((l * m) % 64 >= 32)
	    sum += t ;
	}

      state->u[a] = sum ;
    }

  state->i = 96;
  state->j = 32;
  state->carry = 362436 ;

}

static const gsl_rng_type ranmar_type =
{"ranmar",			/* name */
 0x00ffffffUL,			/* RAND_MAX */
 0,				/* RAND_MIN */
 sizeof (ranmar_state_t),
 &ranmar_set,
 &ranmar_get,
 &ranmar_get_double};

const gsl_rng_type *gsl_rng_ranmar = &ranmar_type;


/****************************************************************
 *
 * vpreprocess: FreqFilter.c
 *
 * Copyright (C) Max Planck Institute
 * for Human Cognitive and Brain Sciences, Leipzig
 *
 * <lipsia@cbs.mpg.de>
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 *
 * $Id: FreqFilter.c 3181 2008-04-01 15:19:44Z karstenm $
 *
 *****************************************************************/

#include <viaio/VImage.h>
#include <viaio/Vlib.h>
#include <viaio/mu.h>
#include <viaio/option.h>

#include <stdio.h>
#include <string.h>
#include <math.h>
#include <stdlib.h>
#include <fftw3.h>

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_errno.h>

extern gsl_matrix *DataMap(VImage *src,VImage mask,size_t nvox,int);

gsl_matrix *DataMap(VImage *src,VImage mask,size_t nvox,int tail)
{
  int j,b,r,c;
  int nrows=0,ncols=0,nt0=0;
  double tiny=1.0e-6;

  int nslices = VImageNBands(mask);
  VImageDimensions(src,nslices,&nt0,&nrows,&ncols);

  int nt = nt0 + 2*tail;
  gsl_matrix *A = gsl_matrix_calloc(nvox,nt);

  size_t n=0;
  for (b=0; b<nslices; b++) {
    for (r=0; r<nrows; r++) {
      for (c=0; c<ncols; c++) {

	double u = VGetPixel(src[b],0,r,c);
	if (fabs(u) < tiny) continue;

	for (j=0; j<tail; j++) {
	  u = (double)VGetPixel(src[b],tail-j,r,c);
	  gsl_matrix_set(A,n,j,u);
	}
	for (j=tail; j<nt-tail; j++) {
	  u = (double)VGetPixel(src[b],j-tail,r,c);
	  gsl_matrix_set(A,n,j,u);
	}
	for (j=nt-tail; j<nt; j++) {
	  u = (double)VGetPixel(src[b],2*nt-3*tail-2-j,r,c);
	  gsl_matrix_set(A,n,j,u);
	}
	n++;
      }
    }
  }
  return A;
}

void VFreqFilter(VAttrList list,VFloat high,VFloat low,VBoolean stop,VFloat sharp)
{
  VString str1=NULL;
  double *in=NULL;
  fftw_complex *out;
  fftw_plan p1,p2;
  int i,j,nc,b,r,c;
  float freq=0,alpha=0;
  double *highp=NULL, *lowp=NULL;
  double x_high, x_low, x;
  double u=0,v=0,tiny=1.0e-6;

  /* dialog messages */
  str1 = (VString)VMalloc(sizeof(char)*4);
  if (stop) strcpy(str1, "stop");
  else strcpy(str1, "pass");
  if (low>0 && high>0)
    fprintf(stderr," band %s filter: 1/%.1f Hz and 1/%.1f Hz\n",str1,high,low);
  else {
    if (high>0) fprintf(stderr," high %s filter: 1/%.1f Hz\n",str1,high);
    if (low>0)  fprintf(stderr," low %s filter: 1/%.1f Hz\n",str1,low);
  }

  /* read functional data */
  int nrows=0,ncols=0,nt=0;
  int nslices = VAttrListNumImages(list);
  VImage *src = VAttrListGetImages(list,nslices);
  VImageDimensions(src,nslices,&nt,&nrows,&ncols);

  /* read header info */
  double *D = (double *)VCalloc(8,sizeof(double));
  for (i=0; i<8; i++) D[i] = 1.0;
  VAttrList geolist = VGetGeoInfo(list);
  if (geolist != NULL) D = VGetGeoPixdim(geolist,D);
  double tr = D[4];

  VFloat xtr=-1;
  if (VGetAttr(VImageAttrList(src[0]),"repetition_time",NULL,VFloatRepn,&xtr) == VAttrFound) {
    tr = (double)xtr;
  }
  if (tr > 100) tr /= 1000.0;  /* convert to seconds */
  if (tr < 0.01) VError("FreqFilter: implausible TR (%f seconds)",tr);
  fprintf(stderr," image dimensions: %d x %d x %d,  nt= %d,  TR= %.3f secs\n",nslices,nrows,ncols,nt,tr);

  /* alloc memory */
  double nx = (double)nt;
  int tail = 50;
  if (nt<tail) tail=nt-2;
  nt  += 2 * tail;
  nc  = (nt / 2) + 1;
  in  = (double *)fftw_malloc(sizeof(double) * nt);
  out = fftw_malloc (sizeof (fftw_complex ) * nc);
  for (i=0; i<nt; i++) in[i] = 0;

  /* make plans */
  p1 = fftw_plan_dft_r2c_1d (nt,in,out,FFTW_ESTIMATE);
  p2 = fftw_plan_dft_c2r_1d (nt,out,in,FFTW_ESTIMATE);

  /* repetition time */
  alpha = (double)nt * tr;

  /* filter function */
  if (sharp > 0) {
    highp = (double *)malloc(sizeof(double) * nc);
    lowp  = (double *)malloc(sizeof(double) * nc);
    for (i=1; i <nc; i++) {
      highp[i] = 1.0 / (1.0 +  exp( (alpha/high -(double)i)*sharp )   );
      lowp[i]  = 1.0 / (1.0 +  exp( ((double)i - alpha/low)*sharp )   );
    }
  }

  /* create ROI mask */
  VImage mask = VCreateImage(nslices,nrows,ncols,VBitRepn);
  VFillImage(mask,VAllBands,0);
  size_t nvox=0;
  for (b=0; b<nslices; b++) {
    for (r=0; r<nrows; r++) {
      for (c=0; c<ncols; c++) {
	double u = VGetPixel(src[b],0,r,c);
	if (fabs(u) < tiny) continue;
	VPixel(mask,b,r,c,VBit) = 1;
	nvox++;
      }
    }
  }

  /* map to double values */
  gsl_matrix *A = DataMap(src,mask,nvox,tail);
  double zmax = gsl_matrix_max(A);
  double zmin = gsl_matrix_min(A);

  /* main process */
  size_t n=0;
  for (b=0; b<nslices; b++) {
    for (r=0; r<nrows; r++) {
      for (c=0; c<ncols; c++) {

	if (VPixel(mask,b,r,c,VBit) == 0) continue;
	for (i=0; i<nt; i++) {
	  in[i] = gsl_matrix_get(A,n,i);
	}

	/*  fft */
	fftw_execute(p1);

	/* remove specified frequencies */
	for (i=1; i <nc; i++) {

	  if (sharp > 0) {

	    /* highpass */
	    if (high > 0) x_high = highp[i];
	    else x_high = 1.0;

	    /* lowpass */
	    if (low > 0) x_low = lowp[i];
	    else x_low = 1.0;

	    x = x_high + x_low - 1.0;
	    if (stop) x = fabs(1.0-x);

	    out[i][0] *= x;
	    out[i][1] *= x;

	  }
	  else {

	    /* hard thresholding */
	    freq = 1.0/(double)i * alpha;     /* 1/frequency */

	    if ((!stop && (freq < low || (freq > high && high>0)))
		|| (stop && !(freq < low || (freq > high && high>0))))
	      out[i][0] = out[i][1] = 0;
	  }
	}

	/* inverse fft */
	fftw_execute(p2);
	for (i=0; i<nt; i++) {
	  gsl_matrix_set(A,n,i,in[i]/nx);
	}
	n++;
      }
    }
  }

  /* rescale */
  VRepnKind repn = VPixelRepn(src[0]);
  double smax = VPixelMaxValue(src[0]);
  double smin = VPixelMinValue(src[0]);

  double ymax = 32000;
  if (repn == VFloatRepn || repn == VDoubleRepn) ymax = 1000.0;

  for (b=0; b<nslices; b++) {
    VFillImage(src[b],VAllBands,0);
  }
  zmax = gsl_matrix_max(A);
  zmin = gsl_matrix_min(A);

  /* write output */
  n=0;
  for (b=0; b<nslices; b++) {
    for (r=0; r<nrows; r++) {
      for (c=0; c<ncols; c++) {
	if (VPixel(mask,b,r,c,VBit) == 0) continue;

	/* map to output image */
	for (j=tail; j<nt-tail; j++) {
	  u = gsl_matrix_get(A,n,j);
	  if (fabs(u) > 0.0001) u = (u-zmin)/(zmax-zmin);
	  else u = 0;
	  if (u > 1.0) u = 1.0;
	  if (u < 0) u = 0;

	  v = u*ymax;
	  if (v > smax) v = smax;
	  if (v < smin) v = smin;
	  VSetPixel(src[b],j-tail,r,c,v);
	}
	n++;
      }
    }
  }
  VDestroyImage(mask);
  gsl_matrix_free(A);
}