/*****************************************************************************

        FFTReal.hpp

        Copyright (c) 2005 Laurent de Soras

--- Legal stuff ---

This library is free software; you can redistribute it and/or

modify it under the terms of the GNU Lesser General Public

License as published by the Free Software Foundation; either

version 2.1 of the License, or (at your option) any later version.

This library 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

Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public

License along with this library; if not, write to the Free Software

Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

*Tab=3***********************************************************************/

#if defined (FFTReal_CURRENT_CODEHEADER)

	#error Recursive inclusion of FFTReal code header.

#endif

#define	FFTReal_CURRENT_CODEHEADER

#if ! defined (FFTReal_CODEHEADER_INCLUDED)

#define	FFTReal_CODEHEADER_INCLUDED

/*\\\ INCLUDE FILES \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*/

#include	<cassert>

#include	<cmath>

static inline bool	FFTReal_is_pow2 (long x)

{

	assert (x > 0);

	return  ((x & -x) == x);

}

static inline int	FFTReal_get_next_pow2 (long x)

{

	--x;

	int				p = 0;

	while ((x & ~0xFFFFL) != 0)

	{

		p += 16;

		x >>= 16;

	}

	while ((x & ~0xFL) != 0)

	{

		p += 4;

		x >>= 4;

	}

	while (x > 0)

	{

		++p;

		x >>= 1;

	}

	return (p);

}

/*\\\ PUBLIC \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*/

/*

==============================================================================

Name: ctor

Input parameters:

	- length: length of the array on which we want to do a FFT. Range: power of

		2 only, > 0.

Throws: std::bad_alloc

==============================================================================

*/

template <class DT>

FFTReal <DT>::FFTReal (long length)

:	_length (length)

,	_nbr_bits (FFTReal_get_next_pow2 (length))

,	_br_lut ()

,	_trigo_lut ()

,	_buffer (length)

,	_trigo_osc ()

{

	assert (FFTReal_is_pow2 (length));

	assert (_nbr_bits <= MAX_BIT_DEPTH);

	init_br_lut ();

	init_trigo_lut ();

	init_trigo_osc ();

}

/*

==============================================================================

Name: get_length

Description:

	Returns the number of points processed by this FFT object.

Returns: The number of points, power of 2, > 0.

Throws: Nothing

==============================================================================

*/

template <class DT>

long	FFTReal <DT>::get_length () const

{

	return (_length);

}

/*

==============================================================================

Name: do_fft

Description:

	Compute the FFT of the array.

Input parameters:

	- x: pointer on the source array (time).

Output parameters:

	- f: pointer on the destination array (frequencies).

		f [0...length(x)/2] = real values,

		f [length(x)/2+1...length(x)-1] = negative imaginary values of

		coefficents 1...length(x)/2-1.

Throws: Nothing

==============================================================================

*/

template <class DT>

void	FFTReal <DT>::do_fft (DataType f [], const DataType x []) const

{

	assert (f != 0);

	assert (f != use_buffer ());

	assert (x != 0);

	assert (x != use_buffer ());

	assert (x != f);

	// General case

	if (_nbr_bits > 2)

	{

		compute_fft_general (f, x);

	}

	// 4-point FFT

	else if (_nbr_bits == 2)

	{

		f [1] = x [0] - x [2];

		f [3] = x [1] - x [3];

		const DataType	b_0 = x [0] + x [2];

		const DataType	b_2 = x [1] + x [3];

		f [0] = b_0 + b_2;

		f [2] = b_0 - b_2;

	}

	// 2-point FFT

	else if (_nbr_bits == 1)

	{

		f [0] = x [0] + x [1];

		f [1] = x [0] - x [1];

	}

	// 1-point FFT

	else

	{

		f [0] = x [0];

	}

}

/*

==============================================================================

Name: do_ifft

Description:

	Compute the inverse FFT of the array. Note that data must be post-scaled:

	IFFT (FFT (x)) = x * length (x).

Input parameters:

	- f: pointer on the source array (frequencies).

		f [0...length(x)/2] = real values

		f [length(x)/2+1...length(x)-1] = negative imaginary values of

		coefficents 1...length(x)/2-1.

Output parameters:

	- x: pointer on the destination array (time).

Throws: Nothing

==============================================================================

*/

