miscmaths.cc

		    const ColumnVector& centre, const float rmax) 
{
  Tracer trcr("rms_deviation");
  Matrix isodiff(4,4);
  try {
    isodiff = affmat1*affmat2.i() - Identity(4);
  } catch(...) {
    cerr << "RMS_DEVIATION ERROR:: Could not invert matrix" << endl;  
    exit(-5); 
  }
  Matrix adiff(3,3);
  adiff = isodiff.SubMatrix(1,3,1,3);
  ColumnVector tr(3);
  tr = isodiff.SubMatrix(1,3,4,4) + adiff*centre;
  float rms = std::sqrt( (tr.t() * tr).AsScalar() + 
		    (rmax*rmax/5.0)*Trace(adiff.t()*adiff) );
  return rms;
}


float rms_deviation(const Matrix& affmat1, const Matrix& affmat2, 
		    const float rmax) 
{
  ColumnVector centre(3);
  centre = 0;
  return rms_deviation(affmat1,affmat2,centre,rmax);
}


  // helper function - calls nifti, but with FSL default case

Matrix Mat44ToNewmat(mat44 m)
{
  Matrix r(4,4);

  for(unsigned short i = 0; i < 4; ++i)
    for(unsigned short j = 0; j < 4; ++j)
      r(i+1, j+1) = m.m[i][j];
      
  return r;
}

mat44 NewmatToMat44(const Matrix& m)
{
  mat44 r;

  for(unsigned short i = 0; i < 4; ++i)
    for(unsigned short j = 0; j < 4; ++j)
      r.m[i][j] = m(i+1, j+1);

  return r;
}

void get_axis_orientations(const Matrix& sform_mat, int sform_code,
			   const Matrix& qform_mat, int qform_code,
			   int& icode, int& jcode, int& kcode)
{
  Matrix vox2mm(4,4);
  if (sform_code!=NIFTI_XFORM_UNKNOWN) {
    vox2mm = sform_mat;
  } else if (qform_code!=NIFTI_XFORM_UNKNOWN) {
    vox2mm = qform_mat;
  } else {
    // ideally should be sampling_mat(), but for orientation it doesn't matter
    vox2mm = Identity(4);
    vox2mm(1,1) = -vox2mm(1,1);
  }
  mat44 v2mm;
  for (int ii=0; ii<4; ii++) { for (int jj=0; jj<4; jj++) {
      v2mm.m[ii][jj] = vox2mm(ii+1,jj+1);
    } }
  mat44_to_orientation(v2mm,&icode,&jcode,&kcode);
}
  

// Added by MWW

//  int getdiag(ColumnVector& diagvals, const Matrix& m)
//  {
//    Tracer ts("MiscMaths::diag"); 
      
//    int num = m.Nrows();
//    diagvals.ReSize(num);
//    for (int j=1; j<=num; j++)
//      diagvals(j)=m(j,j);
//    return 0;
//  }

// float var(const ColumnVector& x)
// {     
//   float m = mean(x);
//   float ssq = (x-m).SumSquare()/(x.Nrows()-1);
      
//   return ssq;
// }

// float mean(const ColumnVector& x)
// {    
//   float m = x.Sum()/x.Nrows();
//   return m;
// }

float median(const ColumnVector& x)
{     
  ColumnVector y = x;
  SortAscending(y);
  float m = y((int)(y.Nrows()/2));
      
  return m;
}




void cart2sph(const ColumnVector& dir, float& th, float& ph)
{
  float mag=sqrt(dir(1)*dir(1)+dir(2)*dir(2)+dir(3)*dir(3));
  if(mag==0){
    ph=M_PI/2;
    th=M_PI/2;
  }
  else{

    if(dir(1)==0 && dir(2)>=0) ph=M_PI/2;
    else if(dir(1)==0 && dir(2)<0) ph=-M_PI/2;
    else if(dir(1)>0) ph=atan(dir(2)/dir(1));
    else if(dir(2)>0) ph=atan(dir(2)/dir(1))+M_PI;
    else ph=atan(dir(2)/dir(1))-M_PI;
    
    if(dir(3)==0) th=M_PI/2;
    else if(dir(3)>0) th=atan(sqrt(dir(1)*dir(1)+dir(2)*dir(2))/dir(3));
    else th=atan(sqrt(dir(1)*dir(1)+dir(2)*dir(2))/dir(3))+M_PI;
  }
}



