/*------------------------------------------------------------------------------*
 * File Name:fft_utils.c 														*
 * Creation: ER 5/15/03															*
 * Purpose: Origin's internal FFT analysis routines								*
 * Copyright (c) Originlab Corp.	2003										*
 * All Rights Reserved															*
 * 																				*
 * Modification Log:															*
 *------------------------------------------------------------------------------*/
 
////////////////////////////////////////////////////////////////////////////////////
#include <origin.h> // The Project and Application class 
#include <OC_nag.h> // this contains all the NAG headers, 
#include <matrix.h>

////////////////////////////////////////////////////////////////////////////////////

/**
*/
int fft_convolute(vector& vecSignal, vector& vecResponse, vector& vecResult, bool bNormalize, bool bWrap)
{
	int iRet;
		
	// Get length of signal and test for NAG function acceptability
	int iSize = vecSignal.GetSize();
	int iLengthDiff = fft_test_length(iSize);
	// Adjust length as needed
	iSize -= iLengthDiff;
	vecSignal.SetSize(iSize);

	// Pad response with zeroes if shorter than signal
	vecResponse.SetSize(iSize);
	
	// Wrap/Normalize response if requested
	if(bWrap) fft_wrap_response(vecResponse);
	if(bNormalize) fft_normalize_response(vecResponse);

	// Call NAG function(s) to perform convolution
	if((iRet = nag_convolution_real(Nag_Convolution, iSize, vecSignal, vecResponse)) != 0) return iRet;
		
	// Copy result 
	vecResult = vecSignal;
	
	// Return 0 error code on successful completion
	return iRet;
}


/**
*/
int fft_deconvolute(vector& vecSignal, vector& vecResponse, vector& vecResult, bool bNormalize, bool bWrap)
{
	int iRet;

	// Get length of signal and test for NAG function acceptability
	int iSize = vecSignal.GetSize();
	int iLengthDiff = fft_test_length(iSize);
	// Adjust length as needed
	iSize -= iLengthDiff;
	vecSignal.SetSize(iSize);

	// Pad response with zeroes if shorter than signal
	vecResponse.SetSize(iSize);

	// Wrap/Normalize response if requested
	if(bWrap) fft_wrap_response(vecResponse);
	if(bNormalize) fft_normalize_response(vecResponse);

	// Call NAG function(s) to perform deconvolution
	// Perform FFT on signal and response
	if((iRet = nag_fft_real(iSize, vecSignal)) != 0) return iRet;
	if((iRet = nag_fft_real(iSize, vecResponse)) != 0) return iRet;

	//Divide FFT of signal by FFT of response to deconvolve of a Hermitian sequence
	vecResult.SetSize(iSize);
	vecResult=0;

	int iSize2=(iSize-1)/2;

	double temp=vecResponse[0];
	if(temp < 1e-15)
	{
		vecResult[0] = 0.0;
	}
	else
	{
		vecResult[0]= vecSignal[0] / vecResponse[0]  / sqrt(iSize * 1.0);
	}

	for(int i=1; i <= iSize2; i++)
	{
		int j=iSize-i;

		// Compute power of FFT of response at current index
		double temp1=pow(vecResponse[i], 2) + pow(vecResponse[j], 2);

		// Check if power is too small
		if(temp1 < 1e-30)
		{
			vecResult[i] = 0.0;
			vecResult[j] = 0.0;
		}
		else
		{
			vecResult[i]= (vecSignal[i] * vecResponse[i] + vecSignal[j] * vecResponse[j])
		                  / temp1  / sqrt(iSize * 1.0);
		    vecResult[j]= (vecSignal[j] * vecResponse[i] - vecSignal[i] * vecResponse[j])
		                  / temp1  / sqrt(iSize * 1.0);
		}
	}

	if(iSize%2 ==0)
	{
		int k=iSize2+1;
		double temp=vecResponse[k];
		if(temp < 1e-15)
		{
			vecResult[k] = 0.0;
		}
		else
		{
			vecResult[k]= vecSignal[k] / vecResponse[k]  / sqrt(iSize * 1.0);
		}
	}

	// Perform an inverse FFT using nag_conjugate_hermitian and nag_fft_hermitian
	if( (iRet = nag_conjugate_hermitian(iSize, vecResult)) != 0) return iRet;
	if( (iRet = nag_fft_hermitian(iSize, vecResult)) != 0) return iRet;

	// Return 0 error code on successful completion
	return 0;
}

