/* -*- mode: c++; c-basic-offset: 3 -*-
 *
 * PANDORE (PANTHEON Project)
 *
 * GREYC IMAGE
 * 6 Boulevard Maréchal Juin
 * F-14050 Caen Cedex France
 *
 * This file is free software. You can use it, distribute it
 * and/or modify it. However, the entire risk to the quality
 * and performance of this program is with you.
 *
 * Please email any bugs, comments, and/or additions to this file to:
 * pandore@greyc.ensicaen.fr
 * http://www.greyc.ensicaen.fr/EquipeImage/Pandore/
 */

/**
 * @author Jalal Fadili
 * Modified by Jean-Baptiste Hoock - 2005-07-28 
 */

/**
 * @file dctutils.cpp
 */

#ifndef DCTUTILS_CC
#define DCTUTILS_CC

#include "utils.cpp"
#define SWAP(a,b) tempr=(a);(a)=(b);(b)=tempr

template<typename U>
Errc CheckWindow(Img2d<U> &im,Long ww) {
   if(ww > MAX(im.Width(),im.Height())){
      std::cerr<<"Incorrect window width format: must be <= max(width,height)"<<std::endl;
      Exit(FAILURE);
   }
   if( 1<<nextpow2(ww) != (int)ww || ww == 1){
      std::cerr<<"Incorrect window width format: must be a power of 2."<<std::endl;
      Exit(FAILURE);
   }
   return SUCCESS;
}

/**
 * four1 : This function implements the Fast Fourier Transform
 * and the Inverse Fast Fourier Transform in one dimension.
 * Inputs :
 *         data  : The inputs data. The pointer must be decreased before.
 *	   n     : The number of complexes
 *	   isign :  -1 => Inverse Fast Fourier Transform.
 *		     1 => Fast Fourier Transform.
 * Description :  The implementation is an iterative version. 
 * This function is often used, so we use some static variables.
 *
 */
void four1(Double *data,Ulong nn,Long isign) {
   static Ulong n,mmax,m,j,istep,i;
   static Double wtemp,wr,wpr,wpi,wi,theta;
   static Double tempr,tempi;

   // We reoarganize the inuput data.
   n=nn << 1;
   j=1;
   for (i=1;i<n;i+=2) {
      if (j > i) {
	 SWAP(data[j],data[i]);
	 SWAP(data[j+1],data[i+1]);
      }
      m=n >> 1;
      while (m >= 2 && j > m) {
	 j -= m;
	 m >>= 1;
      }
      j += m;
   }
	
   mmax=2;
   while (n > mmax) {
      istep=mmax << 1;
      theta=isign*(2*PI/mmax);
      wtemp=sin(0.5*theta);
      wpr = -2.0*wtemp*wtemp;
      wpi=sin(theta);
      wr=1.0;
      wi=0.0;
      for (m=1;m<mmax;m+=2) {
	 for (i=m;i<=n;i+=istep) {
	    j=i+mmax;
	    tempr=wr*data[j]-wi*data[j+1];
	    tempi=wr*data[j+1]+wi*data[j];
	    data[j]=data[i]-tempr;
	    data[j+1]=data[i+1]-tempi;
	    data[i] += tempr;
	    data[i+1] += tempi;
	 }
	 wr=(wtemp=wr)*wpr-wi*wpi+wr;
	 wi=wi*wpr+wtemp*wpi+wi;
      }
      mmax=istep;
   }
}

/**
 * realft : This function implements the transform of a real valued function
 * in one dimension.
 *	    
 * Inputs : 
 *	data  : The inputs data. The pointer must be decreased before.
 *	n     : The number of real values.
 *      isign : 1  => The standard transform.
 *		-1 => The inverse transform.
 * Description : This function allows us to save space and time.
 * This function is often used, so we use some static variables.
 */