void cart2sph(const Matrix& dir,ColumnVector& th,ColumnVector& ph)
{
  if(th.Nrows()!=dir.Ncols()){
    th.ReSize(dir.Ncols());
  }

  if(ph.Nrows()!=dir.Ncols()){
    ph.ReSize(dir.Ncols());
  }

  for (int i=1;i<=dir.Ncols();i++) {
    float mag=sqrt(dir(1,i)*dir(1,i)+dir(2,i)*dir(2,i)+dir(3,i)*dir(3,i));
    if(mag==0){
      ph(i)=M_PI/2;
      th(i)=M_PI/2;
    }
    else{
      if(dir(1,i)==0 && dir(2,i)>=0) ph(i)=M_PI/2;
      else if(dir(1,i)==0 && dir(2,i)<0) ph(i)=-M_PI/2;
      else if(dir(1,i)>0) ph(i)=atan(dir(2,i)/dir(1,i));
      else if(dir(2,i)>0) ph(i)=atan(dir(2,i)/dir(1,i))+M_PI;
      else ph(i)=atan(dir(2,i)/dir(1,i))-M_PI;

      if(dir(3,i)==0) th(i)=M_PI/2;
      else if(dir(3,i)>0) th(i)=atan(sqrt(dir(1,i)*dir(1,i)+dir(2,i)*dir(2,i))/dir(3,i));
      else th(i)=atan(sqrt(dir(1,i)*dir(1,i)+dir(2,i)*dir(2,i))/dir(3,i))+M_PI;

    }
  }
}


// Added by CFB   --- Matlab style Matrix functions
 
ReturnMatrix ones(const int dim1, const int dim2)
{ 
  int tdim = dim2;
  if(tdim<0){tdim=dim1;}
  Matrix res(dim1,tdim); res = 1.0;
  res.Release();
  return res;
}

ReturnMatrix zeros(const int dim1, const int dim2)
{ 
  int tdim = dim2;
  if(tdim<0){tdim=dim1;}
  Matrix res(dim1,tdim); res = 0.0;
  res.Release();
  return res;
}

ReturnMatrix repmat(const Matrix &mat, const int rows, const int cols)
{
  Matrix res = mat;
  for(int ctr = 1; ctr < cols; ctr++){res |= mat;}
  Matrix tmpres = res;
  for(int ctr = 1; ctr < rows; ctr++){res &= tmpres;}
  res.Release();
  return res;
}

ReturnMatrix dist2(const Matrix &mat1, const Matrix &mat2)
{
  Matrix res(mat1.Ncols(),mat2.Ncols());
  for(int ctr1 = 1; ctr1 <= mat1.Ncols(); ctr1++)
    for(int ctr2 =1; ctr2 <= mat2.Ncols(); ctr2++)
      {
	ColumnVector tmp;
	tmp=mat1.Column(ctr1)-mat2.Column(ctr2);
	res(ctr1,ctr2) = std::sqrt(tmp.SumSquare());
      }
  res.Release();
  return res;
}

ReturnMatrix abs(const Matrix& mat)
{
  Matrix res = mat;
  for (int mc=1; mc<=mat.Ncols(); mc++) {
    for (int mr=1; mr<=mat.Nrows(); mr++) {
      res(mr,mc)=std::abs(res(mr,mc));
    }
  }
  res.Release();
  return res;
}

ReturnMatrix sqrt(const Matrix& mat)
{
  Matrix res = mat;
  bool neg_flag = false;
  for (int mc=1; mc<=mat.Ncols(); mc++) {
    for (int mr=1; mr<=mat.Nrows(); mr++) {
      if(res(mr,mc)<0){ neg_flag = true; }
      res(mr,mc)=std::sqrt(std::abs(res(mr,mc)));
    }
  }
  if(neg_flag){
    //cerr << " Matrix contained negative elements" << endl;
    //cerr << " return sqrt(abs(X)) instead" << endl;
  }
  res.Release();
  return res;
}

