tomwalters@0: #include <stdio.h>
tomwalters@0: #include <math.h>
tomwalters@0: #include "sigproc.h"
tomwalters@0: 
tomwalters@0: 
tomwalters@0: /*
tomwalters@0:   C is n complex numbers: (real,imag), (ie 2n floats).
tomwalters@0:   Overwrite the first n floats with the modulus of the n complex numbers.
tomwalters@0: */
tomwalters@0: 
tomwalters@0: Mod( C, n )
tomwalters@0: complex *C ;
tomwalters@0: int      n ;
tomwalters@0: {
tomwalters@0:     register complex *eptr = C+n ;
tomwalters@0:     register float   *V    = (float *)C ;
tomwalters@0: 
tomwalters@0:     for (  ; C < eptr ; C++ )
tomwalters@0: 	*V++ = mod(C) ;
tomwalters@0: }
tomwalters@0: 
tomwalters@0: /*
tomwalters@0:   Return the modulus of the given complex number
tomwalters@0: */
tomwalters@0: 
tomwalters@0: float mod( C )
tomwalters@0: complex *C ;
tomwalters@0: {
tomwalters@0:     return ( sqrt( sq(Re(C)) + sq(Im(C)) ) ) ;
tomwalters@0: }
tomwalters@0: 
tomwalters@0: 
tomwalters@0: 
tomwalters@0: /*
tomwalters@0:   C is n complex numbers: (real,imag), (ie 2n floats).
tomwalters@0:   Overwrite the first n floats with the argument of the n complex numbers.
tomwalters@0: */
tomwalters@0: 
tomwalters@0: Arg( C, n )
tomwalters@0: complex *C ;
tomwalters@0: int      n ;
tomwalters@0: {
tomwalters@0:     register complex *eptr = C+n ;
tomwalters@0:     register float   *V    = (float *)C ;
tomwalters@0: 
tomwalters@0:     for (  ; C < eptr ; C++ )
tomwalters@0: 	*V++ = arg(C) ;
tomwalters@0: }
tomwalters@0: 
tomwalters@0: /*
tomwalters@0:   Return the argument of the given complex number
tomwalters@0: */
tomwalters@0: 
tomwalters@0: float arg( C )
tomwalters@0: complex *C;
tomwalters@0: {
tomwalters@0:     if (Im(C) >= 0) {
tomwalters@0: 	if (Re(C) >= 0) return (         atan(Im(C)/Re(C)) );   /* 1st quad */
tomwalters@0: 	else            return ( PI    + atan(Im(C)/Re(C)) );   /* 2nd quad */
tomwalters@0:     }
tomwalters@0:     else {
tomwalters@0: 	if (Re(C) <  0) return ( PI    + atan(Im(C)/Re(C)) );   /* 1st quad */
tomwalters@0: 	else            return ( TWOPI + atan(Im(C)/Re(C)) );   /* 2nd quad */
tomwalters@0:     }
tomwalters@0: }
tomwalters@0: 
tomwalters@0: 
tomwalters@0: /*
tomwalters@0:   C1 and C2 are both n complex numbers: (real,imag), (ie each 2n floats).
tomwalters@0:   Overwrite the C1 with product of C1 and the complex conjugate of C2.
tomwalters@0:   (When C1==C2, result is squared mod in the real parts, and 0 imag parts).
tomwalters@0: */
tomwalters@0: 
tomwalters@0: conj( C1, C2, n )
tomwalters@0: complex *C1, *C2 ;
tomwalters@0: int   n ;
tomwalters@0: {
tomwalters@0:     register complex *eptr = C1+n ;
tomwalters@0:     float    ReC1, ReC2 ;
tomwalters@0: 
tomwalters@0:     for (  ; C1 < eptr ; C1++, C2++ ) {
tomwalters@0: 	Re(C1) = ( ReC1=Re(C1) ) * ( ReC2=Re(C2) ) + Im(C1) * Im(C2) ;
tomwalters@0: 	Im(C1) =   ReC2          *        Im(C1)   - ReC1   * Im(C2) ;
tomwalters@0:     }
tomwalters@0: }
tomwalters@0: 
tomwalters@0: 
tomwalters@0: /*
tomwalters@0:   Autocorrelation function via fft.
tomwalters@0:   A complex fft (of real data) is multiplied by its conjugate, and also scaled
tomwalters@0:   prior to the inverse fft.
tomwalters@0: */
tomwalters@0: 
tomwalters@0: 
tomwalters@0: 
tomwalters@0: acf( V, m )
tomwalters@0: float *V ;
tomwalters@0: int    m ;
tomwalters@0: {
tomwalters@0:     register complex *C, *eptr ;
tomwalters@0:     register int  n = m>>1 ;
tomwalters@0:     float    tmp ;
tomwalters@0: 
tomwalters@0:     fft( V, m, 1 ) ;
tomwalters@0:     tmp = V[1] ; V[1] = 0 ;
tomwalters@0: 
tomwalters@0:     for ( C=(complex *)V, eptr=C+n ; C<eptr ; C++ ) {
tomwalters@0: 	Re(C) = ( sq(Re(C)) + sq(Im(C)) ) / n ;
tomwalters@0: 	Im(C) = 0 ;
tomwalters@0:     }
tomwalters@0:     V[1] = sq( tmp ) / n ;
tomwalters@0: 
tomwalters@0:     fft( V, m, -1 ) ;
tomwalters@0: }
tomwalters@0: 
tomwalters@0: 
tomwalters@0: /*
tomwalters@0:   Test x: return 1 if x is a power of 2, otherwise return 0.
tomwalters@0: */
tomwalters@0: 
tomwalters@0: int ispower( x )
tomwalters@0: int  x ;
tomwalters@0: {
tomwalters@0:     return ( x > 1 && log2(x) == (int)log2(x) ) ;
tomwalters@0: }
tomwalters@0: 
tomwalters@0: 
tomwalters@0: /*
tomwalters@0:   Return a power of 2, either equal to x (if x is already a power of 2), or
tomwalters@0:   the next power of 2 larger than x.
tomwalters@0: */
tomwalters@0: 
tomwalters@0: int getpower( x )
tomwalters@0: int x ;
tomwalters@0: {
tomwalters@0:     if ( log2(x) > (int)log2(x) )
tomwalters@0: 	x = 1 << ( 1 + (int)log2(x) ) ;
tomwalters@0:     return ( x ) ;
tomwalters@0: }
tomwalters@0: 
tomwalters@0: 
tomwalters@0: /*
tomwalters@0: Copy float array x into short array y, both length n points.
tomwalters@0: Scale each point using the given scale factor.
tomwalters@0: Return 1 if no 16-bit over or underflow.
tomwalters@0: Otherwise return a scale factor to replace the given scale factor.
tomwalters@0: 
tomwalters@0: */
tomwalters@0: 
tomwalters@0: float ftos( x, y, n, scale )
tomwalters@0: float *x ;
tomwalters@0: short *y ;
tomwalters@0: int    n ;
tomwalters@0: float  scale ;
tomwalters@0: {
tomwalters@0:     float  p ;
tomwalters@0:     float  min = MAXSHORT, fmin ;
tomwalters@0:     float  max = MINSHORT, fmax ;
tomwalters@0:     int    i ;
tomwalters@0: 
tomwalters@0:     fmax = fmin = 1. ;
tomwalters@0: 
tomwalters@0:     for ( i = 0 ; i < n ; i++ ) {
tomwalters@0: 	p = x[i] * scale ;
tomwalters@0: 
tomwalters@0: 	if ( p > max ) max = p ;
tomwalters@0: 	if ( p < min ) min = p ;
tomwalters@0: 
tomwalters@0: 	y[i] = (short)p ;
tomwalters@0:     }
tomwalters@0: 
tomwalters@0:     if ( max > MAXSHORT || min < MINSHORT ) {
tomwalters@0: 
tomwalters@0: 	if ( max > MAXSHORT )  fmax = MAXSHORT / max ;
tomwalters@0: 	if ( min < MINSHORT )  fmin = MINSHORT / min ;
tomwalters@0: 
tomwalters@0: 	if ( fmax < fmin ) return ( scale * fmax ) ;
tomwalters@0: 	else               return ( scale * fmin ) ;
tomwalters@0:     }
tomwalters@0: 
tomwalters@0:     return ( 1. ) ;
tomwalters@0: }
tomwalters@0: 
tomwalters@0: 
tomwalters@0: 
tomwalters@0: 
tomwalters@0: /*************************** convolution **********************************/
tomwalters@0: 
tomwalters@0: /*
tomwalters@0: Time-domain convolution. An input signal y of length n points is convolved
tomwalters@0: with a window x of length m points (where m is assumed odd for symmetry).
tomwalters@0: Return convolution signal z which must have allocated space of length n.
tomwalters@0: 
tomwalters@0: Discrete convolution is defined:
tomwalters@0: 	z[i] = sum{j=-inf to inf}  x[i-j].y[j]          for i=-inf to inf
tomwalters@0: But the signal y and window x are assumed to be zero for all time outside
tomwalters@0: the given points so that:
tomwalters@0: 	z[i] = sum{j=-m/2 to m/2}  x[i-j].y[j]          for i=0,1,...,n-1
tomwalters@0: 
tomwalters@0: Since the response of a linear filter to an arbitiary input signal is the
tomwalters@0: convolution of the signal with the filter's impulse response, the window may
tomwalters@0: be seen as the impulse response characterising the filter, and the return
tomwalters@0: z is then the filter output in response to input y.
tomwalters@0: 
tomwalters@0: Alternatively the convolution may be seen as a local averaging operation
tomwalters@0: on y with weights obtained by time-reversing and shifting x.
tomwalters@0: */
tomwalters@0: 
tomwalters@0: convolve_time( y, n, x, m, z )
tomwalters@0: short  *y ;             /* input signal                                  */
tomwalters@0: int     n ;             /* length of array y                             */
tomwalters@0: float  *x ;             /* convolution window or filter impulse response */
tomwalters@0: int     m ;             /* length of array x                             */
tomwalters@0: float  *z ;             /* output signal of length n (same as input)     */
tomwalters@0: {
tomwalters@0:     register int  i, j, k, lim1, lim2 ;
tomwalters@0:     register float sum ;
tomwalters@0: 
tomwalters@0:     for ( i = 0 ; i < n ; i++ ) {
tomwalters@0: 
tomwalters@0: 	k = m - 1 ;
tomwalters@0: 	if ( ( lim1 = i - (m-1)/2 ) < 0 ) {
tomwalters@0: 	    k += lim1 ;
tomwalters@0: 	    lim1 = 0 ;
tomwalters@0: 	}
tomwalters@0: 	if ( ( lim2 = i + (m-1)/2 ) > n )
tomwalters@0: 	    lim2 = n - 1 ;
tomwalters@0: 
tomwalters@0: 	sum = 0 ;
tomwalters@0: 	for ( j = lim1 ; j <= lim2 ; j++, --k )
tomwalters@0: 	    sum += x[k] * y[j] ;
tomwalters@0: 
tomwalters@0: 	z[i] = sum ;
tomwalters@0:     }
tomwalters@0: }
tomwalters@0: 
tomwalters@0: 
tomwalters@0: /*
tomwalters@0: Frequency-domain convolution.
tomwalters@0: */
tomwalters@0: 
tomwalters@0: convolve_freq( y, n, x, m, z )
tomwalters@0: short  *y ;             /* input signal                                  */
tomwalters@0: int     n ;             /* length of array y                             */
tomwalters@0: float  *x ;             /* convolution window or filter impulse response */
tomwalters@0: int     m ;             /* length of array x                             */
tomwalters@0: float  *z ;             /* output signal of length n (same as input)     */
tomwalters@0: {
tomwalters@0:     int     i, j, n1 ;
tomwalters@0:     float  *data, *respns, *ans ;
tomwalters@0: 
tomwalters@0:     n1 = getpower( n + m ) ;    /* padded length of input signal */
tomwalters@0: 
tomwalters@0:     data   = (float *)malloc( ( n1     + 1 ) * sizeof(float) ) ;
tomwalters@0:     respns = (float *)malloc( ( n1     + 1 ) * sizeof(float) ) ;
tomwalters@0:     ans    = (float *)malloc( ( n1 * 2 + 1 ) * sizeof(float) ) ;
tomwalters@0: 
tomwalters@0:     /* copy and pad signal */
tomwalters@0: 
tomwalters@0:     for ( i = 0 ; i < n ; i++ )
tomwalters@0: 	data[i] = y[i] ;
tomwalters@0:     for (  ; i < n1 ; i++ )
tomwalters@0: 	data[i] = (float)0 ;
tomwalters@0: 
tomwalters@0:     /* copy window into wrap-around order */
tomwalters@0: 
tomwalters@0:     for ( i = m/2, j = 0 ; i < m ; i++, j++ )
tomwalters@0: 	respns[i] = x[j] ;
tomwalters@0:     for ( i = 0 ; i < m/2 ; i++, j++ )
tomwalters@0: 	respns[i] = x[j] ;
tomwalters@0: 
tomwalters@0:     /* Convolve and copy output */
tomwalters@0: 
tomwalters@0:     convlv( data-1, n1, respns-1, m, 1, ans-1 ) ;
tomwalters@0: 
tomwalters@0:     for ( i = 0 ; i < n ; i++ )
tomwalters@0: 	z[i] = ans[i] ;
tomwalters@0: 
tomwalters@0:     free( data   ) ;
tomwalters@0:     free( respns ) ;
tomwalters@0:     free( ans    ) ;
tomwalters@0: }
tomwalters@0: 
tomwalters@0: 
tomwalters@0: /*
tomwalters@0: Convolution of two functions. [See NR: pp407-413].
tomwalters@0: Function 1 (the input signal) is in `data', length n, (n power of 2).
tomwalters@0: Function 2 (the response function) is in `respns', length m, (m<=n and odd).
tomwalters@0: (However respns must have n space available).
tomwalters@0: The response file should be stored in respns in "wrap-around order", ie with
tomwalters@0: its last half first and first half last in the array.
tomwalters@0: Return is the first n numbers in `ans', (though ans must have 2*n space).
tomwalters@0: Return convolution if isign==1, or deconvolution if isign==(-1).
tomwalters@0: 
tomwalters@0: The data array should be padded with zeroes to a power of 2.
tomwalters@0: The recommended amount of padding [NR: p411] is at least m/2, but need not be
tomwalters@0: more than m.
tomwalters@0: */
tomwalters@0: 
tomwalters@0: static float sqrarg;
tomwalters@0: #define SQR(a) (sqrarg=(a),sqrarg*sqrarg)
tomwalters@0: 
tomwalters@0: 
tomwalters@0: convlv( data, n, respns, m, isign, ans )
tomwalters@0: float data[], respns[], ans[] ;
tomwalters@0: int   n, m, isign ;
tomwalters@0: {
tomwalters@0:     int   i, no2 ;
tomwalters@0:     float dum, mag2, *fftbuf ;
tomwalters@0: 
tomwalters@0:     fftbuf = (float *)malloc( 2 * n * sizeof(float) ) ;
tomwalters@0: 
tomwalters@0:     for (i=1;i<=(m-1)/2;i++)
tomwalters@0: 	respns[n+1-i]=respns[m+1-i];
tomwalters@0:     for (i=(m+3)/2;i<=n-(m-1)/2;i++)
tomwalters@0: 	respns[i]=0.0;
tomwalters@0: 
tomwalters@0:     twofft(data,respns,fftbuf,ans,n);
tomwalters@0:     no2=n/2;
tomwalters@0:     for (i=2;i<=n+2;i+=2) {
tomwalters@0: 	if (isign == 1) {
tomwalters@0: 	    ans[i-1]=(fftbuf[i-1]*(dum=ans[i-1])-fftbuf[i]*ans[i])/no2;
tomwalters@0: 	    ans[i]=(fftbuf[i]*dum+fftbuf[i-1]*ans[i])/no2;
tomwalters@0: 	} else if (isign == -1) {
tomwalters@0: 	    if ((mag2=SQR(ans[i-1])+SQR(ans[i])) == 0.0)
tomwalters@0: 		fprintf( stderr, "warning: deconvolving at response zero\n" ) ;
tomwalters@0: 	    ans[i-1]=(fftbuf[i-1]*(dum=ans[i-1])+fftbuf[i]*ans[i])/mag2/no2;
tomwalters@0: 	    ans[i]=(fftbuf[i]*dum-fftbuf[i-1]*ans[i])/mag2/no2;
tomwalters@0: 	}
tomwalters@0: 	else {
tomwalters@0: 	    fprintf( stderr, "No meaning for ISIGN in CONVLV\n" ) ;
tomwalters@0: 	    exit( 1 ) ;
tomwalters@0: 	}
tomwalters@0:     }
tomwalters@0:     ans[2]=ans[n+1];
tomwalters@0:     realft(ans,no2,-1);
tomwalters@0: 
tomwalters@0:     free( fftbuf ) ;
tomwalters@0: }
tomwalters@0: 
tomwalters@0: 
tomwalters@0: 
tomwalters@0: /************************ fft ********************************************/
tomwalters@0: 
tomwalters@0: /*
tomwalters@0:  In-place FFT/IFFT routine for real-valued data.
tomwalters@0:  (Ref Numerical recipes, pp417).
tomwalters@0: 
tomwalters@0:  Arguments:
tomwalters@0:     data  = Input array of 2n floats,
tomwalters@0: 	      also output data array of n complex numbers.
tomwalters@0:     n     = Number of data points, (must be a power of two).
tomwalters@0: 	      Input data is 2n real numbers.
tomwalters@0: 	      Output data is  n complex numbers.
tomwalters@0:     isign = FFT when 1; IFFT when -1.
tomwalters@0: 
tomwalters@0:  Each complex number occupies two consecutive locations: real and imag.
tomwalters@0:  The output data is n real and imag parts which are the positive frequency
tomwalters@0:  half of the symmetric Fourier transform.
tomwalters@0: 
tomwalters@0:  Indexing conversion:
tomwalters@0: 
tomwalters@0:  Both input and output data are indexed: 1,..,n.
tomwalters@0:  where:       Input data is 2n real numbers.
tomwalters@0: 	      Output data is  n complex numbers, (ie 2n floats).
tomwalters@0:  To convert to conventional indexing:    0,..,m-1
tomwalters@0:  where:       Input data is m real numbers (eg. input framesize).
tomwalters@0: 	      Output data is m/2 complex numbers, (ie m floats).
tomwalters@0: 
tomwalters@0:  allocated space for "data" of:
tomwalters@0: 
tomwalters@0: 	      (m+1)*sizeof(float).
tomwalters@0: 
tomwalters@0:  call to routine "realft":
tomwalters@0: 
tomwalters@0: 	      realft( data-1, m/2, isign ) ;
tomwalters@0: 
tomwalters@0:  for example via the define:
tomwalters@0: 
tomwalters@0: #define fft(data,m,isign)  ( realft((float *)(data)-1, m>>1, isign) )
tomwalters@0: 
tomwalters@0:  This works because the location (data-1) is never indexed in routine realft.
tomwalters@0:  Output data will then be data[0] to data[m/2-1] complex numbers,
tomwalters@0:  ie data[0] to data[m-1] floats, to be interpreted as m/2 (real,imag) pairs.
tomwalters@0: 
tomwalters@0:  The first of the m/2 points of the magnitude spectrum is then:
tomwalters@0: 	      sqrt( sq( data[0] ) + sq( data[1] ) )
tomwalters@0: 
tomwalters@0: 
tomwalters@0:  The results of the IFFT are not normalized by multplication by 1/N.
tomwalters@0:  The user should multiply each returned element by 1/n.
tomwalters@0: 
tomwalters@0:  Routine realft returns the first and last real components in data[1] and
tomwalters@0:  data[2]. To make this correspond with the return from twofft, the mod below
tomwalters@0:  sets data[2]=0.
tomwalters@0: */
tomwalters@0: 
tomwalters@0: 
tomwalters@0: realft(data,n,isign)
tomwalters@0: float data[];
tomwalters@0: int n,isign;
tomwalters@0: {
tomwalters@0: 	int i,i1,i2,i3,i4,n2p3;
tomwalters@0: 	float c1=0.5,c2,h1r,h1i,h2r,h2i;
tomwalters@0: 	double wr,wi,wpr,wpi,wtemp,theta;
tomwalters@0: 
tomwalters@0: 	theta=3.141592653589793/(double) n;
tomwalters@0: 	if (isign == 1) {
tomwalters@0: 		c2 = -0.5;
tomwalters@0: 		four1(data,n,1);
tomwalters@0: 	} else {
tomwalters@0: 		c2=0.5;
tomwalters@0: 		theta = -theta;
tomwalters@0: 	}
tomwalters@0: 	wtemp=sin(0.5*theta);
tomwalters@0: 	wpr = -2.0*wtemp*wtemp;
tomwalters@0: 	wpi=sin(theta);
tomwalters@0: 	wr=1.0+wpr;
tomwalters@0: 	wi=wpi;
tomwalters@0: 	n2p3=2*n+3;
tomwalters@0: 	for (i=2;i<=n/2;i++) {
tomwalters@0: 		i4=1+(i3=n2p3-(i2=1+(i1=i+i-1)));
tomwalters@0: 		h1r=c1*(data[i1]+data[i3]);
tomwalters@0: 		h1i=c1*(data[i2]-data[i4]);
tomwalters@0: 		h2r = -c2*(data[i2]+data[i4]);
tomwalters@0: 		h2i=c2*(data[i1]-data[i3]);
tomwalters@0: 		data[i1]=h1r+wr*h2r-wi*h2i;
tomwalters@0: 		data[i2]=h1i+wr*h2i+wi*h2r;
tomwalters@0: 		data[i3]=h1r-wr*h2r+wi*h2i;
tomwalters@0: 		data[i4] = -h1i+wr*h2i+wi*h2r;
tomwalters@0: 		wr=(wtemp=wr)*wpr-wi*wpi+wr;
tomwalters@0: 		wi=wi*wpr+wtemp*wpi+wi;
tomwalters@0: 	}
tomwalters@0: 	if (isign == 1) {
tomwalters@0: 		data[1] = (h1r=data[1])+data[2];
tomwalters@0: 		data[2] = h1r-data[2];
tomwalters@0: 	} else {
tomwalters@0: 		data[1]=c1*((h1r=data[1])+data[2]);
tomwalters@0: 		data[2]=c1*(h1r-data[2]);
tomwalters@0: 		four1(data,n,-1);
tomwalters@0: 	}
tomwalters@0: }
tomwalters@0: 
tomwalters@0: 
tomwalters@0: #define SWAP(a,b) tempr=(a);(a)=(b);(b)=tempr
tomwalters@0: 
tomwalters@0: four1(data,nn,isign)
tomwalters@0: float data[];
tomwalters@0: int nn,isign;
tomwalters@0: {
tomwalters@0: 	int n,mmax,m,j,istep,i;
tomwalters@0: 	double wtemp,wr,wpr,wpi,wi,theta;
tomwalters@0: 	float tempr,tempi;
tomwalters@0: 
tomwalters@0: 	n=nn << 1;
tomwalters@0: 	j=1;
tomwalters@0: 	for (i=1;i<n;i+=2) {
tomwalters@0: 		if (j > i) {
tomwalters@0: 			SWAP(data[j],data[i]);
tomwalters@0: 			SWAP(data[j+1],data[i+1]);
tomwalters@0: 		}
tomwalters@0: 		m=n >> 1;
tomwalters@0: 		while (m >= 2 && j > m) {
tomwalters@0: 			j -= m;
tomwalters@0: 			m >>= 1;
tomwalters@0: 		}
tomwalters@0: 		j += m;
tomwalters@0: 	}
tomwalters@0: 	mmax=2;
tomwalters@0: 	while (n > mmax) {
tomwalters@0: 		istep=2*mmax;
tomwalters@0: 		theta=6.28318530717959/(isign*mmax);
tomwalters@0: 		wtemp=sin(0.5*theta);
tomwalters@0: 		wpr = -2.0*wtemp*wtemp;
tomwalters@0: 		wpi=sin(theta);
tomwalters@0: 		wr=1.0;
tomwalters@0: 		wi=0.0;
tomwalters@0: 		for (m=1;m<mmax;m+=2) {
tomwalters@0: 			for (i=m;i<=n;i+=istep) {
tomwalters@0: 				j=i+mmax;
tomwalters@0: 				tempr=wr*data[j]-wi*data[j+1];
tomwalters@0: 				tempi=wr*data[j+1]+wi*data[j];
tomwalters@0: 				data[j]=data[i]-tempr;
tomwalters@0: 				data[j+1]=data[i+1]-tempi;
tomwalters@0: 				data[i] += tempr;
tomwalters@0: 				data[i+1] += tempi;
tomwalters@0: 			}
tomwalters@0: 			wr=(wtemp=wr)*wpr-wi*wpi+wr;
tomwalters@0: 			wi=wi*wpr+wtemp*wpi+wi;
tomwalters@0: 		}
tomwalters@0: 		mmax=istep;
tomwalters@0: 	}
tomwalters@0: }
tomwalters@0: 
tomwalters@0: 
tomwalters@0: twofft(data1,data2,fft1,fft2,n)
tomwalters@0: float data1[],data2[],fft1[],fft2[];
tomwalters@0: int n;
tomwalters@0: {
tomwalters@0: 	int nn3,nn2,jj,j;
tomwalters@0: 	float rep,rem,aip,aim;
tomwalters@0: 
tomwalters@0: 	nn3=1+(nn2=2+n+n);
tomwalters@0: 	for (j=1,jj=2;j<=n;j++,jj+=2) {
tomwalters@0: 		fft1[jj-1]=data1[j];
tomwalters@0: 		fft1[jj]=data2[j];
tomwalters@0: 	}
tomwalters@0: 	four1(fft1,n,1);
tomwalters@0: 	fft2[1]=fft1[2];
tomwalters@0: 	fft1[2]=fft2[2]=0.0;
tomwalters@0: 	for (j=3;j<=n+1;j+=2) {
tomwalters@0: 		rep=0.5*(fft1[j]+fft1[nn2-j]);
tomwalters@0: 		rem=0.5*(fft1[j]-fft1[nn2-j]);
tomwalters@0: 		aip=0.5*(fft1[j+1]+fft1[nn3-j]);
tomwalters@0: 		aim=0.5*(fft1[j+1]-fft1[nn3-j]);
tomwalters@0: 		fft1[j]=rep;
tomwalters@0: 		fft1[j+1]=aim;
tomwalters@0: 		fft1[nn2-j]=rep;
tomwalters@0: 		fft1[nn3-j] = -aim;
tomwalters@0: 		fft2[j]=aip;
tomwalters@0: 		fft2[j+1] = -rem;
tomwalters@0: 		fft2[nn2-j]=aip;
tomwalters@0: 		fft2[nn3-j]=rem;
tomwalters@0: 	}
tomwalters@0: }
tomwalters@0: 
tomwalters@0: 
tomwalters@0: /*********************** windows *******************************************/
tomwalters@0: 
tomwalters@0: /*
tomwalters@0: Allocate space for and compute an n-point Hann or raised cosine window,
tomwalters@0: returning array.
tomwalters@0: */
tomwalters@0: 
tomwalters@0: float *raised_cosine( n )
tomwalters@0: int  n ;
tomwalters@0: {
tomwalters@0:     float            *W ;
tomwalters@0:     register int      i ;
tomwalters@0:     register double   k ;
tomwalters@0: 
tomwalters@0:     W = (float *)malloc( n * sizeof(float) ) ;
tomwalters@0: 
tomwalters@0:     k = TWOPI/(double)(n-1);
tomwalters@0:     for (i=0 ; i<n ; i++)
tomwalters@0: 	W[i] = 0.5 * ( 1.0 - cos(k*i) ) ;
tomwalters@0: 
tomwalters@0:     return W ;
tomwalters@0: }
tomwalters@0: 
tomwalters@0: 
tomwalters@0: /*
tomwalters@0: Allocate space for and compute an n-point Hamming window, returning array.
tomwalters@0: */
tomwalters@0: 
tomwalters@0: float *hamming( n )
tomwalters@0: int  n ;
tomwalters@0: {
tomwalters@0:     float            *W ;
tomwalters@0:     register int      i ;
tomwalters@0:     register double   k ;
tomwalters@0: 
tomwalters@0:     W = (float *)malloc( n * sizeof(float) ) ;
tomwalters@0: 
tomwalters@0:     k = TWOPI/(double)(n-1);
tomwalters@0:     for (i=0 ; i<n ; i++)
tomwalters@0: 	W[i] = 0.54 - 0.46*cos(k*i) ;
tomwalters@0: 
tomwalters@0:     return W ;
tomwalters@0: }
tomwalters@0: 
tomwalters@0: 
tomwalters@0: /*
tomwalters@0: Allocate space for and compute a Gaussian window with given standard deviation
tomwalters@0: `s' (samples) over a range of `n' standard deviations either side of a zero
tomwalters@0: mean. The size of the returned window (returned via variable `num') will be:
tomwalters@0: 	2 * n * s  +  1   [points]
tomwalters@0: (This is always odd as the window is symmetrical).
tomwalters@0: */
tomwalters@0: 
tomwalters@0: float *gauss_window( s, n, num )
tomwalters@0: float  s, n ;
tomwalters@0: int   *num ;
tomwalters@0: {
tomwalters@0:     float  x, *y, var ;
tomwalters@0:     int    i, points  ;
tomwalters@0: 
tomwalters@0:     points = 2 * n * s + 1 ;
tomwalters@0:     y = (float *)malloc( points * sizeof(float) ) ;
tomwalters@0: 
tomwalters@0:     x   = ( - n * s ) ;
tomwalters@0:     var = s * s ;
tomwalters@0:     for ( i = 0 ; i < points ; i++, x++ )
tomwalters@0: 	y[i] = exp( -(double)( 0.5 * x * x / var ) ) ;
tomwalters@0: 
tomwalters@0:     *num = points ;
tomwalters@0:     return y ;
tomwalters@0: }
tomwalters@0: 
tomwalters@0: 
tomwalters@0: /*********************** frames *******************************************/
tomwalters@0: 
tomwalters@0: /*
tomwalters@0: Return a ptr to the n'th frame (numbered 1,2,3,...,n), with given
tomwalters@0: framewidth (size) and frameshift (step). Return the null ptr if eof before
tomwalters@0: the n'th frame. Frame number n=0 refers to the last frame before eof.
tomwalters@0: Successive calls to getframe must request increasing frame numbers.
tomwalters@0: 
tomwalters@0: Data (binary shorts) is buffered in an array of "blocksize" shorts,
tomwalters@0: allocated internally, where:
tomwalters@0:     blocksize = size + ( frames_per_block - 1 ) * step
tomwalters@0: This means that the buffer can hold "frames_per_block" frames of the given
tomwalters@0: size and step, and ensures that no frame crosses the boundary between
tomwalters@0: successive buffers. At the buffer boundary, the overlap part
tomwalters@0: (where overlap = size - step) of the last frame in the buffer must
tomwalters@0: be shifted to the start of the buffer, and the step part of the next frame
tomwalters@0: must be read so as to follow it in the buffer, completing the first frame of
tomwalters@0: the next buffer.
tomwalters@0: To protect the last frame (in the event that the next step part is actually
tomwalters@0: the eof), the first frame of the next buffer must not corrupt the last frame
tomwalters@0: of the previous buffer. This is ensured by arranging that:
tomwalters@0:     size <= ( frames_per_block - 1 ) * step
tomwalters@0: This constrains frames_per_block to be:
tomwalters@0:     frames_per_block >= ( size / step ) + 1
tomwalters@0: Therefore the minimum number of frames_per_block (when step = size, and there
tomwalters@0: is no overlap) is 2.
tomwalters@0: The actual value you give to frames_per_block is an efficiency issue, being
tomwalters@0: a trade-off between space usage and the number of times a shift is necessary
tomwalters@0: at a buffer boundary. A reasonable guess would be (see define below):
tomwalters@0:     frames_per_block = ( size / step ) + 1 + FRAMES_PER_BLOCK
tomwalters@0: 
tomwalters@0: An example of a shifting frame buffer, from frame a to b inclusive in steps
tomwalters@0: of 1 frame, would be:
tomwalters@0: 
tomwalters@0:     while ( ( buf = getframe( fp, a, size, step ) ) != (short *)0  && ( a<=b || b==0 ) ) {
tomwalters@0: 	process( buf, size ) ;
tomwalters@0: 	a++ ;
tomwalters@0:     }
tomwalters@0:     if ( a<=b && b>0 )
tomwalters@0: 	fprintf(stderr,"warning: not enough frames for request\n");
tomwalters@0: 
tomwalters@0: */
tomwalters@0: 
tomwalters@0: #define FRAMES_PER_BLOCK    10
tomwalters@0: 
tomwalters@0: short *getframe( fp, n, size, step )
tomwalters@0: FILE *fp ;
tomwalters@0: int   n, size, step ;
tomwalters@0: {
tomwalters@0:     static int    first = 1 ;
tomwalters@0:     static short *buf, *endptr ;
tomwalters@0:     static short *bptr = (short *) 0 ;
tomwalters@0:     static int    blocksize, overlap, frames_per_block ;
tomwalters@0:     static int    m = 0 ;       /* frame counter */
tomwalters@0:     int           startsize, i ;
tomwalters@0: 
tomwalters@0:     if ( n < m )                /* catches invalid calls and also case of n==0 */
tomwalters@0: 	return (short *) 0 ;
tomwalters@0: 
tomwalters@0:     if ( first ) {
tomwalters@0: 	first = 0 ;
tomwalters@0: 	frames_per_block = ( size / step ) + 1 + FRAMES_PER_BLOCK ;
tomwalters@0: 	blocksize = size + ( frames_per_block - 1 ) * step ;
tomwalters@0: 	overlap   = size - step ;
tomwalters@0: 	bptr      = buf = (short *)malloc( blocksize * sizeof(short) ) ;
tomwalters@0: 	endptr    = buf + blocksize ;
tomwalters@0: 
tomwalters@0: 	if ( n > 0 ) {
tomwalters@0: 
tomwalters@0: 	    /* seek the n'th frame in blocks of blocksize. */
tomwalters@0: 
tomwalters@0: 	    startsize = ( n - 1 ) * step ;
tomwalters@0: 	    for ( i = blocksize ; i < startsize ; i += blocksize )
tomwalters@0: 		if ( fread( buf, sizeof(short), blocksize, fp ) < blocksize )
tomwalters@0: 		    return (short *) 0 ;
tomwalters@0: 	    if ( ( startsize -= ( i - blocksize ) ) > 0 )
tomwalters@0: 		if ( fread( buf, sizeof(short), startsize, fp ) < startsize )
tomwalters@0: 		    return (short *) 0 ;
tomwalters@0: 
tomwalters@0: 	    if ( ( fread( bptr, sizeof(short), size, fp ) ) < size )
tomwalters@0: 		return (short *) 0 ;
tomwalters@0: 	    m = n ;
tomwalters@0: 	}
tomwalters@0: 	else {
tomwalters@0: 
tomwalters@0: 	    /* seek the last frame in blocks of blocksize */
tomwalters@0: 
tomwalters@0: 	    if ( fread( buf, sizeof(short), size, fp ) < size )
tomwalters@0: 		return (short *) 0 ;
tomwalters@0: 	    startsize = blocksize - size ;
tomwalters@0: 	    for ( m = 1 ; ( i = fread( buf+size, sizeof(short), startsize, fp ) ) == startsize ; m += frames_per_block - 1 ) {
tomwalters@0: 		bptr = endptr-size ;
tomwalters@0: 		while ( bptr < endptr )
tomwalters@0: 		    *buf++ = *bptr++ ;
tomwalters@0: 		buf -= size ;
tomwalters@0: 	    }
tomwalters@0: 	    bptr = buf + ( i / step) * step ;
tomwalters@0: 	    m += i / step ;
tomwalters@0: 	}
tomwalters@0: 	return ( bptr ) ;
tomwalters@0:     }
tomwalters@0: 
tomwalters@0:     for (  ; m < n || n == 0 ; m++ ) {
tomwalters@0: 
tomwalters@0: 	if ( bptr + size >= endptr ) {  /* shift last overlap to buf start */
tomwalters@0: 	    while ( bptr < endptr )
tomwalters@0: 		*buf++ = *bptr++ ;
tomwalters@0: 	    buf -= overlap ;
tomwalters@0: 	    bptr = buf - step ;
tomwalters@0: 	}
tomwalters@0: 
tomwalters@0: 	if ( ( fread( bptr+size, sizeof(short), step, fp ) ) < step ) {
tomwalters@0: 	    if ( n == 0 ) {
tomwalters@0: 		if ( bptr < buf ) return ( endptr - size ) ;
tomwalters@0: 		else              return ( bptr          ) ;
tomwalters@0: 	    }
tomwalters@0: 	    return (short *) 0 ;
tomwalters@0: 	}
tomwalters@0: 	bptr += step ;
tomwalters@0:     }
tomwalters@0:     return ( bptr ) ;
tomwalters@0: }
tomwalters@0: 
tomwalters@0: 
tomwalters@0: /*************************************************************************/
tomwalters@0: 
tomwalters@0: /*
tomwalters@0: Seek past n frames in blocks of blocksize.
tomwalters@0: Frames have width=size and shift=step.
tomwalters@0: When step == size, the seek reads ( n * size ) shorts from the i/p stream.
tomwalters@0: When step < size,  the seek reads ( size + ( n - 1 ) * step ) shorts.
tomwalters@0: Return 1, or otherwise 0 for an improper seek (insufficient data in stream).
tomwalters@0: */
tomwalters@0: 
tomwalters@0: seekframes( fp, n, size, step )
tomwalters@0: FILE *fp ;
tomwalters@0: int   n, size, step ;
tomwalters@0: {
tomwalters@0:     static short *buf ;
tomwalters@0:     static int    first = 1 ;
tomwalters@0:     static int    blocksize, frames_per_block ;
tomwalters@0:     int           startsize = size + ( n - 1 ) * step ;
tomwalters@0:     int           i ;
tomwalters@0: 
tomwalters@0:     if ( first ) {
tomwalters@0: 	first = 0 ;
tomwalters@0: 	frames_per_block = ( size / step ) + 1 + FRAMES_PER_BLOCK ;
tomwalters@0: 	blocksize = size + ( frames_per_block - 1 ) * step ;
tomwalters@0: 	buf = (short *)malloc( blocksize * sizeof(short) ) ;
tomwalters@0:     }
tomwalters@0:     for ( i = blocksize ; i < startsize ; i += blocksize )
tomwalters@0: 	if ( fread( buf, sizeof(short), blocksize, fp ) < blocksize )
tomwalters@0: 	    return 0 ;
tomwalters@0:     if ( (startsize -= (i - blocksize)) > 0 )
tomwalters@0: 	if ( fread( buf, sizeof(short), startsize, fp ) < startsize )
tomwalters@0: 	    return 0 ;
tomwalters@0: 
tomwalters@0:     return 1 ;
tomwalters@0: }
tomwalters@0: 
tomwalters@0: /*************************************************************************/
tomwalters@0: 
tomwalters@0: /*
tomwalters@0: Seek past n bytes from current position in stream.
tomwalters@0: Return 1, or otherwise 0 for an improper seek (insufficient data in stream).
tomwalters@0: */
tomwalters@0: 
tomwalters@0: seekbytes( fp, n )
tomwalters@0: FILE  *fp ;
tomwalters@0: int    n  ;
tomwalters@0: {
tomwalters@0:     static char  *buf ;
tomwalters@0:     static int    first = 1 ;
tomwalters@0: 
tomwalters@0:     if ( n > 0 ) {
tomwalters@0: 	if ( first ) {
tomwalters@0: 	    first = 0 ;
tomwalters@0: 	    buf = (char *)malloc( n ) ;
tomwalters@0: 	}
tomwalters@0: 
tomwalters@0: 	if ( fread( buf, 1, n, fp ) < n )
tomwalters@0: 	    return 0 ;
tomwalters@0:     }
tomwalters@0:     return 1 ;
tomwalters@0: }
tomwalters@0: 
tomwalters@0: 
tomwalters@0: 
tomwalters@0: /*********************** normalizing **************************************/
tomwalters@0: 
tomwalters@0: /*
tomwalters@0: Normalize the n-array y so that the sum of the points (ie the area) is unity.
tomwalters@0: */
tomwalters@0: 
tomwalters@0: normalize_area( y, n )
tomwalters@0: float *y ;
tomwalters@0: int    n ;
tomwalters@0: {
tomwalters@0:     float  sum = 0 ;
tomwalters@0:     int    i ;
tomwalters@0: 
tomwalters@0:     for ( i=0 ; i < n ; i++ )
tomwalters@0: 	sum += y[i] ;
tomwalters@0:     for ( i=0 ; i < n ; i++ )
tomwalters@0: 	y[i] /= sum ;
tomwalters@0: }
tomwalters@0: 
tomwalters@0: 
tomwalters@0: /*
tomwalters@0: Normalize the n-array y so that the range of y[i] is [0,1].
tomwalters@0: */
tomwalters@0: 
tomwalters@0: normalize_range( y, n )
tomwalters@0: float *y ;
tomwalters@0: int    n ;
tomwalters@0: {
tomwalters@0:     int   i ;
tomwalters@0:     float max = ( -256000. ) ;
tomwalters@0:     float min =    256000.   ;
tomwalters@0: 
tomwalters@0:     for ( i = 0 ; i < n ; i++ ) {       /* find max and min */
tomwalters@0: 	if ( y[i] > max ) max = y[i] ;
tomwalters@0: 	if ( y[i] < min ) min = y[i] ;
tomwalters@0:     }
tomwalters@0: 
tomwalters@0:     for ( i = 0 ; i < n ; i++ )
tomwalters@0: 	y[i] = ( y[i] - min ) / ( max - min ) ;
tomwalters@0: }
tomwalters@0: 
tomwalters@0: