$$\phantom{******** If you see this on the webpage then the browser could not locate *********}$$
$$\phantom{******** jsMath/easy/load.js or the variable root is set wrong in this file *********}$$
$$\newcommand{\vector}[1]{\left[\begin{array}{c} #1 \end{array}\right]}$$ $$\newenvironment{matrix}{\left[\begin{array}{cccccccccc}} {\end{array}\right]}$$ $$\newcommand{\A}{{\cal A}}$$ $$\newcommand{\W}{{\cal W}}$$

/* Copyright 1998-2014 Research Foundation State University of New York   */

/* This file is part of QUB Express.                                      */

/* QUB Express 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 3 of the License, or      */
/* (at your option) any later version.                                    */

/* QUB Express 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,     */
/* named LICENSE.txt, in the QUB Express program directory.  If not, see  */
/* <http://www.gnu.org/licenses/>.                                        */

/*

*/

#ifdef _WIN32
  #include <windows.h>
  #include <time.h>
  #include <float.h>
  #define sleep Sleep
#else
  #include <stdlib.h>
  #define BOOL int
  #define TRUE 1
  #define FALSE 0
#endif

#include <iostream>
using std::cout;
using std::cerr;
using std::endl;
#include <math.h>
#include <string.h>
#include <vector>
#include <map>
using std::vector;
using std::map;
using std::pair;

#include "qub_filter.h"


void errexit(QUBFAST_VAR_NOT_USED
             const char *msg) {
    //fprintf(stderr,"\n%s\n",msg);
    //exit(1);
	}
	#define sqr(x)      ((x)*(x))

//===== Magic numbers 
	const double PI = 3.14159265358979;
	const double PI2 = 6.28318530717959;

#define EXIT_ERROR /*fprintf(stderr,"Memory allocation error\n"); exit(1);*/return NULL;

double* dalloc1(int size) {
    double * ptr = (double *)malloc(size*sizeof(double));
    if (ptr==NULL)
       EXIT_ERROR
    return ptr;
	}

template<class T>
T* talloc1(int size) {
  T * ptr = (T*) malloc(size*sizeof(T));
  if (ptr == NULL)
    EXIT_ERROR
  return ptr;
}

//----- local functions 
//void four1_0(double *data, long nn, int isign);
//void twofft0(double *data1, double *data2, double *fft1, double *fft2, long n);
//void realft0(double *data, long n, int isign);

//	-----------------------------------------------------------------------------------
//	Fast fourier transform :
//	Replaces data [0..2*nn-1] with ...
//		isign=1 : its discrete fourier transform
//		isign=-1: nn * its inverse discrete fourier transform
//	Data is a complex array of length nn or a real array of length nn
//	nn must be an integer power of 2 !!!
#define SWAP(a, b) tempswap = (a); (a) = (b); (b) = tempswap
template<class T>
void four1_0(T *data, long nn, int isign){
	long n, mmax, m, j, istep, i;
	double wtemp, wr, wpr, wpi, wi, theta;
	double tempr, tempi;
	T tempswap;

	n = nn << 1;
	j = 1;
	for( i=1; i<n; i+=2 ) {		// This is the bit reversal section of the routine
		if (j > i) {			// Exchange the two complex numbers
			SWAP( data[j-1], data[i-1]);
			SWAP( data[j], data[i]);
			}
		m = n >> 1;
		while (m >= 2 && j > m) {
			j -= m;
			m >>= 1;
			}
		j += m;
		}

	//----- Danielson Lanczos 
	mmax = 2;
	while( n>mmax ) {					// outer loop executed log2(nn) times
		istep = mmax << 1;
		theta = ((T)isign) * ( PI2 / ((T)mmax));	// initialize the trigonometric occurrence
		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;			// Danielson Lanczos formula
				tempr = wr*data[j-1] - wi*data[j];	
				tempi = wr*data[j] + wi*data[j-1];
				data[j-1] = (T) (data[i-1] - tempr);
				data[j] = (T) (data[i] - tempi);
				data[i-1] += (T) tempr;
				data[i] += (T) tempi;
				}
			wr = (wtemp=wr)*wpr - wi*wpi + wr;	// Trigonometric recurrence
			wi = wi*wpr + wtemp*wpi + wi;
			}
		mmax = istep;
		}
	}
#undef SWAP