ReturnMatrix log(const Matrix& mat)
{
  Matrix res = mat;
  bool neg_flag = false;
  for (int mc=1; mc<=mat.Ncols(); mc++) {
    for (int mr=1; mr<=mat.Nrows(); mr++) {
      if(res(mr,mc)<0){ neg_flag = true; }
      res(mr,mc)=std::log(std::abs(res(mr,mc)));
    }
  }
  if(neg_flag){
    //  cerr << " Matrix contained negative elements" << endl;
    //  cerr << " return log(abs(X)) instead" << endl;
  }
  res.Release();
  return res; 
}

ReturnMatrix exp(const Matrix& mat)
{
  Matrix res = mat;
  for (int mc=1; mc<=mat.Ncols(); mc++) {
    for (int mr=1; mr<=mat.Nrows(); mr++) {
      res(mr,mc)=std::exp(res(mr,mc));
    }
  }
  res.Release();
  return res;
}

ReturnMatrix tanh(const Matrix& mat)
{
  Matrix res = mat;
  for (int mc=1; mc<=mat.Ncols(); mc++) {
    for (int mr=1; mr<=mat.Nrows(); mr++) {
      res(mr,mc)=std::tanh(res(mr,mc));
    }
  }
  res.Release();
  return res;
}

ReturnMatrix pow(const Matrix& mat, const double exp)
{
  Matrix res = mat;
  for (int mc=1; mc<=mat.Ncols(); mc++) {
    for (int mr=1; mr<=mat.Nrows(); mr++) {
      res(mr,mc)=std::pow(res(mr,mc),exp);
    }
  }
  res.Release();
  return res;
}

ReturnMatrix max(const Matrix& mat)
{
  Matrix res;
  if(mat.Nrows()>1){
    res=zeros(1,mat.Ncols());
    res=mat.Row(1);
    for(int mc=1; mc<=mat.Ncols();mc++){
      for(int mr=2; mr<=mat.Nrows();mr++){
	if(mat(mr,mc)>res(1,mc)){res(1,mc)=mat(mr,mc);}
      }
    }
  }
  else{
    res=zeros(1);
    res=mat(1,1);
    for(int mc=2; mc<=mat.Ncols(); mc++){
      if(mat(1,mc)>res(1,1)){res(1,1)=mat(1,mc);}
    }
  }
  res.Release();
  return res;
}

ReturnMatrix max(const Matrix& mat,ColumnVector& index)
{
  index.ReSize(mat.Nrows());
  index=1;
  Matrix res;
  if(mat.Nrows()>1){
    res=zeros(1,mat.Ncols());
    res=mat.Row(1);
    for(int mc=1; mc<=mat.Ncols();mc++){
      for(int mr=2; mr<=mat.Nrows();mr++){
	if(mat(mr,mc)>res(1,mc))
	  {
	    res(1,mc)=mat(mr,mc);
	    index(mr)=mc;
	  }
      }
    }
  }
  else{
    res=zeros(1);
    res=mat(1,1);
    for(int mc=2; mc<=mat.Ncols(); mc++){
      if(mat(1,mc)>res(1,1))
	{
	  res(1,1)=mat(1,mc);
	  index(1)=mc;
	}
    }
  }
  res.Release();
  return res;
}

ReturnMatrix min(const Matrix& mat)
{
  Matrix res;
  if(mat.Nrows()>1){
    res=zeros(1,mat.Ncols());
    res=mat.Row(1);
    for(int mc=1; mc<=mat.Ncols();mc++){
      for(int mr=2; mr<=mat.Nrows();mr++){
	if(mat(mr,mc)<res(1,mc)){res(1,mc)=mat(mr,mc);}
      }
    }
  }
  else{
    res=zeros(1);
    res=mat(1,1);
    for(int mc=2; mc<=mat.Ncols(); mc++){
      if(mat(1,mc)<res(1,1)){res(1,1)=mat(1,mc);}
    }
  }
  res.Release();
  return res;
}
  

