#include "stm32f10x.h"

#include <string.h>
#include "fft.h"




/**
 * Complex FFT samples input.
 */
    u32 fft_output[FFT_LENGTH];

/**
 * Compute a real FFT with real or complex values. Use the ST DSP
 * library for STM32.
 * @param samples [in], the pointer to the samples in time domain
 * @param size, the size of the sample buffer
 * @return a pointer to the FFT output (real numbers only)
 */
u32* compute_fft(u32* samples)
{

	// Performs the FFT with the ST DSP library
	#if FFT_LENGTH == 64
		cr4_fft_64_stm32(fft_output, samples, FFT_LENGTH);
	#elif FFT_LENGTH == 256
		cr4_fft_256_stm32(fft_output, samples, FFT_LENGTH);
	#elif FFT_LENGTH == 1024
		cr4_fft_1024_stm32(fft_output, samples, FFT_LENGTH);
	#else
		#error FFT is not developped for this FFT_LENGTH samples !
	#endif

	// Convert the complex values to real values
	u16 i;
	for(i=0;i<FFT_LENGTH/2;i++)
	{
		fft_output[i] = abs((s16)(fft_output[i]>>16)) + abs((s16)(0x0000FFFF&fft_output[i]));
	}

	// Returns the converted FFT output buffer
	return fft_output;
}

/**
 * Find the fundamental frequency in a spectrum.
 * @param spectrum [in], the pointer to the spectrum
 * @param sampling_rate, the sampling rate used for the spectrum
 * @return the fundamental frequency
 */
u16 fundamental_frequency(u32* sprectrum, u16 sampling_rate)
{
	// Find the maximal value
	u32 max_value = 0;
	u16 max_pos = 0;
	u16 i;
	for(i=0;i<FFT_LENGTH/2;i++)
	{
		if(sprectrum[i] > max_value)
		{
			max_value = sprectrum[i];
			max_pos = i;
		}
	}

	// Convert the spectrum index to a frequency
	return max_pos*sampling_rate/FFT_LENGTH;
}


/**
 * Check if a frequency is in the valid range.
 * @param frequency, the frequency to check
 * @return TRUE if the frequency is valid, FALSE else
 */
bool is_valid(u16 frequency)
{
	if(frequency>LOWBAND_LIMIT && frequency<HIGHBAND_LIMIT) return TRUE;
	else return FALSE;
}

/**
 * FIFO array for the moving average.
 */
u16 average_fifo[AVERAGE_LENGTH];

/**
 * FIFO head pointer for the moving average.
 */
u8 fifo_pointer;

/**
 * Sum of the last value to compute the moving average.
 */
u32 sum;

/**
 * Counter for successive outband values.
 */
u8 outband_count = 0;

/**
 * Init the moving average FIFO and sum to zero.
 * @return nothing
 */
void init_average()
{
	sum = 0;
	fifo_pointer = 0;
	memset(average_fifo, 0, AVERAGE_LENGTH*sizeof(u16));
}

/**
 * Comptue a moving average on inputed values. If values are outside
 * considered band they are ignored (considered as 'outband').
 * @param value, the value to average
 * @return the moving averaged value
 */
u16 moving_average(u16 value)
{
	// If the value is in the valid band
	if(is_valid(value)) 
	{
		// Compute the average
		sum -= average_fifo[fifo_pointer];
		sum += value;
		average_fifo[fifo_pointer] = value;
		fifo_pointer++;
		fifo_pointer = fifo_pointer%AVERAGE_LENGTH;

		// Decrease the outband counter
		if(outband_count>0) outband_count--;
	}

	// Else the value is outband, increase the counter
	else
	{
		if(outband_count<OUTBAND_TIMEOUT) outband_count++;
	}

	// If the outband timer is not reached return the average value
	if(outband_count<OUTBAND_TIMEOUT)
	{
		return sum/AVERAGE_LENGTH;
	}

	// Reset the moving average and return zero
	else
	{
		init_average();
		return 0;
	}
}