//	-----------------------------------------------------------------------------------
//	Simultaneous four1() of two real arrays using real and imag of complex numbers alg.
//	Given data1[0..n-1] data2[0..n-1], call four1 and return fft1[0..2n-1],fft2[0..2n-1]
//	complex arrays of FFT's
//	n must be an integer power of 2 !! 
template<class T>
void twofft0(T *data1, T *data2, T *fft1, T *fft2, long n){
	T rep, rem, aip, aim;
	long j, nn=n+n;

	// pack the two real arrays into one complex array
	for( j=0; j<n; ++j) {
		fft1[j+j] = data1[j];
		fft1[j+j+1] = data2[j];
		}

	// transform the complex array 
	four1_0(fft1, n, 1);

	fft2[0] = fft1[1];
	fft1[1] = fft2[1] = 0.0;

	for( j=2; j<=n; j+=2 ) {	// Use symmetries to separate the two transforms
		rep = (T) (0.5 * (fft1[j] + fft1[nn-j]));
		rem = (T) (0.5 * (fft1[j] - fft1[nn-j]));
		aip = (T) (0.5 * (fft1[j+1] + fft1[nn+1-j]));
		aim = (T) (0.5 * (fft1[j+1] - fft1[nn+1-j]));
		fft1[j] = rep;			// Ship them out in two complex arrays
		fft1[j+1] = aim;
		fft1[nn-j] = rep;
		fft1[nn+1-j] = -aim;
		fft2[j] = aip;
		fft2[j+1] = -rem;
		fft2[nn-j] = aip;
		fft2[nn+1-j] = rem;
		}
	}

//	-----------------------------------------------------------------------------------
//	FFT a single real data set:  data1(0:n-1)
template<class T>
void realft0(T *data, long n, int isign){
	long i, ii;
	T c1 = 0.5, c2, h1r, h1i, h2r, h2i;
    T wr, wi, wpr, wpi, wtemp, theta;

    theta = (T) (PI / (double) (n >> 1));

	if (isign == 1) {
		c2 = -0.5;
		four1_0(data, n >> 1, 1);
		} 
	else {
		c2 = 0.5;
		theta = -theta;
		}

	wtemp = (T) sin(0.5*theta);
	wpr = (T) (-2.0*wtemp*wtemp);
	wpi = (T) sin(theta);
	wr = (T) (1.0 + wpr);
	wi = wpi;

	for( i=2; i<=(n>>2); i++) {
		ii=i+i;
		h1r = c1*( data[ii-2] + data[n+2-ii] );
		h1i = c1*( data[ii-1] - data[n+3-ii] );
		h2r =-c2*( data[ii-1] + data[n+3-ii] );
		h2i = c2*( data[ii-2] - data[n+2-ii] );
		data[ii-2] = h1r + wr*h2r - wi*h2i;
		data[ii-1] = h1i + wr*h2i + wi*h2r;
		data[n+2-ii] = h1r - wr*h2r + wi*h2i;
		data[n+3-ii] =-h1i + wr*h2i + wi*h2r;
		wr = (wtemp = wr)*wpr - wi*wpi + wr;
		wi = wi*wpr + wtemp*wpi + wi;
		}

    if (isign == 1) {
		data[0] = (h1r = data[0]) + data[1];
		data[1] = h1r - data[1];
		} 
	else {
		data[0] = c1*( (h1r=data[0]) + data[1] );
		data[1] = c1*( h1r-data[1] );
		four1_0(data, n >> 1, -1);
		}
	}

/*
 * Convolve or deconvolve a real data set data[0..n-1] with a response 
 * function respns[0..n-1].
 *  1. n must be an integer power of two.
 *  2. data[] must be padded with zeros (at the end) in the calling 
 *     program, and the number of zeros should not be less than the 
 *     positive duration of the response function (not including t=0).
 *  3. m must be an odd integer.
 *  4. The response function must be stored in wrap-around order in  
 *     the first m elements of respons[].
 *  5. The result is returned in the first n components of ans[].
 *     However, ans[] must be supplied in the calling program with 
 *     dimensions [0..2*n-1].
 */
template<class T>
void convlv0(T *data, long n, T *respns, long m, int isign, T *ans){
	long i, no2;
	T dum, mag2, *fft;

	fft = talloc1<T>(2*(int)n);
   
    for( i=1; i<=(m-1)/2; i++)
		respns[n-i] = respns[m-i];

    for( i=(m+3)/2; i<=n-(m-1)/2; i++)
		respns[i-1]=0.0;

    twofft0(data, respns, fft, ans, n);

    no2 = n >> 1;

    for( i=0; i<=n; i+=2) {
		if (isign == 1) {
			dum = ans[i];
			ans[i]	= (T) ((fft[i] * dum - fft[i+1] * ans[i+1]) / ((T)no2));
			ans[i+1]= (T) ((fft[i+1] * dum + fft[i+1] * ans[i+1]) / ((T)no2));
			} 
		else if (isign == - 1) {
			if ((mag2 = sqr(ans[i]) + sqr(ans[i+1])) == 0.0) 
				errexit("Deconvolving at response zero in convlv.");
			dum = ans[i];
			ans[i]	= (T) ((fft[i] * dum + fft[i+1] * ans[i+1]) / mag2 / ((T)no2));
			ans[i+1]= (T) ((fft[i+1] * dum - fft[i] * ans[i+1]) / mag2 / ((T)no2));
			}
		}

	ans[1] = ans[n];
	realft0(ans, n, -1);
    
	free(fft);
	}