ReturnMatrix sum(const Matrix& mat, const int dim)
{
  Matrix tmp;

  if (dim == 1) {tmp=mat;}
  else {tmp=mat.t();}
  Matrix res(1,tmp.Ncols());
  res = 0.0;  
  for (int mc=1; mc<=tmp.Ncols(); mc++) {
    for (int mr=1; mr<=tmp.Nrows(); mr++) {
      res(1,mc) += tmp(mr,mc);
    }
  }
  if (!(dim == 1)) {res=res.t();}
  res.Release();
  return res;
}

ReturnMatrix mean(const Matrix& mat, const int dim)
{
  Matrix tmp;
  if (dim == 1) {tmp=mat;}
  else {tmp=mat.t();}

  int N = tmp.Nrows();

  Matrix res(1,tmp.Ncols());
  res = 0.0;  
  for (int mc=1; mc<=tmp.Ncols(); mc++) {
    for (int mr=1; mr<=tmp.Nrows(); mr++) {
      res(1,mc) += tmp(mr,mc)/N;
    }
  }
  if (!(dim == 1)) {res=res.t();}

  res.Release();
  return res;
}

ReturnMatrix var(const Matrix& mat, const int dim)
{
  Matrix tmp;
  if (dim == 1) {tmp=mat;}
  else {tmp=mat.t();}
  int N = tmp.Nrows();
  Matrix res(1,tmp.Ncols());
  res = 0.0;

  if(N>1){    
    tmp -= ones(tmp.Nrows(),1)*mean(tmp,1);   
    for (int mc=1; mc<=tmp.Ncols(); mc++) 
      for (int mr=1; mr<=tmp.Nrows(); mr++) 
        res(1,mc) += tmp(mr,mc) / (N-1) * tmp(mr,mc);
  }

  if (!(dim == 1)) {res=res.t();}
  res.Release();
  return res;
}

ReturnMatrix stdev(const Matrix& mat, const int dim)
{
  return sqrt(var(mat,dim));
}

ReturnMatrix gt(const Matrix& mat1,const Matrix& mat2)
{
  int ctrcol = std::min(mat1.Ncols(),mat2.Ncols());
  int ctrrow = std::min(mat1.Nrows(),mat2.Nrows());
  Matrix res(ctrrow,ctrcol);
  res=0.0;

  for (int ctr1 = 1; ctr1 <= ctrrow; ctr1++) {
    for (int ctr2 =1; ctr2 <= ctrcol; ctr2++) {
      if( mat1(ctr1,ctr2) > mat2(ctr1,ctr2)){
	res(ctr1,ctr2) = 1.0;
      }
    }
  }

  res.Release();
  return res;
}


ReturnMatrix lt(const Matrix& mat1,const Matrix& mat2)
{
  int ctrcol = std::min(mat1.Ncols(),mat2.Ncols());
  int ctrrow = std::min(mat1.Nrows(),mat2.Nrows());
  Matrix res(ctrrow,ctrcol);
  res=0.0;

  for (int ctr1 = 1; ctr1 <= ctrrow; ctr1++) {
    for (int ctr2 =1; ctr2 <= ctrcol; ctr2++) {
      if( mat1(ctr1,ctr2) < mat2(ctr1,ctr2)){
	res(ctr1,ctr2) = 1.0;
      }
    }
  }

  res.Release();
  return res;
}


ReturnMatrix geqt(const Matrix& mat1,const Matrix& mat2) 
{
  int ctrcol = std::min(mat1.Ncols(),mat2.Ncols());
  int ctrrow = std::min(mat1.Nrows(),mat2.Nrows());
  Matrix res(ctrrow,ctrcol);
  res=0.0;

  for (int ctr1 = 1; ctr1 <= ctrrow; ctr1++) {
    for (int ctr2 =1; ctr2 <= ctrcol; ctr2++) {
      if( mat1(ctr1,ctr2) >= mat2(ctr1,ctr2)){
	res(ctr1,ctr2) = 1.0;
      }
    }
  }

  res.Release();
  return res;
}
ReturnMatrix geqt(const Matrix& mat,const float a) 
{
  int ncols = mat.Ncols();
  int nrows = mat.Nrows();
  Matrix res(nrows,ncols);
  res=0.0;

  for (int ctr1 = 1; ctr1 <= nrows; ctr1++) {
    for (int ctr2 =1; ctr2 <= ncols; ctr2++) {
      if( mat(ctr1,ctr2) >= a){
	res(ctr1,ctr2) = 1.0;
      }
    }
  }

  res.Release();
  return res;
}