/**
*/
int fft_correlate(vector& vecSignal1, vector& vecSignal2, vector& vecCorrelation, bool bNormalize)
{
	int iRet;
	
	// Get size of signal vectors
	int n = vecSignal1.GetSize();

	// Declare matrices and plug signal vectors into them
	matrix<complex> matFFT1, matFFT2;
	matFFT1.SetSize(1,n);
	matFFT2.SetSize(1,n);
	matFFT1.SetByVector(vecSignal1);
	matFFT2.SetByVector(vecSignal2);
	
	// Perform FFT of both signals
	if( (iRet = FFT(matFFT1, matFFT1)) != 0) return iRet;
	if( (iRet = FFT(matFFT2, matFFT2)) != 0) return iRet;
	
	// Get conjugate of 2nd FFT and multiply with first one
	matrix<complex> matResult;
	matResult = matFFT1;
	matFFT2.Conjugate();
	matResult.DotMultiply(matFFT2);
	
	// Perform inverse FFT and shift result
	if( (iRet = IFFT(matResult, matResult)) != 0) return iRet;
	matResult.IFFTShift(matResult);
	
	// Get real part of IFFT result 
	matrix<double> matReal;
	matResult.GetReal(matReal);
	matReal.GetRow(vecCorrelation, 0);


	// Normalize result to +/-1 if asked for
	if(bNormalize)
	{
		// Compute normalization factor
		double dNorm1 = 0;
		double dNorm2 = 0;
		for(int ii = 0; ii < n; ii++)
		{
			complex c1, c2;
			c1 = matFFT1[0][ii];
			c2 = matFFT2[0][ii];
			dNorm1 += cabs(c1) * cabs(c1);
			dNorm2 += cabs(c2) * cabs(c2);
		}
		double dNorm = sqrt(dNorm1 * dNorm2);
	
		vecCorrelation = vecCorrelation * sqrt(n) / dNorm;	// sqrt(n) takes care of NAG normalization
	}
	
	return 0;
}



/**
*/
int fft_lowpass(Curve& crvSignal, double dFc)
{
	// Check the validity of the curve
	if(!crvSignal.IsValid())
		return -2;

	// If the frequency cutoff is negative, return error code -3
	if(dFc<0)
	{
		return -3;
	}

	int np=crvSignal.GetSize();   //the number of points in the original data
	int nRet;

	// Check the size of the signal
	if(np==0)
	{
		return 0;
	}
	else if(np==1)
	{
		if(dFc>0)
			crvSignal=0;
		return 0;
	}

	// Check the time resolution
	double dFreqStep;
	if(!fft_sampling_resolution(crvSignal, dFreqStep))
		return -1;

	// Find where to cut off in the frequency spectrum
	int nFc=dFc/dFreqStep+1;

	// If nFc is larger than the maximum of frequency, no frequency component would be cut
	if(nFc>np/2)
	{
		return 0;
	}

	// Calculate the spectra by FFT
	vector<complex> vecSignal(np), vecFFT(np);
	vecSignal=crvSignal;

	nRet=FFT(vecSignal, vecFFT);
	if(nRet!=0)
		return nRet;

	// Set all frequencies in cut-off region to zero
	// FFT has wrapped around results, so need to cut off from both ends
	for(int i=nFc; i<=np/2; i++)
	{
		vecFFT[i]=0.0;
		vecFFT[np-i]=0.0;
	}

	// Compute inverse FFT, extract real part, and place in signal curve
	nRet = IFFT(vecFFT, vecFFT);
	if( nRet !=0 )
		return nRet;

	vector vecReal(np);
	vecFFT.GetReal(vecReal);
	crvSignal = vecReal;

	return 0;
}