template <class DT>

void	FFTReal <DT>::do_ifft (const DataType f [], DataType x []) const

{

	assert (f != 0);

	assert (f != use_buffer ());

	assert (x != 0);

	assert (x != use_buffer ());

	assert (x != f);

	// General case

	if (_nbr_bits > 2)

	{

		compute_ifft_general (f, x);

	}

	// 4-point IFFT

	else if (_nbr_bits == 2)

	{

		const DataType	b_0 = f [0] + f [2];

		const DataType	b_2 = f [0] - f [2];

		x [0] = b_0 + f [1] * 2;

		x [2] = b_0 - f [1] * 2;

		x [1] = b_2 + f [3] * 2;

		x [3] = b_2 - f [3] * 2;

	}

	// 2-point IFFT

	else if (_nbr_bits == 1)

	{

		x [0] = f [0] + f [1];

		x [1] = f [0] - f [1];

	}

	// 1-point IFFT

	else

	{

		x [0] = f [0];

	}

}

/*

==============================================================================

Name: rescale

Description:

	Scale an array by divide each element by its length. This function should

	be called after FFT + IFFT.

Input parameters:

	- x: pointer on array to rescale (time or frequency).

Throws: Nothing

==============================================================================

*/

template <class DT>

void	FFTReal <DT>::rescale (DataType x []) const

{

	const DataType	mul = DataType (1.0 / _length);

	if (_length < 4)

	{

		long				i = _length - 1;

		do

		{

			x [i] *= mul;

			--i;

		}

		while (i >= 0);

	}

	else

	{

		assert ((_length & 3) == 0);

		// Could be optimized with SIMD instruction sets (needs alignment check)

		long				i = _length - 4;

		do

		{

			x [i + 0] *= mul;

			x [i + 1] *= mul;

			x [i + 2] *= mul;

			x [i + 3] *= mul;

			i -= 4;

		}

		while (i >= 0);

	}

}

/*

==============================================================================

Name: use_buffer

Description:

	Access the internal buffer, whose length is the FFT one.

	Buffer content will be erased at each do_fft() / do_ifft() call!

	This buffer cannot be used as:

		- source for FFT or IFFT done with this object

		- destination for FFT or IFFT done with this object

Returns:

	Buffer start address

Throws: Nothing

==============================================================================

*/

template <class DT>

typename FFTReal <DT>::DataType *	FFTReal <DT>::use_buffer () const

{

	return (&_buffer [0]);

}

/*\\\ PROTECTED \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*/

/*\\\ PRIVATE \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*/

template <class DT>

void	FFTReal <DT>::init_br_lut ()

{

	const long		length = 1L << _nbr_bits;

	_br_lut.resize (length);

	_br_lut [0] = 0;

	long				br_index = 0;

	for (long cnt = 1; cnt < length; ++cnt)

	{

		// ++br_index (bit reversed)

		long				bit = length >> 1;

		while (((br_index ^= bit) & bit) == 0)

		{

			bit >>= 1;

		}

		_br_lut [cnt] = br_index;

	}

}

template <class DT>

void	FFTReal <DT>::init_trigo_lut ()

{

	using namespace std;

	if (_nbr_bits > 3)

	{

		const long		total_len = (1L << (_nbr_bits - 1)) - 4;

		_trigo_lut.resize (total_len);

		for (int level = 3; level < _nbr_bits; ++level)

		{

			const long		level_len = 1L << (level - 1);

			DataType	* const	level_ptr =

				&_trigo_lut [get_trigo_level_index (level)];

			const double	mul = PI / (level_len << 1);

			for (long i = 0; i < level_len; ++ i)

			{

				level_ptr [i] = static_cast <DataType> (cos (i * mul));

			}

		}

	}

}

template <class DT>

void	FFTReal <DT>::init_trigo_osc ()