ReturnMatrix leqt(const Matrix& mat1,const Matrix& mat2) 
{
  int ctrcol = std::min(mat1.Ncols(),mat2.Ncols());
  int ctrrow = std::min(mat1.Nrows(),mat2.Nrows());
  Matrix res(ctrrow,ctrcol);
  res=0.0;

  for (int ctr1 = 1; ctr1 <= ctrrow; ctr1++) {
    for (int ctr2 =1; ctr2 <= ctrcol; ctr2++) {
      if( mat1(ctr1,ctr2) <= mat2(ctr1,ctr2)){
	res(ctr1,ctr2) = 1.0;
      }
    }
  }

  res.Release();
  return res;
}


ReturnMatrix eq(const Matrix& mat1,const Matrix& mat2)
{
  int ctrcol = std::min(mat1.Ncols(),mat2.Ncols());
  int ctrrow = std::min(mat1.Nrows(),mat2.Nrows());
  Matrix res(ctrrow,ctrcol);
  res=0.0;

  for (int ctr1 = 1; ctr1 <= ctrrow; ctr1++) {
    for (int ctr2 =1; ctr2 <= ctrcol; ctr2++) {
      if( mat1(ctr1,ctr2) == mat2(ctr1,ctr2)){
	res(ctr1,ctr2) = 1.0;
      }
    }
  }

  res.Release();
  return res;
}


ReturnMatrix neq(const Matrix& mat1,const Matrix& mat2) 
{
  int ctrcol = std::min(mat1.Ncols(),mat2.Ncols());
  int ctrrow = std::min(mat1.Nrows(),mat2.Nrows());
  Matrix res(ctrrow,ctrcol);
  res=0.0;

  for (int ctr1 = 1; ctr1 <= ctrrow; ctr1++) {
    for (int ctr2 =1; ctr2 <= ctrcol; ctr2++) {
      if( mat1(ctr1,ctr2) != mat2(ctr1,ctr2)){
	res(ctr1,ctr2) = 1.0;
      }
    }
  }

  res.Release();
  return res;
}

ReturnMatrix SD(const Matrix& mat1,const Matrix& mat2) 
{
  if((mat1.Nrows() != mat2.Nrows()) ||
     (mat1.Ncols() != mat2.Ncols()) ){
    cerr <<"MISCMATHS::SD - matrices are of different dimensions"<<endl;
    exit(-1);
  }
  Matrix ret(mat1.Nrows(),mat1.Ncols());
  for (int r = 1; r <= mat1.Nrows(); r++) {
    for (int c =1; c <= mat1.Ncols(); c++) {
      if( mat2(r,c)==0)
	ret(r,c)=0;
      else
	ret(r,c) = mat1(r,c)/mat2(r,c);
    }
  }

  ret.Release();
  return ret;
}


ReturnMatrix vox_to_vox(const ColumnVector& xyz1,const ColumnVector& dims1,const ColumnVector& dims2,const Matrix& xfm){
  ColumnVector xyz1_mm(4),xyz2_mm,xyz2(3);
  xyz1_mm<<xyz1(1)*dims1(1)<<xyz1(2)*dims1(2)<<xyz1(3)*dims1(3)<<1;
  xyz2_mm=xfm*xyz1_mm;
  xyz2_mm=xyz2_mm/xyz2_mm(4);
  xyz2<<xyz2_mm(1)/dims2(1)<<xyz2_mm(2)/dims2(2)<<xyz2_mm(3)/dims2(3);
  xyz2.Release();
  return xyz2;
}