/**
*/
int fft_highpass(Curve& crvSignal, double dFc, bool bAddOffset)
{
	// Check the validity of the curve
	if(!crvSignal.IsValid())
		return -2;

	int np=crvSignal.GetSize();   //the number of points in the original data
	int nRet;

	// If the frequency cutoff is negative, return error code -3
	if(dFc<0)
		return -3;

	// If the frequency cutoff is zero, no cut off
	if(fabs(dFc)<1e-15)
		return 0;

	// Check the size of the signal
	if(np==0)
		return 0;

	if(np==1)
	{
		if(!bAddOffset)
			crvSignal=0;
		return 0;
	}

	// Check the time resolution
	double dFreqStep;
	if(!fft_sampling_resolution(crvSignal, dFreqStep))
		return -1;

	// Find where to cut off in the frequency spectrum
	int nFc=dFc/dFreqStep ;

	if(nFc>np/2)
	{
		nFc=np/2;
	}

	// Calculate the spectra by FFT
	vector<complex> vecSignal(np), vecFFT(np);
	vecSignal=crvSignal;

	nRet=FFT(vecSignal, vecFFT);
	if(nRet!=0)
		return nRet;

	// Set all frequencies in cut-off region to zero
	// FFT has wrapped around results, so need to cut off from both ends
	if(nFc>0)
	{
		for(int i=1; i<=nFc; i++)
		{
			vecFFT[i]=0.0;
			vecFFT[np-i]=0.0;
		}
	}
	if(!bAddOffset)
		vecFFT[0]=0.0;


	// Compute inverse FFT, extract real part, and place in signal curve
	nRet = IFFT(vecFFT, vecFFT);
	if( nRet !=0 )
		return nRet;

	vector vecReal(np);
	vecFFT.GetReal(vecReal);
	crvSignal = vecReal;

	return 0;
}


/**
*/
int fft_bandpass(Curve& crvSignal, double dFLow, double dFHigh, bool bAddOffset)
{
	// Check the validity of the curve
	if(!crvSignal.IsValid())
		return -2;

	int nRet;

	// Check the input cutoff frequency
	if((dFLow<0) || (dFHigh<0) || (dFLow>dFHigh))
		return -3;

	// Check the size of the signal
	int np=crvSignal.GetSize();   //the number of points in the original data
	if(np==0)
		return 0;
	if(np==1)
	{
		if((dFLow>0)&& !bAddOffset)
			crvSignal=0;
		return 0;
	}

	// Check the time resolution
	double dFreqStep;
	if(!fft_sampling_resolution(crvSignal, dFreqStep))
		return -1;

	// Find where to cut off in the frequency spectrum
	int nFLow=dFLow/dFreqStep;
	int nFHigh=dFHigh/dFreqStep+1;

	if(nFLow>np/2)
	{
		nFLow=np/2;
	}

	// Calculate the spectra by FFT
	vector<complex> vecSignal(np), vecFFT(np);
	vecSignal=crvSignal;

	nRet=FFT(vecSignal, vecFFT);
	if(nRet!=0)
		return nRet;

	// Set all frequencies in cut-off region to zero
	// FFT has wrapped around results, so need to cut off from both ends
	if(nFLow>0)
	{
		for(int i=1;i<=nFLow; i++)
		{
			vecFFT[i]=0.0;
			vecFFT[np-i]=0.0;
		}

		if(!bAddOffset)
			vecFFT[0]=0.0;
	}
	else if(!bAddOffset)
	{
		vecFFT[0]=0.0;
	}

	if(nFHigh<=np/2)
	{
		for(int i=nFHigh; i<=np/2; i++)
		{
			vecFFT[i]=0.0;
			vecFFT[np-i]=0.0;
		}
	}

	// Compute inverse FFT, extract real part, and place in signal curve
	nRet = IFFT(vecFFT, vecFFT);
	if( nRet !=0 )
		return nRet;

	vector vecReal(np);
	vecFFT.GetReal(vecReal);
	crvSignal = vecReal;

	return 0;
}