{

	const int		nbr_osc = _nbr_bits - TRIGO_BD_LIMIT;

	if (nbr_osc > 0)

	{

		_trigo_osc.resize (nbr_osc);

		for (int osc_cnt = 0; osc_cnt < nbr_osc; ++osc_cnt)

		{

			OscType &		osc = _trigo_osc [osc_cnt];

			const long		len = 1L << (TRIGO_BD_LIMIT + osc_cnt);

			const double	mul = (0.5 * PI) / len;

			osc.set_step (mul);

		}

	}

}

template <class DT>

const long *	FFTReal <DT>::get_br_ptr () const

{

	return (&_br_lut [0]);

}

template <class DT>

const typename FFTReal <DT>::DataType *	FFTReal <DT>::get_trigo_ptr (int level) const

{

	assert (level >= 3);

	return (&_trigo_lut [get_trigo_level_index (level)]);

}

template <class DT>

long	FFTReal <DT>::get_trigo_level_index (int level) const

{

	assert (level >= 3);

	return ((1L << (level - 1)) - 4);

}

// Transform in several passes

template <class DT>

void	FFTReal <DT>::compute_fft_general (DataType f [], const DataType x []) const

{

	assert (f != 0);

	assert (f != use_buffer ());

	assert (x != 0);

	assert (x != use_buffer ());

	assert (x != f);

	DataType *		sf;

	DataType *		df;

	if ((_nbr_bits & 1) != 0)

	{

		df = use_buffer ();

		sf = f;

	}

	else

	{

		df = f;

		sf = use_buffer ();

	}

	compute_direct_pass_1_2 (df, x);

	compute_direct_pass_3 (sf, df);

	for (int pass = 3; pass < _nbr_bits; ++ pass)

	{

		compute_direct_pass_n (df, sf, pass);

		DataType * const	temp_ptr = df;

		df = sf;

		sf = temp_ptr;

	}

}

template <class DT>

void	FFTReal <DT>::compute_direct_pass_1_2 (DataType df [], const DataType x []) const

{

	assert (df != 0);

	assert (x != 0);

	assert (df != x);

	const long * const	bit_rev_lut_ptr = get_br_ptr ();

	long				coef_index = 0;

	do

	{

		const long		rev_index_0 = bit_rev_lut_ptr [coef_index];

		const long		rev_index_1 = bit_rev_lut_ptr [coef_index + 1];

		const long		rev_index_2 = bit_rev_lut_ptr [coef_index + 2];

		const long		rev_index_3 = bit_rev_lut_ptr [coef_index + 3];

		DataType	* const	df2 = df + coef_index;

		df2 [1] = x [rev_index_0] - x [rev_index_1];

		df2 [3] = x [rev_index_2] - x [rev_index_3];

		const DataType	sf_0 = x [rev_index_0] + x [rev_index_1];

		const DataType	sf_2 = x [rev_index_2] + x [rev_index_3];

		df2 [0] = sf_0 + sf_2;

		df2 [2] = sf_0 - sf_2;

		coef_index += 4;

	}

	while (coef_index < _length);

}

template <class DT>

void	FFTReal <DT>::compute_direct_pass_3 (DataType df [], const DataType sf []) const

{

	assert (df != 0);

	assert (sf != 0);

	assert (df != sf);

	const DataType	sqrt2_2 = DataType (SQRT2 * 0.5);

	long				coef_index = 0;

	do

	{

		DataType			v;

		df [coef_index] = sf [coef_index] + sf [coef_index + 4];

		df [coef_index + 4] = sf [coef_index] - sf [coef_index + 4];

		df [coef_index + 2] = sf [coef_index + 2];

		df [coef_index + 6] = sf [coef_index + 6];

		v = (sf [coef_index + 5] - sf [coef_index + 7]) * sqrt2_2;

		df [coef_index + 1] = sf [coef_index + 1] + v;

		df [coef_index + 3] = sf [coef_index + 1] - v;

		v = (sf [coef_index + 5] + sf [coef_index + 7]) * sqrt2_2;

		df [coef_index + 5] = v + sf [coef_index + 3];

		df [coef_index + 7] = v - sf [coef_index + 3];

		coef_index += 8;

	}

	while (coef_index < _length);

}