ReturnMatrix mni_to_imgvox(const ColumnVector& mni,const ColumnVector& mni_origin,const Matrix& mni2img, const ColumnVector& img_dims){
  ColumnVector mni_new_origin(4),img_mm;//homogeneous
  ColumnVector img_vox(3);
  mni_new_origin<<mni(1)+mni_origin(1)<<mni(2)+mni_origin(2)<<mni(3)+mni_origin(3)<<1;
  img_mm=mni2img*mni_new_origin;
  img_vox<<img_mm(1)/img_dims(1)<<img_mm(2)/img_dims(2)<<img_mm(3)/img_dims(3);
  img_vox.Release();
  return img_vox;
}




ReturnMatrix remmean(const Matrix& mat, const int dim)
{ 
  Matrix res;
  if (dim == 1) {res=mat;}
  else {res=mat.t();}

  Matrix Mean;
  Mean = mean(res);

  for (int ctr = 1; ctr <= res.Nrows(); ctr++) {
    for (int ctr2 =1; ctr2 <= res.Ncols(); ctr2++) {
      res(ctr,ctr2)-=Mean(1,ctr2);
    }
  }
  if (dim != 1) {res=res.t();}
  res.Release();
  return res;
}


void remmean(const Matrix& mat, Matrix& demeanedmat, Matrix& Mean,  const int dim)
{ 
  if (dim == 1) {demeanedmat=mat;}
  else {demeanedmat=mat.t();}

  Mean = mean(demeanedmat);

  for (int ctr = 1; ctr <= demeanedmat.Nrows(); ctr++) {
    for (int ctr2 =1; ctr2 <= demeanedmat.Ncols(); ctr2++) {
      demeanedmat(ctr,ctr2)-=Mean(1,ctr2);
    }
  }
  if (dim != 1){demeanedmat = demeanedmat.t();Mean = Mean.t();}
}

ReturnMatrix cov(const Matrix& mat, const int norm)
{ 
  SymmetricMatrix res;
  Matrix tmp;
  int N;
  tmp=remmean(mat);
  if (norm == 1) {N = mat.Nrows();}
  else {N = mat.Nrows()-1;}  
  res << tmp.t()*tmp;
  res = res/N;

  res.Release();
  return res; 
}

ReturnMatrix corrcoef(const Matrix& mat, const int norm)
{ 
  SymmetricMatrix res;
  SymmetricMatrix C;
  C = cov(mat,norm);
  Matrix D;
  D=diag(C);
  D=pow(sqrt(D*D.t()),-1);
  res << SP(C,D);
  res.Release();
  return res; 
}

void symm_orth(Matrix &Mat)
{
  SymmetricMatrix Metric;
  Metric << Mat.t()*Mat;
  Metric = Metric.i();
  Matrix tmpE;
  DiagonalMatrix tmpD;
  EigenValues(Metric,tmpD,tmpE);
  Mat = Mat * tmpE * sqrt(abs(tmpD)) * tmpE.t();
}

void powerspectrum(const Matrix &Mat1, Matrix &Result, bool useLog)
  //calculates the powerspectrum for every column of Mat1
{
  Matrix res;
  for(int ctr=1; ctr <= Mat1.Ncols(); ctr++)
    {
      ColumnVector tmpCol;
      tmpCol=Mat1.Column(ctr);
      ColumnVector FtmpCol_real;
      ColumnVector FtmpCol_imag;
      ColumnVector tmpPow;
      
      RealFFT(tmpCol,FtmpCol_real,FtmpCol_imag);
      tmpPow = pow(FtmpCol_real,2)+pow(FtmpCol_imag,2);
      tmpPow = tmpPow.Rows(2,tmpPow.Nrows());
      if(useLog){tmpPow = log(tmpPow);}
      if(res.Storage()==0){res= tmpPow;}else{res|=tmpPow;}
    }
    Result=res;
}


void element_mod_n(Matrix& Mat,double n)
{
 //represent each element in modulo n (useful for wrapping phases (n=2*M_PI))

  double tmp;
  for( int j=1;j<=Mat.Ncols();j++){
    for( int i=1;i<=Mat.Nrows();i++){

      while( !( (Mat(i,j)>0) && (Mat(i,j)<n) ) ){ 
	tmp = ( Mat(i,j) - rounddouble(Mat(i,j)/n)*n );
	Mat(i,j)= tmp > 0 ? tmp : tmp + n;
      }    
      
    }
    
  }
  
}