//	-----------------------------------------------------------------------------------
template<class T>
T *gsfir(double fc, int *nfir){
    int	k, n;
    T tmp, sigma, *h;
    
    sigma = (T) (0.1325/fc);
    n = (int)(4*sigma);               
    *nfir = 2*n + 1;

    h = talloc1<T>(*nfir + 3);
	
    if (sigma < 0.6) {
		*nfir = 3;
		h[0] = h[2] = (T) (sqr(sigma)/2.0);
		h[1] = (T) (1.0f - h[0] - h[2]);
		} 
	else {
	        h[n] = (T) (1.0/(sqrt(2.0*PI)*sigma));
		for (k = 1; k <= n; k++) {
		  tmp = (T) (((T)k)/sigma);
			tmp = (T) sqr(tmp);
			h[n+k] = h[n-k] = (T) (h[n]*exp(-tmp/2.0));
			}
		}
    return h;
	}

//	-----------------------------------------------------------------------------------
//	nh must be an odd integer
//	This is a strange function.   Used in filtering ?
//	Example output  : wrap( [1,2,3,4,5,6,7] ) -> [4,5,6,7,6,5,4]
template<class T>
void wrap(int nh, T *h)  {
    int i, m = (nh-1)/2;
    
    for( i=0; i<=m; i++) 
		h[i] = h[m+i];

    for( i=1; i<=m; i++)
		h[m+i] = h[m-i+1];
	}

template<class T>
void ovrlap(T *data, long ndata, T *respns, long m, T *ans, int nfft){
    long i, nd, np; 
	T *s, *r, *y;

	nd = nfft - (m - 1);

	s = talloc1<T>(nfft);
	r = talloc1<T>(nfft);
	y = talloc1<T>(2*nfft);
    
	memset( ans, 0, ndata*sizeof(T));	// for( i=0; i<ndata; i++) ans[i]=0.0;

    for( np=0; np<ndata; np+=nd) {
		for( i=1; i<=nfft; i++) 
			s[i-1] = 0.0;
		for( i=0; i<nd; i++)
			if( np+i < ndata )
				s[(m-1)/2+i] = data[np+i];
		for( i=0; i<m; i++) 
			r[i]=respns[i];

		try {
			convlv0(s, nfft, r, m, 1, y);
			} 
		catch (...) {
			}

		for( i=0; i<(m-1)/2; i++)
			if (np-i>=1) 
				ans[np-i-1] += y[(m-1)/2 - i - 1];

		for( i=1; i<=nd+(m-1)/2; i++)
			if (np+i<=ndata) 
				ans[np+i-1] += y[(m-1)/2 + i - 1];
		}

	free(s);
    free(r);
    free(y);
	}

// -----------------------------------------------------------------------------------
extern "C" QUBFAST_API int qub_filterdatad(double *data, double *fdata, int ndata, double dt, double freq) {
    int	nh;
    double * h = NULL;
    int rtnVal = 0;
    int fft_count = 1024;
    
    try {
      h = gsfir<double>(freq*dt, &nh);
      wrap(nh, h);
      while ( fft_count < nh )
	fft_count *= 2;
      ovrlap(data, ndata, h, nh, fdata, fft_count);
    } 
    catch (...) {
      rtnVal = 1;
    }
#if defined(_WIN32)
    unsigned int fstat = _clearfp(); // just in case
    _fpreset();
#endif
    free(h);
    return rtnVal;
}

extern "C" QUBFAST_API int qub_filterdataf(float *data, float *fdata, int ndata, float dt, float freq) {
    int	nh;
    float * h = NULL;
    int rtnVal = 0;
    int fft_count = 1024;
    
    try {
      h = gsfir<float>(freq*dt, &nh);
      wrap(nh, h);
      while ( fft_count < nh )
	fft_count *= 2;
      ovrlap(data, ndata, h, nh, fdata, fft_count);
    } 
    catch (...) {
      rtnVal = 1;
    }
#if defined(_WIN32)
    unsigned int fstat = _clearfp(); // just in case
    _fpreset();
#endif
    free(h);
    return rtnVal;
}