/**
*/
int fft_bandblock(Curve& crvSignal, double dFLow, double dFHigh, bool bAddOffset)
{
	// Check the validity of the curve
	if(!crvSignal.IsValid())
		return -2;

	int np=crvSignal.GetSize();   //the number of points in the original data
	int nRet;

	// Check the input cutoff frequency
	if( dFLow>dFHigh ||dFHigh<0 || dFLow<0)
		return -3;

	// Check the size of the signal
	if(np==0)
		return 0;
	if(np==1)
	{
		if(dFLow<=0 && !bAddOffset)
		{
			crvSignal=0;
		}
		return 0;
	}

	// Check the time resolution
	double dFreqStep;
	if(!fft_sampling_resolution(crvSignal, dFreqStep))
		return -1;

	// Find where to cut off in the frequency spectrum
	int nFLow=dFLow/dFreqStep+1;
	int nFHigh=dFHigh/dFreqStep;

	// Calculate the spectra by FFT
	vector<complex> vecSignal(np), vecFFT(np);
	vecSignal=crvSignal;

	nRet=FFT(vecSignal, vecFFT);
	if(nRet!=0)
		return nRet;

	// Set all frequencies in cut-off region to zero
	// FFT has wrapped around results, so need to cut off from both ends
	if(nFHigh>np/2)
	{
		nFHigh=np/2;
	}
	if(nFLow>nFHigh)
		return 0;

	for(int i=nFLow;i<=nFHigh;i++)
	{
		vecFFT[i] = 0.0;
		vecFFT[np-i] = 0.0;
	}

	// Compute inverse FFT, extract real part, and place in signal curve
	nRet = IFFT(vecFFT, vecFFT);
	if( nRet !=0 )
		return nRet;

	vector vecReal(np);
	vecFFT.GetReal(vecReal);
	crvSignal = vecReal;

	return 0;
}


/**
*/
int fft_threshold_filter(Curve& crvSignal, double dThreshold)
{
	// Check the validity of the curve
	if(!crvSignal.IsValid())
		return -2;

	int np=crvSignal.GetSize();   //the number of points in the original data
	int nRet;

	// Check the size of the signal
	if(np==0)
		return 0;

	// Check the threshold
	if(dThreshold<0)
		return -3;

	// Check the time resolution
	double dFreqStep;
	if(!fft_sampling_resolution(crvSignal, dFreqStep))
		return -1;

	// Calculate the spectra by FFT
	vector<complex> vecSignal(np), vecFFT(np);
	vecSignal=crvSignal;

	nRet=FFT(vecSignal, vecFFT);
	if(nRet!=0)
		return nRet;

	// Discard the components below dThreshold.
	double dAmp;
	for(int i=1;i<=np/2;i++)
	{
		dAmp=2*cabs(vecFFT[i])/sqrt(np);
		if(dAmp<dThreshold)
		{
			vecFFT[i]=0.0;
			vecFFT[np-i]=0.0;
		}
	}

	if(cabs(vecFFT[0])/sqrt(np)<dThreshold)
		vecFFT[0]=0.0;

	// Compute inverse FFT, extract real part, and place in signal curve
	nRet = IFFT(vecFFT, vecFFT);
	if( nRet !=0 )
		return nRet;

	vector vecReal(np);
	vecFFT.GetReal(vecReal);
	crvSignal = vecReal;

	return 0;
}


////////////////////////////////////////////////////////////////////////////////////
// This function checks the length of signal for NAG FFT call compatibility
//
static int fft_test_length(int iSize)
{
	// Check if length is a product of primes less than or equal to 19
	int iRet = fft_test_product_of_primes(iSize);
	if(iRet == 0) return 0;
	// If no, then sucessively decrement length till suitable length is found
	int iSizeNew = iSize;
	do
	{
		iSizeNew--;
		iRet = fft_test_product_of_primes(iSizeNew);
	} while (iRet != 0);
	return (iSize - iSizeNew);
}