int nextpow2(int n)
{
  return (int)pow(2,ceil(log(float(n))/log(float(2))));
}

void xcorr(const Matrix& p_ts, Matrix& ret, int lag, int p_zeropad)
{
  Tracer tr("MISCMATHS::xcorr");

  int sizeTS = p_ts.Nrows();
  int numTS = p_ts.Ncols();
  
  if(p_zeropad == 0)
    p_zeropad = sizeTS;
  if(lag == 0)
    lag = sizeTS;

  ColumnVector x(p_zeropad);
  x = 0;
  ColumnVector fft_real;
  ColumnVector fft_im;
  ColumnVector dummy(p_zeropad);
  ColumnVector dummy2;
  dummy = 0;
  ColumnVector realifft(p_zeropad);
  ret.ReSize(lag,numTS);
  ret = 0;

  for(int i = 1; i <= numTS; i++)
    {
      x.Rows(1,sizeTS) = p_ts.Column(i);
      FFT(x, dummy, fft_real, fft_im);
      
      for(int j = 1; j <= p_zeropad; j++)
	{
	  // (x+iy)(x-iy) = x^2 + y^2
	  fft_real(j) = fft_real(j)*fft_real(j) + fft_im(j)*fft_im(j);
	  fft_im(j) = 0;
	}
      
      FFTI(fft_real, fft_im, realifft, dummy2);
      
      float varx = var(x.Rows(1,sizeTS)).AsScalar();
      ret.Column(i) = realifft.Rows(1,lag);
      
      for(int j = 1; j <= lag-1; j++)
	{
	  // Correction to make autocorr unbiased and normalised
	  ret(j,i) = ret(j,i)/((sizeTS-j)*varx);
	}  
    }
}

ReturnMatrix xcorr(const Matrix& p_ts, int lag, int p_zeropad )
{
  Matrix r;
  
  xcorr(p_ts,r,lag,p_zeropad);
  r.Release();
  return r;
}

void detrend(Matrix& p_ts, int p_level)
{
  Tracer trace("MISCMATHS::detrend");

  int sizeTS = p_ts.Nrows();     

  // p_ts = b*a + e (OLS regression)
  // e is detrended data
  Matrix a(sizeTS, p_level+1);  

  // Create a
  for(int t = 1; t <= sizeTS; t++)
    {
      for(int l = 0; l <= p_level; l++)
	a(t,l+1) = pow((float)t/sizeTS,l);
    }
          
  // Form residual forming matrix R:
  Matrix R = Identity(sizeTS)-a*pinv(a);

  for(int t = 1; t <= sizeTS; t++)
    {
      p_ts.Column(t) = R*p_ts.Column(t);
    }      
}



ReturnMatrix read_vest(string p_fname)
{
  ifstream in;
  in.open(p_fname.c_str(), ios::in);
  
  if(!in)
    {
      //cerr << "Unable to open " << p_fname << endl;
      throw Exception("Unable to open vest file");
    }
  
  int numWaves = 0;
  int numPoints = 0;
  
  string str;
  
  while(true)
    {
      if(!in.good())
	{
	  cerr << p_fname << "is not a valid vest file"  << endl;
	  throw Exception("Not a valid vest file");
	}
      in >> str;
      if(str == "/Matrix")
	break;
      else if(str == "/NumWaves")
	{
	  in >> numWaves;
	}
      else if(str == "/NumPoints" || str == "/NumContrasts")
	{
	  in >> numPoints;
	}
    }
  
  Matrix p_mat(numPoints, numWaves);
  
  for(int i = 1; i <= numPoints; i++)
    {
      for(int j = 1; j <= numWaves; j++)    
	{
	  in >> p_mat(i,j);
	}
    }
  
  in.close();

  p_mat.Release();
  return p_mat;
}