void realft(Double data[], Ulong n, Long isign) {
   static Ulong i,i1,i2,i3,i4,np3;
   static Double c1,c2,h1r,h1i,h2r,h2i;
   static Double wr,wi,wpr,wpi,wtemp,theta;

   c1 = 0.5;
   theta = M_PI/(Double) (n>>1);
   if (isign == 1) {
      c2 = -0.5;
      four1(data,n>>1,1);
   } else {
      c2 = 0.5;
      theta = -theta;
   }
	
   wtemp = sin(0.5*theta);
   wpr   = -2.0*wtemp*wtemp;
   wpi   = sin(theta);
   wr    = 1.0+wpr;
   wi    = wpi;
   np3   = n+3;
   for (i=2;i<=(n>>2);i++) {
      i4=1+(i3=np3-(i2=1+(i1=i+i-1)));
      h1r=c1*(data[i1]+data[i3]);
      h1i=c1*(data[i2]-data[i4]);
      h2r = -c2*(data[i2]+data[i4]);
      h2i=c2*(data[i1]-data[i3]);
      data[i1]=h1r+wr*h2r-wi*h2i;
      data[i2]=h1i+wr*h2i+wi*h2r;
      data[i3]=h1r-wr*h2r+wi*h2i;
      data[i4] = -h1i+wr*h2i+wi*h2r;
      wr=(wtemp=wr)*wpr-wi*wpi+wr;
      wi=wi*wpr+wtemp*wpi+wi;
   }
   if (isign == 1) {
      data[1] = (h1r=data[1])+data[2];
      data[2] = h1r-data[2];
   } else {
      data[1]=c1*((h1r=data[1])+data[2]);
      data[2]=c1*(h1r-data[2]);
      four1(data,n>>1,-1);
   }
}
#undef SWAP

/**
 * dct_1D : This function implements the Discret Cosine Transform
 * and the Inverse Discret Cosine Transform in one dimension.
 * Inputs :
 *	y     : The input data; The pointer must be decreased before.
 *      n     : The number of real values.
 *      isign :  1 => The standard transform.
 *      isign : -1 => The inverse transform.
 * Description : This transform uses the realft function.
 */
void dct_i1D(Double y[], Ulong n, Long isign) {
   Ulong i;
   Double sum,sum1,y1,y2,ytemp;
   Double theta,wi=0.0,wi1,wpi,wpr,wr=1.0,wr1,wtemp;

   theta = 0.5*PI/n;
   wr1   = cos(theta);
   wi1   = sin(theta);
   wpr   = -2.0*wi1*wi1;
   wpi   = sin(2.0*theta);
	
   if (isign == 1)
      {
	 for (i=1;i<=n/2;i++)
	    {
	       y1=0.5*(y[i]+y[n-i+1]);
	       y2=wi1*(y[i]-y[n-i+1]);
	       y[i]=y1+y2;
	       y[n-i+1]=y1-y2;
	       wr1=(wtemp=wr1)*wpr-wi1*wpi+wr1;
	       wi1=wi1*wpr+wtemp*wpi+wi1;
	    }
	 realft(y,n,1);
	 for (i=3;i<=n;i+=2)
	    {
	       wr=(wtemp=wr)*wpr-wi*wpi+wr;
	       wi=wi*wpr+wtemp*wpi+wi;
	       y1=y[i]*wr-y[i+1]*wi;
	       y2=y[i+1]*wr+y[i]*wi;
	       y[i]=y1;
	       y[i+1]=y2;
	    }
	 sum=0.5*y[2];
	 for (i=n;i>=2;i-=2)
	    {
	       sum1=sum;
	       sum += y[i];
	       y[i]=sum1;
	    }
      }
   else if (isign == -1) 
      {
	 ytemp=y[n];
	 for (i=n;i>=4;i-=2) 
	    y[i]=y[i-2]-y[i];
	 y[2]=2.0*ytemp;
	 for (i=3;i<=n;i+=2)
	    {
	       wr=(wtemp=wr)*wpr-wi*wpi+wr;
	       wi=wi*wpr+wtemp*wpi+wi;
	       y1=y[i]*wr+y[i+1]*wi;
	       y2=y[i+1]*wr-y[i]*wi;
	       y[i]=y1;
	       y[i+1]=y2;
	    }
	 realft(y,n,-1);
	 for (i=1;i<=n/2;i++)
	    {
	       y1=y[i]+y[n-i+1];
	       y2=(0.5/wi1)*(y[i]-y[n-i+1]);
	       y[i]=0.5*(y1+y2);
	       y[n-i+1]=0.5*(y1-y2);
	       wr1=(wtemp=wr1)*wpr-wi1*wpi+wr1;
	       wi1=wi1*wpr+wtemp*wpi+wi1;
	    }
      }
}