////////////////////////////////////////////////////////////////////////////////////
// This function checks if a number is a product of primes less than or equal to 19
// and further that the number of elements in the product is not more than 20, which
// are NAG call requirements
//
static int fft_test_product_of_primes(int iNum)
{
	vector<int> iPrimes = {2,3,5,7,11,13,17,19};
	int iNum2, iQuotient, iRemainder;
	
	int ii = 0;
	int jj = 0;
	iNum2 = iNum;
	do
	{
		do
		{
			iQuotient = iNum2 / iPrimes[ii];
			iRemainder = iNum2 - iPrimes[ii] * iQuotient;
			if(iRemainder) break;
			if(++jj > 20) return 1;
			if(iQuotient == 1) return 0;
			iNum2 = iQuotient;
		} while(1);
		ii++;		
	} while (ii < 8);
	return 1;
}


////////////////////////////////////////////////////////////////////////////////////
// This function performs wrapping of the response function
//
static void fft_wrap_response(vector& vecResponse)
{
	// Find position of max element in response
	Dataset ds(vecResponse);
	BasicStats	stats;
	Data_sum(&ds, &stats);
	int iMax = stats.iMax;

	// Take response and "wrap it around" so that the point with the max value
	// is now the first point in the response dataset.
	if(iMax != 0)
	{
		int iSize = vecResponse.GetSize();
		vector vecTemp;
		vecTemp.SetSize(iSize);
		for (int ii = iMax; ii < iSize; ii++)
		{
			vecTemp[ii - iMax] = vecResponse[ii];
		}	
		for (ii = 0; ii < iMax; ii++)
		{
			vecTemp[iSize - iMax + ii] = vecResponse[ii];
		}
		vecResponse = vecTemp;
	}
}


////////////////////////////////////////////////////////////////////////////////////
// This function performs normalization of the response function
//
static void fft_normalize_response(vector& vecResponse)
{
	// Find the sum of all response elements
	Dataset ds(vecResponse);
	BasicStats	stats;
	Data_sum(&ds, &stats);
	double dSumResp = stats.total;

	// Normalize response so that sum of all elements is unity
	vecResponse /= dSumResp; 
}


////////////////////////////////////////////////////////////////////////////////////
// This function checks sampling resolution of time vector for FFT operations
//
static bool fft_sampling_resolution(Curve& crvSignal, double& dFreqStep)
{
	// Get the size of crvSignal
	int np=crvSignal.GetSize();
	if(np<2)
	{
		return false;
	}

	// Compute frequency scale for curve data
	// Get X dataset of the signal curve
	Dataset dsTime;
	if(crvSignal.HasX())   // If the curve has x column
	{
		if(!crvSignal.AttachX(dsTime)) return false;  //fail to attach x column

		int iSize=dsTime.GetSize();
		if(iSize!=np)
		{
			return false;
		}
	}
	else  // the curve does not have x column, use 1 as the sampling rate
	{
		dFreqStep=1.0/np;
		return true;
	}

	// Check the time resolution of crvSignal
	double dDeltaLo, dDeltaHi, dDeltaCur;

	// Obtain the interval of dsTime
	double dTimeStep = dsTime[1] - dsTime[0];     // Get Interval between first two elements
	if(fabs(dTimeStep)<1e-20)
		return false;

	if( dTimeStep < 0 )                            // If dDelta is negative...
	{
		dDeltaLo = 1.05 * dTimeStep;     // Compute lower absolute dDelta from relative tolerance
		dDeltaHi = 0.95 * dTimeStep;     // Compute higher absolute dDelta from relative tolerance
	}
	else                                        // Else if dDelta is not negative...
	{
		dDeltaLo = 0.95 * dTimeStep;     // Compute lower absolute dDelta from relative tolerance
		dDeltaHi = 1.05 * dTimeStep;     // Compute higher absolute dDelta from relative tolerance
	}


	for( int i = 1; i < np; i++ )           // For each element in specified range of vector...
	{
		dDeltaCur = dsTime[i] - dsTime[i-1];    // Get absolute delta between current and next element
		if( dDeltaCur < dDeltaLo || dDeltaCur > dDeltaHi ) // If current absolute delta is not within tolerance...
			return false;                       // Elements of vector not uniformly spaced
	}

	// Frequency step is 1/(numpts * time step)
	dFreqStep=1.0/np/dTimeStep;

	return true;
}