void ols(const Matrix& data,const Matrix& des,const Matrix& tc, Matrix& cope,Matrix& varcope){
  // ols
  // data is t x v
  // des is t x ev (design matrix)
  // tc is cons x ev (contrast matrix)
  // cope and varcope will be cons x v
  // but will be resized if they are wrong
  // hence may be passed in uninitialised
  // TB 2004
  if(data.Nrows() != des.Nrows()){
    cerr <<"MISCMATHS::ols - data and design have different number of time points"<<endl;
    exit(-1);
  }
  if(des.Ncols() != tc.Ncols()){
    cerr <<"MISCMATHS::ols - design and contrast matrix have different number of EVs"<<endl;
    exit(-1);
  }  
  
  Matrix pdes = pinv(des);
  Matrix prevar=diag(tc*pdes*pdes.t()*tc.t());
  Matrix R=Identity(des.Nrows())-des*pdes;
  float tR=R.Trace();
  Matrix pe=pdes*data;
  cope=tc*pe;
  Matrix res=data-des*pe;
  Matrix sigsq=sum(SP(res,res))/tR;
  varcope=prevar*sigsq;
  
  
}

int ols_dof(const Matrix& des){
  Matrix pdes = pinv(des);
  Matrix R=Identity(des.Nrows())-des*pdes;
  return int(R.Trace());
}

int conjgrad(ColumnVector& x, const Matrix& A, const ColumnVector& b, int maxit,
	     float reltol)
{
  // solves:  A * x = b    (for x)
  // implementation of algorithm in Golub and Van Loan (3rd ed, page 527)
  ColumnVector rk1, rk2, pk, apk;
  double betak, alphak, rk1rk1=0, rk2rk2, r00=0;
  int k=0;
  rk1 = b - A*x;  // a *big* calculation
  for (int n=1; n<=maxit; n++) {
    k++;
    if (k==1) {
      pk = rk1;
      rk1rk1 = (rk1.t() * rk1).AsScalar();
      r00=rk1rk1;
    } else {
      rk2rk2 = rk1rk1;  // from before
      rk1rk1 = (rk1.t() * rk1).AsScalar();
      if (rk2rk2<1e-10*rk1rk1) {
	cerr << "WARNING:: Conj Grad - low demoninator (rk2rk2)" << endl; 
	if (rk2rk2<=0) {
	  cerr << "Aborting conj grad ..." << endl;
	  return 1;  
	}
      }
      betak = rk1rk1 / rk2rk2;
      pk = rk1 + betak * pk;  // note RHS pk is p(k-1) in algorithm
    }  
    // stop if sufficient accuracy is achieved
    if (rk1rk1<reltol*reltol*r00) return 0;

    apk = A * pk;  // the *big* calculation in this algorithm

    ColumnVector pap = pk.t() * apk;
    if (pap.AsScalar()<0) { 
      cerr << "ERROR:: Conj Grad - negative eigenvector found (matrix must be symmetric and positive-definite)\nAborting ... " << endl; 
      return 2;
    } else if (pap.AsScalar()<1e-10) { 
      cerr << "WARNING:: Conj Grad - nearly null eigenvector found (terminating early)" << endl; 
      return 1;
    } else {
      alphak = rk1rk1 / pap.AsScalar();
    }
    x = x + alphak * pk;  // note LHS is x(k) and RHS is x(k-1) in algorithm
    rk2 = rk1;  // update prior to the next step
    rk1 = rk1 - alphak * apk;  // note LHS is r(k) in algorithm
  }
  return 0;
}

int conjgrad(ColumnVector& x, const Matrix& A, const ColumnVector& b, int maxit)
{ 
  return conjgrad(x,A,b,maxit,1e-10);
}

float csevl(const float x, const ColumnVector& cs, const int n)
  {
 
    float b0 = 0;
    float b1 = 0;
    float b2 = 0;
    const float twox=2*x;
    
    for(int i=1; i<=n; i++)
      {
	b2=b1;
	b1=b0;
	b0=twox*b1-b2+cs(n+1-i);
      }
    
    return 0.5*(b0-b2);
  }

  float digamma(const float x)
  { 
    int ntapsi(16);
    int ntpsi(23);
    ColumnVector psics(ntpsi);
    ColumnVector apsics(ntapsi);

    psics << -.038057080835217922E0<<