/**
 * dct_iv1D : This function implements the Discrete Cosine Transform IV
 * The elements of the basis are normalized to a unit L2 norm.
 * Inputs:
 *  y : The input data.
 *  n : The number of real values. 
 **/
void dct_iv1D(Double y[], Ulong n) {
   Ulong i,isign;
   Ulong n8;
   Double scal;
   Double *tmp;
	
   n8  =  n<<3 ;
   tmp = new Double[n8];

   for( i = 0; i < n8; i++)
      tmp[i] = 0.0;
		
   for( i = 0; i < n; i++)
      tmp[2*i+1] = y[i];
	
   isign = 1;   /* IMPORTANT */
   realft(tmp-1,n8,isign); /* Decrease the pointer for compatibility with NRC.*/
	
   scal = sqrt(2/((Double) n));
   for( i = 0; i < n; i++)
      y[i] = scal * tmp[2*(2*i+1)];
		
   delete []tmp;
}


void copydouble(Double* x,Double* y,Ulong n) {
   while (n--) *y++ = *x++;
}

void copyreverse(Double* x,Double* y,Ulong n) {
   Double* yend;
   yend = y+n;
   while (n--) *--yend = *x++;
}

void changesign(Double* x,Ulong n) {
   for ( ; n; --n, x++)
      *x = -*x;
}

void fold(Double* xc,Double* xl,Double* xr,Double* bp,Double* bm,Ulong m,Ulong n,Double* y) {
   Ulong i;
   copydouble(xc,y,n);
   for ( i=0; i<m; i++){
      y[i]     = bp[i]*y[i]      + bm[i]*xl[n-1-i];
      y[n-1-i] = bp[i]*y[n-1-i]  - bm[i]*xr[i];
   }
}

void makebell(Ulong m,Double* bp,Double* bm) {
   Ulong i;
   Double xi;
	
   for (i=0 ; i<m; i++)
      {
	 xi    = PI  * (1 + (.5 + i)/m)/4.0;
	 bp[i] = sin(xi); 
	 bm[i] = sqrt(1 - bp[i]*bp[i]);
      }
}
	
void edgefold(Ulong which,Double* x,Double* y,Ulong n,Ulong m,Double* bp,Double* bm) {
   Ulong i;
	
   if (which == LEFT) {
      for( i=0; i< m; i++)
	 y[n-1-i] = x[i] * (1.-bp[i])/bm[i];
   } else {
      for( i=0; i< m; i++)
	 y[i] = (-x[n-1-i]) * (1.-bp[i])/bm[i];
   }
}

void fillzeros(Double* x,Ulong n) {
   while(n--) *x++ = 0.0;
}

void copyadd(Double* x,Double* y,Ulong n) {
   while(n--) *y++ += *x++;
}

void edgeadd(Ulong which,Double* x,Double* y,Ulong n,Ulong m) {
   Ulong i;
	
   if (which == LEFT) {
      for( i = 0; i < m; i++)
	 y[n-1-i] += x[n-1-i];
   } else {
      for( i = 0; i < m; i++)
	 y[i] += x[i];
   }
}

void unfold(Double* S,Double* xc,Double* xl,Double* xr,Ulong n,Ulong m,Double* bp,Double* bm) {
   Ulong i;
   copydouble(S,xc,n);
   for ( i=0; i<m; i++) {
      xl[n-1-i] = S[i]*bm[i];
      xr[i]     = S[n-1-i]*((-1) * bm[i]);
      xc[i]     = bp[i]*S[i];
      xc[n-1-i] = bp[i]*S[n-1-i];
   }
}

void edgeunfold(Ulong which,Double* x,Ulong n,Ulong m,Double* bp,Double* bm) {
   Ulong i;
	
   if (which == LEFT) {
      for( i = 0; i < m; i++)
	 x[i] += x[i] * (1.-bp[i])/bp[i];
   } else {
      for( i = 0; i < m; i++)
	 x[n-1-i] += x[n-1-i] * (1.-bp[i])/bp[i];
   }
}

// The old formula had some problems with O2 optimization
#ifdef _WIN32
#define LOG2(x) log((double)(x))/log(2.0)
#else 
#define LOG2(x) log2((double)(x))
#endif

/** This function computes ILDCTIV in one dimension.
 *  The elements of the basis are normalized to a unit L2 norm.
 * Inputs:
 *  cp : The input data.
 *  ww : The length of the window.
 *  n  : The number of real values. 
 */
void ildct_iv1D(Double* cp, Ulong ww, Ulong n) {
   Ulong d,b,ixpkt,nbox,m,blen,twodee;
   Double *xc, *xl, *xr;
   Ulong i;
   Double *temp = new Double[8*n];
   Double *bm, *bp, *x, *y, *scratch;
	
   bm      = temp;
   bp      = temp+n;
   y       = temp+2*n;
   xl      = temp+3*n;
   xr      = temp+4*n;
   x       = temp+5*n;
   scratch	= temp+6*n;

   xc = scratch;
		
   d = (Ulong)floor(LOG2((Double)n/(Double)ww));
   twodee = 1 << d;
   m = n/ twodee /2;
   makebell(m,bp,bm);
	
   nbox  = twodee;
   blen  = n / nbox;
		
   fillzeros(x,n);
	
   for( b=0; b < nbox; b++){
	
      ixpkt = blen * b;
      for( i=0; i<blen; i++ ) 
	 y[i] = cp[i+ixpkt];
		
      dct_iv1D(y,blen);
      unfold(y,xc,xl,xr,blen,m,bp,bm);
      copyadd(xc,&x[ixpkt],blen);
		
      if(b == 0)
	 edgeunfold(LEFT,&x[ixpkt],blen,m,bp,bm);
      else 
	 edgeadd(LEFT,xl,&x[ixpkt-blen],blen,m);
			
      if( b+1 < nbox )
	 edgeadd(RIGHT,xr,&x[ixpkt+blen],blen,m);
      else 
	 edgeunfold(RIGHT,&x[ixpkt],blen,m,bp,bm);
   }
   for( i = 0; i<n; i++ ) 
      cp[i] = x[i];
	
   delete []temp;
}

/**
 * This function computes LDCTIV in one dimension.
 * The elements of the basis are normalized to a unit L2 norm.
 * Inputs:
 *  x : The input data.
 *  ww : The length of the window.
 *  n  : The number of real values. 
 **/
void ldct_iv1D(Double *x,Ulong ww,Ulong n) {
   Ulong ixpkt,twodee,m,i;
   Ulong nbox, blen, d, b;
   Double *pxc=0, *pxl=0, *pxr=0;
   Double *temp = new Double[5*n];
   Double *bm, *bp, *xt, *y, *cp;

   bm = temp;
   bp = temp+n;
   xt = temp+2*n;
   y  = temp+3*n;
   cp = new Double[n];
		
   d = (Ulong)floor(LOG2((Double)n/(Double)ww));
   twodee = 1 << d;
   m = n/ twodee /2;
	
   makebell(m,bp,bm);
	
   nbox  = twodee;
   blen  = n / nbox;


   for( b = 0; b < nbox; b++){
      ixpkt = blen * b;
      if (b == 0) {
	 pxc = x;
	 edgefold(LEFT,x,xt,blen,m,bp,bm);
	 pxl = xt;
      } else {
	 pxl = pxc;
	 pxc = pxr;
      }
      if( b+1 < nbox )
	 pxr = &x[ixpkt+blen];
      else {
	 edgefold(RIGHT,pxc,xt,blen,m,bp,bm);
	 pxr = xt;
      }
      fold(pxc,pxl,pxr,bp,bm,m,blen,y);
      dct_iv1D(y,blen);
      for( i = 0; i < blen; i++ ) 
	 cp[i+ixpkt] = y[i];
   }
   
   for( i = 0; i < n; i++ ) 
      x[i] = cp[i];
   
   delete []temp;
   delete []cp;
}
#undef LOG2

/**
 * This function computes DST and  DST_I 1D (auto-invertible, i.e. DST_I(DST_I(y))=y).
 * (C) Copr. 1986-92 Numerical Recipes Software 'j9#.
 * The elements of the basis are not normalized to a unit L2 norm (orthogonal only).
 * Must multiply by sqrt(2/n).
 * Inputs :
 *  y : The input data.
 *  n : The number or real values.
 *	   isign :  -1 => Inverse Transform.
 *		     1 => Transform. 
 */
void dst_i1D(Double y[], Ulong n,Long isign) {
   Ulong j,n2=n+2;
   Double sum,y1,y2;
   Double theta,wi=0.0,wr=1.0,wpi,wpr,wtemp;

   theta=PI/(Double) n;
   wtemp=sin(0.5*theta);
   wpr = -2.0*wtemp*wtemp;
   wpi=sin(theta);
   y[1]=0.0;
   for (j=2;j<=(n>>1)+1;j++) {
      wr = (wtemp=wr)*wpr-wi*wpi+wr;
      wi = wi*wpr+wtemp*wpi+wi;
      y1 = wi*(y[j]+y[n2-j]);
      y2 = 0.5*(y[j]-y[n2-j]);
      y[j] = y1+y2;
      y[n2-j] = y1-y2;
   }
   realft(y,n,1);
   y[1]*=0.5;
   sum=y[2]=0.0;
   for (j=1;j<=n-1;j+=2) {
      sum += y[j];
      y[j] = y[j+1];
      y[j+1] = sum;
   }
}

/**
 * This function computes DST_II and IDST_II.
 * The elements of the basis are not normalized to a unit L2 norm (orthogonal only).
 * Must multiply by sqrt(2/n).
 * Inputs :
 *  y : The input data.
 *  n : The number or real values.
 *	   isign :  -1 => Inverse Transform.
 *		     1 => Transform. 
 */
void dst_ii1D(Double y[], Ulong n, Long isign) {
   Ulong     n2, i;
   Double   *xp1, *xp2;
   Double   temp;
	
   /*  Compute:  n   = length of transform n2  = n/2    */
   n2 = n >> 1;
	
   if(isign==1) {
      /* Multiply data by sequence of alternating +1's and -1's   */
      xp1 = y + 2;
      for ( i = 1; i <= n2; i++ ) {
	 *xp1 = - *xp1;
	 xp1 += 2;
      }

      /*  Get DCT   */
      dct_i1D(y,n,isign);

      /*  Reverse order of transformed coefficients    */

      xp1 = y + 1;
      xp2 = y + n;
      for ( i = 1; i <= n2; i++ ) {
	 temp = *xp1;
	 *xp1 = *xp2;
	 *xp2 = temp;
	 xp1++;
	 xp2--;
      }
   } else {
      /* Reverse order of transformed coefficients  */
      xp1 = y + 1;
      xp2 = y + n;
      for ( i = 1; i <= n2; i++ ) {
	 temp = *xp1;
	 *xp1 = *xp2;
	 *xp2 = temp;
	 xp1++;
	 xp2--;
      }
      
      /*  Get IDCT   */
      dct_i1D(y,n,isign);
      
      /*  Multiply data by a sequence of alternating +1's and -1's  */
      xp1 = y + 2;
      for( i = 1; i <= n2; i++) {
	 *xp1 = - *xp1;
	 xp1 += 2;
      }
   }
}
#endif
