#include "fft.h"
#include "main.h"
#include "arm_const_structs.h"
#include "audio_filters.h"
#include "screen_layout.h"
#include "lcd.h"

//Public variables
bool FFT_need_fft = true;					// need to prepare data for display on the screen
bool FFT_new_buffer_ready = false;				// buffer is full, can be processed
uint32_t FFT_buff_index = 0;		// current buffer index when it is filled with FPGA
bool FFT_buff_current = 0;		// current FFT Input buffer A - false, B - true
float32_t FFTInput_I_A[FFT_SIZE] = {0}; // incoming buffer FFT I
float32_t FFTInput_Q_A[FFT_SIZE] = {0}; // incoming buffer FFT Q
float32_t FFTInput_I_B[FFT_SIZE] = {0}; // incoming buffer FFT I
float32_t FFTInput_Q_B[FFT_SIZE] = {0}; // incoming buffer FFT Q
uint16_t FFT_FPS = 0;
uint16_t FFT_FPS_Last = 0;

//Private variables
#if FFT_SIZE == 1024
const static arm_cfft_instance_f32 *FFT_Inst = &arm_cfft_sR_f32_len1024;
#endif
#if FFT_SIZE == 512
const static arm_cfft_instance_f32 *FFT_Inst = &arm_cfft_sR_f32_len512;
#endif
#if FFT_SIZE == 256
const static arm_cfft_instance_f32 *FFT_Inst = &arm_cfft_sR_f32_len256;
#endif
#if FFT_SIZE == 128
const static arm_cfft_instance_f32 *FFT_Inst = &arm_cfft_sR_f32_len128;
#endif

static float32_t FFTInputCharge[FFT_DOUBLE_SIZE_BUFFER] = {0}; // charge FFT I and Q buffer
static float32_t FFTInput[FFT_DOUBLE_SIZE_BUFFER] = {0};   // combined FFT I and Q buffer
static float32_t FFTInput_tmp[LAY_FFT_PRINT_SIZE] = {0};	   // temporary buffer for sorting, moving and fft compressing
static float32_t FFTOutput_mean[LAY_FFT_PRINT_SIZE] = {0}; // averaged FFT buffer (for output)
static float32_t maxValueFFT_rx = 0;					   // maximum value of the amplitude in the resulting frequency response
static float32_t maxValueFFT_tx = 0;					   // maximum value of the amplitude in the resulting frequency response
static uint32_t currentFFTFreq = 0;
static uint32_t lastWTFFreq = 0;								//last WTF printed freq
static uint16_t color_scale[LAY_FFT_HEIGHT] = {0};							  // color gradient in height FFT
static uint16_t palette_fft[LAY_FFT_HEIGHT + 1] = {0};									 // color palette with FFT colors
static uint16_t palette_bg_gradient[LAY_FFT_HEIGHT + 1] = {0};							 // color palette with gradient background of FFT
static uint16_t palette_bw_fft_colors[LAY_FFT_HEIGHT + 1] = {0};						 // color palette with bw highlighted FFT colors
static uint16_t palette_bw_bg_colors[LAY_FFT_HEIGHT + 1] = {0};							 // color palette with bw highlighted background colors
IRAM1 static uint16_t fft_output_buffer[LAY_FFT_HEIGHT][LAY_FFT_PRINT_SIZE] = {{0}};	 //buffer with fft print data
IRAM1 __attribute__((aligned(32))) static uint8_t indexed_wtf_buffer[LAY_WTF_HEIGHT][LAY_FFT_PRINT_SIZE] = {{0}}; //indexed color buffer with wtf
static uint32_t wtf_buffer_freqs[LAY_WTF_HEIGHT] = {0};						 // frequencies for each row of the waterfall
IRAM1 static uint16_t wtf_output_line[LAY_FFT_PRINT_SIZE] = {0};						 // temporary buffer to draw the waterfall
static uint16_t fft_header[LAY_FFT_PRINT_SIZE] = {0};				//buffer with fft colors output
static int32_t grid_lines_pos[20] = {-1};										//grid lines positions
static int16_t bw_line_start = 0;													 //BW bar params
static int16_t bw_line_width = 0;													 //BW bar params
static int16_t bw_line_end = 0;														 //BW bar params
static int16_t bw_line_center = 0;														 //BW bar params
static uint16_t print_wtf_yindex = 0;												  // the current coordinate of the waterfall output via DMA
static float32_t window_multipliers[FFT_SIZE] = {0};								  // coefficients of the selected window function
static uint16_t bandmap_line_tmp[LAY_FFT_PRINT_SIZE] = {0};					  // temporary buffer to move the waterfall
static arm_sort_instance_f32 FFT_sortInstance = {0};								  // sorting instance (to find the median)
static uint32_t print_fft_dma_estimated_size = 0;			//block size for dma
static uint32_t print_fft_dma_position = 0;			//positior for dma fft print
// Decimator for Zoom FFT
static arm_fir_decimate_instance_f32 DECIMATE_ZOOM_FFT_I;
static arm_fir_decimate_instance_f32 DECIMATE_ZOOM_FFT_Q;
static float32_t decimZoomFFTIState[FFT_SIZE + 4 - 1];
static float32_t decimZoomFFTQState[FFT_SIZE + 4 - 1];
static uint_fast16_t zoomed_width = 0;
static uint16_t fft_peaks[LAY_FFT_PRINT_SIZE] = {0};		//buffer with fft peaks
//Коэффициенты для ZoomFFT lowpass filtering / дециматора
static arm_biquad_cascade_df2T_instance_f32 IIR_biquad_Zoom_FFT_I =
	{
		.numStages = ZOOMFFT_DECIM_STAGES,
		.pCoeffs = (float32_t *)(float32_t[ZOOMFFT_DECIM_STAGES * 5]){0},
		.pState = (float32_t *)(float32_t[ZOOMFFT_DECIM_STAGES * 2]){0}};
static arm_biquad_cascade_df2T_instance_f32 IIR_biquad_Zoom_FFT_Q =
	{
		.numStages = ZOOMFFT_DECIM_STAGES,
		.pCoeffs = (float32_t *)(float32_t[ZOOMFFT_DECIM_STAGES * 5]){0},
		.pState = (float32_t *)(float32_t[ZOOMFFT_DECIM_STAGES * 2]){0}};

static const float32_t *mag_coeffs[17] =
	{
		NULL, // 0
		NULL, // 1
		// 2x magnify, 24kHz, sample rate 96k, 60dB stopband
		(float32_t *)(const float32_t[ZOOMFFT_DECIM_STAGES * 5]){2.484242790213,0,0,0,0,1,1.898288195851,1,0.6974136319001,-0.3065583381015,0.02825209609289,0,0,0,0,1,1.500253906972,1,0.008112054577326,-0.7721086004364,1,0,0,0,0},
		NULL, // 3
		// 4x magnify, 12kHz, sample rate 96k, 60dB stopband
		(float32_t *)(const float32_t[ZOOMFFT_DECIM_STAGES * 5]){0.7134827863049,0,0,0,0,1,1.472005720002,1,1.415948015621,-0.5717655848516,0.01368406787431,0,0,0,0,1,0.1832282425444,1,1.300866126796,-0.8337859400983,1,0,0,0,0},
		NULL, // 5
		NULL, // 6
		NULL, // 7
		// 8x magnify, 6kHz, sample rate 96k, 60dB stopband
		(float32_t *)(const float32_t[ZOOMFFT_DECIM_STAGES * 5]){0.2759821831997,0,0,0,0,1,0.4104549042334,1,1.718681040914,-0.7605249789395,0.009291375667003,0,0,0,0,1,-1.132037961389,1,1.762711686353,-0.9065677781814,1,0,0,0,0},
		NULL, // 9
		NULL, // 10
		NULL, // 11
		NULL, // 12
		NULL, // 13
		NULL, // 14
		NULL, // 15
		// 16x magnify, 3kHz, sample rate 96k, 60dB stopband
		(float32_t *)(const float32_t[ZOOMFFT_DECIM_STAGES * 5]){0.1614831396677,0,0,0,0,1,-0.9158954187733,1,1.861713716539,-0.8727253274572,0.008184762202728,0,0,0,0,1,-1.745517115598,1,1.914110094101,-0.9512646565581,1,0,0,0,0},
};

static const arm_fir_decimate_instance_f32 FirZoomFFTDecimate[17] =
	{
		{0}, // 0
		{0}, // 1
		// 48ksps, 12kHz lowpass
		{
			.numTaps = 4,
			.pCoeffs = (float32_t *)(const float32_t[]){475.1179397144384210E-6f, 0.503905202786044337f, 0.503905202786044337f, 475.1179397144384210E-6f},
			.pState = NULL},
		{0}, // 3
		// 48ksps, 6kHz lowpass
		{
			.numTaps = 4,
			.pCoeffs = (float32_t *)(const float32_t[]){0.198273254218889416f, 0.298085149879260325f, 0.298085149879260325f, 0.198273254218889416f},
			.pState = NULL},
		{0}, // 5
		{0}, // 6
		{0}, // 7
		// 48ksps, 3kHz lowpass
		{
			.numTaps = 4,
			.pCoeffs = (float32_t *)(const float32_t[]){0.199820836596682871f, 0.272777397353925699f, 0.272777397353925699f, 0.199820836596682871f},
			.pState = NULL},
		{0}, // 9
		{0}, // 10
		{0}, // 11
		{0}, // 12
		{0}, // 13
		{0}, // 14
		{0}, // 15
		// 48ksps, 1.5kHz lowpass
		{
			.numTaps = 4,
			.pCoeffs = (float32_t *)(const float32_t[]){0.199820836596682871f, 0.272777397353925699f, 0.272777397353925699f, 0.199820836596682871f},
			.pState = NULL},
};

//Prototypes
static uint16_t getFFTColor(uint_fast8_t height);											  // get color from signal strength
static void FFT_fill_color_palette(void);													  // prepare the color palette
static void FFT_move(int32_t _freq_diff);													  // shift the waterfall
static int32_t getFreqPositionOnFFT(uint32_t freq);											  // get the position on the FFT for a given frequency
static uint32_t FFT_getLensCorrection(uint32_t normal_distance_from_center);
	
// FFT initialization
void FFT_PreInit(void)
{
	//Windowing
	//Dolph–Chebyshev
	if (TRX.FFT_Window == 1)
	{
		const float64_t atten = 100.0;
		float64_t max = 0.0;
		float64_t tg = pow(10.0, atten / 20.0);
		float64_t x0 = cosh((1.0 / ((float64_t)FFT_SIZE - 1.0)) * acosh(tg));
		float64_t M = (FFT_SIZE - 1) / 2;
		if((FFT_SIZE % 2) == 0) 
			M = M + 0.5; /* handle even length windows */
		for(uint32_t nn=0; nn < ((FFT_SIZE / 2) + 1); nn++)
		{
			float64_t n = nn - M;
			float64_t sum = 0.0;
			for(uint32_t i = 1; i <= M; i++)
			{
				float64_t cheby_poly = 0.0;
				float64_t cp_x = x0 * cos(F_PI * i / (float64_t)FFT_SIZE);
				float64_t cp_n = FFT_SIZE - 1;
				if (fabs(cp_x) <= 1) 
					cheby_poly = cos(cp_n * acos(cp_x));
				else 
					cheby_poly = cosh(cp_n * acosh(cp_x));
				
				sum += cheby_poly * cos(2.0 * n * F_PI * (float64_t)i / (float64_t)FFT_SIZE);
			}
			window_multipliers[nn] = tg + 2 * sum;
			window_multipliers[FFT_SIZE - nn - 1] = window_multipliers[nn];
			if(window_multipliers[nn] > max)
				max = window_multipliers[nn];
		}
		for(uint32_t nn=0; nn < FFT_SIZE; nn++) 
			window_multipliers[nn] /= max; /* normalise everything */
	}
	for (uint_fast16_t i = 0; i < FFT_SIZE; i++)
	{
		//Blackman-Harris
		if (TRX.FFT_Window == 2)
			window_multipliers[i] = 0.35875f - 0.48829f * arm_cos_f32(2.0f * F_PI * (float32_t)i / ((float32_t)FFT_SIZE - 1.0f)) + 0.14128f * arm_cos_f32(4.0f * F_PI * (float32_t)i / ((float32_t)FFT_SIZE - 1.0f)) - 0.01168f * arm_cos_f32(6.0f * F_PI * (float32_t)i / ((float32_t)FFT_SIZE - 1.0f));
		//Nutall
		else if (TRX.FFT_Window == 3)
			window_multipliers[i] = 0.355768f - 0.487396f * arm_cos_f32(2.0f * F_PI * (float32_t)i / ((float32_t)FFT_SIZE - 1.0f)) + 0.144232f * arm_cos_f32(4.0f * F_PI * (float32_t)i / ((float32_t)FFT_SIZE - 1.0f)) - 0.012604 * arm_cos_f32(6.0f * F_PI * (float32_t)i / ((float32_t)FFT_SIZE - 1.0f));
		//Blackman-Nutall
		else if (TRX.FFT_Window == 4)
			window_multipliers[i] = 0.3635819f - 0.4891775f * arm_cos_f32(2.0f * F_PI * (float32_t)i / ((float32_t)FFT_SIZE - 1.0f)) + 0.1365995f * arm_cos_f32(4.0f * F_PI * (float32_t)i / ((float32_t)FFT_SIZE - 1.0f)) - 0.0106411f * arm_cos_f32(6.0f * F_PI * (float32_t)i / ((float32_t)FFT_SIZE - 1.0f));
		//Hann
		else if (TRX.FFT_Window == 5)
			window_multipliers[i] = 0.5f * (1.0f - arm_cos_f32(2.0f * F_PI * (float32_t)i / (float32_t)FFT_SIZE));
		//Hamming
		else if (TRX.FFT_Window == 6)
			window_multipliers[i] = 0.54f - 0.46f * arm_cos_f32((2.0f * F_PI * (float32_t)i) / ((float32_t)FFT_SIZE - 1.0f));
		//No window
		else if (TRX.FFT_Window == 7)
			window_multipliers[i] = 1.0f;
	}
	
	// initialize sort
	arm_sort_init_f32(&FFT_sortInstance, ARM_SORT_HEAP, ARM_SORT_ASCENDING);
}

void FFT_Init(void)
{
	FFT_fill_color_palette();
	//ZoomFFT
	if (TRX.FFT_Zoom > 1)
	{
		IIR_biquad_Zoom_FFT_I.pCoeffs = mag_coeffs[TRX.FFT_Zoom];
		IIR_biquad_Zoom_FFT_Q.pCoeffs = mag_coeffs[TRX.FFT_Zoom];
		memset(IIR_biquad_Zoom_FFT_I.pState, 0x00, sizeof(float32_t) * ZOOMFFT_DECIM_STAGES * 2);
		memset(IIR_biquad_Zoom_FFT_Q.pState, 0x00, sizeof(float32_t) * ZOOMFFT_DECIM_STAGES * 2);
		arm_fir_decimate_init_f32(&DECIMATE_ZOOM_FFT_I,
								  FirZoomFFTDecimate[TRX.FFT_Zoom].numTaps,
								  TRX.FFT_Zoom, // Decimation factor
								  FirZoomFFTDecimate[TRX.FFT_Zoom].pCoeffs,
								  decimZoomFFTIState, // Filter state variables
								  FFT_SIZE);

		arm_fir_decimate_init_f32(&DECIMATE_ZOOM_FFT_Q,
								  FirZoomFFTDecimate[TRX.FFT_Zoom].numTaps,
								  TRX.FFT_Zoom, // Decimation factor
								  FirZoomFFTDecimate[TRX.FFT_Zoom].pCoeffs,
								  decimZoomFFTQState, // Filter state variables
								  FFT_SIZE);
		zoomed_width = FFT_SIZE / TRX.FFT_Zoom;
	}
	else
		zoomed_width = FFT_SIZE;
	
	// clear the buffers
	memset(&fft_output_buffer, 0x00, sizeof(fft_output_buffer));
	memset(&indexed_wtf_buffer, LAY_FFT_HEIGHT, sizeof(indexed_wtf_buffer));
	memset(&FFTInputCharge, 0x00, sizeof(FFTInputCharge));
}

// FFT calculation
void FFT_bufferPrepare(void)
{
	if (!TRX.FFT_Enabled)
		return;
	if (!FFT_new_buffer_ready)
		return;
	/*if (CPU_LOAD.Load > 90)
		return;*/
	
	float32_t* FFTInput_I_current = !FFT_buff_current ? (float32_t*)&FFTInput_I_A : (float32_t*)&FFTInput_I_B; //inverted
	float32_t* FFTInput_Q_current = !FFT_buff_current ? (float32_t*)&FFTInput_Q_A : (float32_t*)&FFTInput_Q_B;
	
	//Process DC corrector filter
	if (!TRX_on_TX())
	{
		dc_filter(FFTInput_I_current, FFT_SIZE, DC_FILTER_FFT_I);
		dc_filter(FFTInput_Q_current, FFT_SIZE, DC_FILTER_FFT_Q);
	}
	//-----------------------------------------------------------------------------------------------------------------------------------------
	//FFT Peaks
	if(TRX.FFT_HoldPeaks)
	{
		uint32_t fft_y_prev = 0;
		for (uint32_t fft_x = 0; fft_x < LAY_FFT_PRINT_SIZE; fft_x++)
		{
			uint32_t fft_y = LAY_WTF_HEIGHT - fft_peaks[fft_x];
			int32_t y_diff = (int32_t)fft_y - (int32_t)fft_y_prev;
			if (fft_x == 0 || (y_diff <= 1 && y_diff >= -1))
			{
				fft_output_buffer[fft_y][fft_x] = palette_fft[LAY_WTF_HEIGHT / 2];
			}
			else
			{
				for (uint32_t l = 0; l < (abs(y_diff / 2) + 1); l++) //draw line
				{
					fft_output_buffer[fft_y_prev + ((y_diff > 0) ? l : -l)][fft_x - 1] = palette_fft[LAY_WTF_HEIGHT / 2];
					fft_output_buffer[fft_y + ((y_diff > 0) ? -l : l)][fft_x] = palette_fft[LAY_WTF_HEIGHT / 2];
				}
			}
			fft_y_prev = fft_y;
		}
	}
//-----------------------------------------------------------------------------------------------------------------------------------------
	//FFT Peaks
	if(TRX.FFT_HoldPeaks)
	{
		if(lastWTFFreq == currentFFTFreq)
		{
			for (uint32_t fft_x = 0; fft_x < LAY_FFT_PRINT_SIZE; fft_x++)
				if(fft_peaks[fft_x] <= fft_header[fft_x])
					fft_peaks[fft_x] = fft_header[fft_x];
				else if(fft_peaks[fft_x] > 0)
					fft_peaks[fft_x]--;
		}
		else
		{
			for (uint32_t fft_x = 0; fft_x < LAY_FFT_PRINT_SIZE; fft_x++)
				fft_peaks[fft_x] = fft_header[fft_x];
		}
	}
	//-----------------------------------------------------------------------------------------------------------------------------------------
	
	//ZoomFFT
	if (TRX.FFT_Zoom > 1)
	{
		//Biquad LPF filter
		arm_biquad_cascade_df2T_f32(&IIR_biquad_Zoom_FFT_I, FFTInput_I_current, FFTInput_I_current, FFT_SIZE);
		arm_biquad_cascade_df2T_f32(&IIR_biquad_Zoom_FFT_Q, FFTInput_Q_current, FFTInput_Q_current, FFT_SIZE);
		// Decimator
		arm_fir_decimate_f32(&DECIMATE_ZOOM_FFT_I, FFTInput_I_current, FFTInput_I_current, FFT_SIZE);
		arm_fir_decimate_f32(&DECIMATE_ZOOM_FFT_Q, FFTInput_Q_current, FFTInput_Q_current, FFT_SIZE);
		// Shift old data
		memcpy(&FFTInputCharge[0], &FFTInputCharge[zoomed_width], sizeof(float32_t) * (FFT_SIZE - zoomed_width));
		// Add new data
		for (uint_fast16_t i = 0; i < zoomed_width; i++)
		{
			uint16_t wind_pos = i * (FFT_SIZE / zoomed_width);
			FFTInputCharge[(FFT_SIZE - zoomed_width + i) * 2] = FFTInput_I_current[i] * window_multipliers[wind_pos];
			FFTInputCharge[(FFT_SIZE - zoomed_width + i) * 2 + 1] = FFTInput_Q_current[i] * window_multipliers[wind_pos];
		}
	}
	else
	{
		// make a combined buffer for calculation
		for (uint_fast16_t i = 0; i < FFT_SIZE; i++)
		{
			FFTInputCharge[i * 2] = FFTInput_I_current[i] * window_multipliers[i];
			FFTInputCharge[i * 2 + 1] = FFTInput_Q_current[i] * window_multipliers[i];
		}
	}
	FFT_new_buffer_ready = false;
}

void FFT_doFFT(void)
{
	if (!TRX.FFT_Enabled)
		return;
	if (!FFT_need_fft)
		return;
	if (!TRX_Inited)
		return;
	/*if (CPU_LOAD.Load > 90)
		return;*/

	// Get charge buffer
	memcpy(&FFTInput, &FFTInputCharge, sizeof(FFTInput));
	memset(&FFTInputCharge, 0x00, sizeof(FFTInputCharge));

	arm_cfft_f32(FFT_Inst, FFTInput, 0, 1);
	arm_cmplx_mag_f32(FFTInput, FFTInput, FFT_SIZE);

	// Swap fft parts
	memcpy(&FFTInput[FFT_SIZE], &FFTInput[0], sizeof(float32_t) * (FFT_SIZE / 2)); //left - > tmp
	memcpy(&FFTInput[0], &FFTInput[FFT_SIZE / 2], sizeof(float32_t) * (FFT_SIZE / 2)); //right - > left
	memcpy(&FFTInput[FFT_SIZE / 2], &FFTInput[FFT_SIZE], sizeof(float32_t) * (FFT_SIZE / 2)); //tmp - > right
	
	// Compress the calculated FFT to visible
	memcpy(&FFTInput[0], &FFTInput[FFT_SIZE / 2 - FFT_USEFUL_SIZE / 2], sizeof(float32_t) * FFT_USEFUL_SIZE); //useful fft part
	float32_t fft_compress_rate = (float32_t)FFT_USEFUL_SIZE / (float32_t)LAY_FFT_PRINT_SIZE;
	float32_t fft_compress_rate_half = floorf(fft_compress_rate / 2.0f); //full points
	float32_t fft_compress_rate_parts = fmodf(fft_compress_rate / 2.0f, 1.0f); //partial points
	
	//Compress FFT
	for (uint32_t i = 0; i < LAY_FFT_PRINT_SIZE; i++)
	{
		int32_t left_index = (uint32_t)((float32_t)i * fft_compress_rate - fft_compress_rate_half);
		if(left_index < 0)
			left_index = 0;
		int32_t right_index = (uint32_t)((float32_t)i * fft_compress_rate + fft_compress_rate_half);
		if(right_index >= FFT_USEFUL_SIZE)
			right_index = FFT_USEFUL_SIZE - 1;
		
		float32_t points = 0;
		float32_t accum = 0.0f;
		//full points
		for(uint32_t index = left_index; index <= right_index ; index++)
		{
			accum += FFTInput[index];
			points += 1.0f;
		}
		//partial points
		if(fft_compress_rate_parts > 0.0f)
		{
			if(left_index > 0)
			{
				accum += FFTInput[left_index - 1] * fft_compress_rate_parts;
				points += fft_compress_rate_parts;
			}
			if(right_index < (FFT_USEFUL_SIZE - 1))
			{
				accum += FFTInput[right_index + 1] * fft_compress_rate_parts;
				points += fft_compress_rate_parts;
			}
		}
		FFTInput_tmp[i] = accum / points;
	}

	memcpy(&FFTInput, FFTInput_tmp, sizeof(FFTInput_tmp));
	
	//Delete noise
	float32_t minAmplValue = 0;
	uint32_t minAmplIndex = 0;
	arm_min_f32(FFTInput, LAY_FFT_PRINT_SIZE, &minAmplValue, &minAmplIndex);
	if (!TRX_on_TX())
		arm_offset_f32(FFTInput, -minAmplValue * 0.8f, FFTInput, LAY_FFT_PRINT_SIZE);

	// Looking for the median in frequency response
	arm_sort_f32(&FFT_sortInstance, FFTInput, FFTInput_tmp, LAY_FFT_PRINT_SIZE);
	float32_t medianValue = FFTInput_tmp[LAY_FFT_PRINT_SIZE / 2];

	// Maximum amplitude
	float32_t maxValueFFT = maxValueFFT_rx;
	if (TRX_on_TX())
		maxValueFFT = maxValueFFT_tx;
	float32_t maxValue = (medianValue * FFT_MAX);
	float32_t targetValue = (medianValue * FFT_TARGET);
	float32_t minValue = (medianValue * FFT_MIN);

	// Looking for the maximum in frequency response
	float32_t maxAmplValue = 0;
	arm_max_no_idx_f32(FFTInput, LAY_FFT_PRINT_SIZE, &maxAmplValue);

	// Auto-calibrate FFT levels
	maxValueFFT += (targetValue - maxValueFFT) / FFT_STEP_COEFF;

	// minimum-maximum threshold for median
	if (maxValueFFT < minValue)
		maxValueFFT = minValue;
	if (maxValueFFT > maxValue)
		maxValueFFT = maxValue;

	// Compress peaks
	float32_t compressTargetValue = (maxValueFFT * FFT_COMPRESS_INTERVAL);
	float32_t compressSourceInterval = maxAmplValue - compressTargetValue;
	float32_t compressTargetInterval = maxValueFFT - compressTargetValue;
	float32_t compressRate = compressTargetInterval / compressSourceInterval;
	if (!TRX_on_TX() && TRX.FFT_Compressor)
	{
		for (uint_fast16_t i = 0; i < LAY_FFT_PRINT_SIZE; i++)
			if (FFTInput[i] > compressTargetValue)
				FFTInput[i] = compressTargetValue + ((FFTInput[i] - compressTargetValue) * compressRate);
	}
	// Auto-calibrate FFT levels
	/*if (TRX_on_TX() || (TRX.FFT_Automatic && TRX.FFT_Sensitivity == FFT_MAX_TOP_SCALE)) //Fit FFT to MAX
	{
		maxValueFFT = maxValueFFT * 0.95f + maxAmplValue * 0.05f;
		if (maxValueFFT < maxAmplValue)
			maxValueFFT = maxAmplValue;
		minValue = (medianValue * 6.0f);
		if (maxValueFFT < minValue)
			maxValueFFT = minValue;
	}
	else if (TRX.FFT_Automatic) //Fit by median (automatic)
	{
		maxValueFFT += (targetValue - maxValueFFT) / FFT_STEP_COEFF;

		// minimum-maximum threshold for median
		if (maxValueFFT < minValue)
			maxValueFFT = minValue;
		if (maxValueFFT > maxValue)
			maxValueFFT = maxValue;

		// Compress peaks
		float32_t compressTargetValue = (maxValueFFT * FFT_COMPRESS_INTERVAL);
		float32_t compressSourceInterval = maxAmplValue - compressTargetValue;
		float32_t compressTargetInterval = maxValueFFT - compressTargetValue;
		float32_t compressRate = compressTargetInterval / compressSourceInterval;
		if (TRX.FFT_Compressor && TRX.FFT_Sensitivity < 50)
		{
			for (uint_fast16_t i = 0; i < LAYOUT->FFT_PRINT_SIZE; i++)
				if (FFTOutput_mean[i] > compressTargetValue)
					FFTOutput_mean[i] = compressTargetValue + ((FFTOutput_mean[i] - compressTargetValue) * compressRate);
		}
	}
	else //Manual Scale
	{
		
		float32_t minManualAmplitude = sqrtf(db2rateP((float32_t)TRX.FFT_ManualBottom)) * (float32_t)FFT_SIZE * (float32_t)TRX.FFT_Averaging;
		float32_t maxManualAmplitude = sqrtf(db2rateP((float32_t)TRX.FFT_ManualTop)) * (float32_t)FFT_SIZE * (float32_t)TRX.FFT_Averaging;
		arm_offset_f32(FFTOutput_mean, -minManualAmplitude, FFTOutput_mean, LAYOUT->FFT_PRINT_SIZE);
		for (uint_fast16_t i = 0; i < LAYOUT->FFT_PRINT_SIZE; i++)
			if (FFTOutput_mean[i] < 0)
				FFTOutput_mean[i] = 0;
		maxValueFFT = maxManualAmplitude - minManualAmplitude;
	}*/
	//limits
	if (TRX_on_TX())
		maxValueFFT = maxAmplValue;
	if (maxValueFFT < 0.0000001f)
		maxValueFFT = 0.0000001f;

	// save values ​​for switching RX / TX
	if (TRX_on_TX())
		maxValueFFT_tx = maxValueFFT;
	else
		maxValueFFT_rx = maxValueFFT;

	// Normalize the frequency response to one
	if (maxValueFFT > 0)
		arm_scale_f32(FFTInput, 1.0f / maxValueFFT, FFTInput, LAY_FFT_PRINT_SIZE);

	// Averaging values ​​for subsequent output
	float32_t averaging = (float32_t)TRX.FFT_Averaging;
	if (averaging < 1.0f)
		averaging = 1.0f;
	for (uint_fast16_t i = 0; i < LAY_FFT_PRINT_SIZE; i++)
		if (FFTOutput_mean[i] < FFTInput[i])
			FFTOutput_mean[i] += (FFTInput[i] - FFTOutput_mean[i]) / averaging;
		else
			FFTOutput_mean[i] -= (FFTOutput_mean[i] - FFTInput[i]) / averaging;

	FFT_need_fft = false;
}

// FFT output
bool FFT_printFFT(void)
{
	if (LCD_busy)
		return false;
	if (!TRX.FFT_Enabled)
		return false;
	if (!TRX_Inited)
		return false;
	if (FFT_need_fft)
		return false;
	if (LCD_systemMenuOpened)
		return false;
	/*if (CPU_LOAD.Load > 90)
		return;*/
	LCD_busy = true;

	uint16_t height = 0; // column height in FFT output
	uint16_t tmp = 0;
	

	if (CurrentVFO()->Freq != currentFFTFreq)
	{
		//calculate scale lines
		memset(grid_lines_pos, 0x00, sizeof(grid_lines_pos));
		uint8_t index = 0;
		for (int8_t i = 0; i < FFT_MAX_GRID_NUMBER; i++)
		{
			int32_t pos = -1;
			if (TRX.FFT_Grid > 0)
			{
				pos = getFreqPositionOnFFT((CurrentVFO()->Freq / 5000 * 5000) + ((i - 6) * 5000));
			}
			if (pos >= 0)
			{
				grid_lines_pos[index] = pos;
				index++;
			}
		}

		// offset the fft if needed
		FFT_move((int32_t)CurrentVFO()->Freq - (int32_t)currentFFTFreq);
		currentFFTFreq = CurrentVFO()->Freq;
	}

	// move the waterfall down using DMA
	for (tmp = LAY_WTF_HEIGHT - 1; tmp > 0; tmp--)
	{
		HAL_DMA_Start(&hdma_memtomem_dma2_stream7, (uint32_t)&indexed_wtf_buffer[tmp - 1], (uint32_t)&indexed_wtf_buffer[tmp], LAY_FFT_PRINT_SIZE / 4); //32bit dma, 8bit index data
		HAL_DMA_PollForTransfer(&hdma_memtomem_dma2_stream7, HAL_DMA_FULL_TRANSFER, HAL_MAX_DELAY);
		wtf_buffer_freqs[tmp] = wtf_buffer_freqs[tmp - 1];
	}

	// calculate the colors for the waterfall
	for (uint32_t fft_x = 0; fft_x < LAY_FFT_PRINT_SIZE; fft_x++)
	{
		height = (uint16_t)((float32_t)FFTOutput_mean[(uint_fast16_t)fft_x] * LAY_FFT_HEIGHT);
		if (height > LAY_FFT_HEIGHT)
			height = LAY_FFT_HEIGHT;

		wtf_buffer_freqs[0] = currentFFTFreq;
		fft_header[fft_x] = height;
		indexed_wtf_buffer[0][fft_x] = LAY_FFT_HEIGHT - height;
		if (fft_x == (LAY_FFT_PRINT_SIZE / 2))
			continue;
	}

	// calculate bw bar size
	switch (CurrentVFO()->Mode)
	{
	case TRX_MODE_LSB:
	case TRX_MODE_CW_L:
	case TRX_MODE_DIGI_L:
		bw_line_width = (int16_t)(CurrentVFO()->RX_LPF_Filter_Width / FFT_HZ_IN_PIXEL * TRX.FFT_Zoom);
		if (bw_line_width > (LAY_FFT_PRINT_SIZE / 2))
			bw_line_width = LAY_FFT_PRINT_SIZE / 2;
		bw_line_start = LAY_FFT_PRINT_SIZE / 2 - bw_line_width;
		break;
	case TRX_MODE_USB:
	case TRX_MODE_CW_U:
	case TRX_MODE_DIGI_U:
		bw_line_width = (int16_t)(CurrentVFO()->RX_LPF_Filter_Width / FFT_HZ_IN_PIXEL * TRX.FFT_Zoom);
		if (bw_line_width > (LAY_FFT_PRINT_SIZE / 2))
			bw_line_width = LAY_FFT_PRINT_SIZE / 2;
		bw_line_start = LAY_FFT_PRINT_SIZE / 2;
		break;
	case TRX_MODE_NFM:
	case TRX_MODE_AM:
		bw_line_width = (int16_t)(CurrentVFO()->RX_LPF_Filter_Width / FFT_HZ_IN_PIXEL * TRX.FFT_Zoom * 2);
		if (bw_line_width > LAY_FFT_PRINT_SIZE)
			bw_line_width = LAY_FFT_PRINT_SIZE;
		bw_line_start = LAY_FFT_PRINT_SIZE / 2 - (bw_line_width / 2);
		break;
	case TRX_MODE_WFM:
		bw_line_width = LAY_FFT_PRINT_SIZE;
		bw_line_start = LAY_FFT_PRINT_SIZE / 2 - (bw_line_width / 2);
		break;
	default:
		break;
	}
	bw_line_center = bw_line_start + bw_line_width / 2;
	bw_line_end = bw_line_start + bw_line_width;
	
	// prepare FFT print over the waterfall
	uint16_t background = BG_COLOR;
	for (uint32_t fft_y = 0; fft_y < LAY_FFT_HEIGHT; fft_y++)
	{
		if (TRX.FFT_Background)
			background = palette_bg_gradient[fft_y];

		//gradient
		for (uint32_t fft_x = 0; fft_x < LAY_FFT_PRINT_SIZE; fft_x++)
		{
			if (fft_x >= bw_line_start && fft_x <= bw_line_end) //bw bar
			{
				if (fft_y >= (LAY_FFT_HEIGHT - fft_header[fft_x]))
					fft_output_buffer[fft_y][fft_x] = palette_bw_fft_colors[fft_y];
				else
					fft_output_buffer[fft_y][fft_x] = palette_bw_bg_colors[fft_y];
			}
			else //other fft data
			{
				if (fft_y >= (LAY_FFT_HEIGHT - fft_header[fft_x]))
					fft_output_buffer[fft_y][fft_x] = palette_fft[fft_y];
				else
					fft_output_buffer[fft_y][fft_x] = background;
			}
		}
	}

	//draw grids
	if (TRX.FFT_Grid == 1 || TRX.FFT_Grid == 2)
	{
		for (int32_t grid_line_index = 0; grid_line_index < FFT_MAX_GRID_NUMBER; grid_line_index++)
			if (grid_lines_pos[grid_line_index] > 0 && grid_lines_pos[grid_line_index] < LAY_FFT_PRINT_SIZE && grid_lines_pos[grid_line_index] != (LAY_FFT_PRINT_SIZE / 2))
				for (uint32_t fft_y = 0; fft_y < LAY_FFT_HEIGHT; fft_y++)
					fft_output_buffer[fft_y][grid_lines_pos[grid_line_index]] = palette_fft[LAY_FFT_HEIGHT * 3 / 4]; //mixColors(fft_output_buffer[fft_y][grid_lines_pos[grid_line_index]], palette_fft[fftHeight / 2], FFT_SCALE_LINES_BRIGHTNESS);
	}

	//Gauss filter center
	if (TRX.CW_GaussFilter && (CurrentVFO()->Mode == TRX_MODE_CW_L || CurrentVFO()->Mode == TRX_MODE_CW_U))
	{
		for (uint32_t fft_y = 0; fft_y < LAY_FFT_HEIGHT; fft_y++)
			fft_output_buffer[fft_y][bw_line_center] = palette_fft[LAY_FFT_HEIGHT / 2]; //mixColors(fft_output_buffer[fft_y][bw_line_center], palette_fft[fftHeight / 2], FFT_SCALE_LINES_BRIGHTNESS);
	}

	//draw center line
	for (uint32_t fft_y = 0; fft_y < LAY_FFT_HEIGHT; fft_y++)
		fft_output_buffer[fft_y][(LAY_FFT_PRINT_SIZE / 2)] = palette_fft[LAY_FFT_HEIGHT / 2]; //mixColors(fft_output_buffer[fft_y][(LAY_FFT_PRINT_SIZE / 2)], palette_fft[fftHeight / 2], FFT_SCALE_LINES_BRIGHTNESS);

	//Print FFT
	LCDDriver_SetCursorAreaPosition(0, LAY_FFT_FFTWTF_POS_Y, LAY_FFT_PRINT_SIZE - 1, (LAY_FFT_FFTWTF_POS_Y + LAY_FFT_HEIGHT));
	print_fft_dma_estimated_size = LAY_FFT_PRINT_SIZE * LAY_FFT_HEIGHT;
	print_fft_dma_position = 0;
	FFT_afterPrintFFT();
	return true;
}

//actions after FFT_printFFT
void FFT_afterPrintFFT(void)
{
	//continue DMA draw?
	if(print_fft_dma_estimated_size > 0)
	{
		if(print_fft_dma_estimated_size <= FFT_DMA_MAX_BLOCK)
		{
			HAL_DMA_Start_IT(&hdma_memtomem_dma2_stream5, (uint32_t)&fft_output_buffer[0] + print_fft_dma_position * 2, LCD_FSMC_DATA_ADDR, print_fft_dma_estimated_size);
			print_fft_dma_estimated_size = 0;
			print_fft_dma_position = 0;
		}
		else
		{
			print_fft_dma_estimated_size -= FFT_DMA_MAX_BLOCK;
			HAL_DMA_Start_IT(&hdma_memtomem_dma2_stream5, (uint32_t)&fft_output_buffer[0] + print_fft_dma_position * 2, LCD_FSMC_DATA_ADDR, FFT_DMA_MAX_BLOCK);
			print_fft_dma_position += FFT_DMA_MAX_BLOCK;
		}
		return;
	}
	
	// clear and display part of the vertical bar
	memset(bandmap_line_tmp, 0x00, sizeof(bandmap_line_tmp));

	// output bandmaps
	int8_t band_curr = getBandFromFreq(CurrentVFO()->Freq, true);
	int8_t band_left = band_curr;
	if (band_curr > 0)
		band_left = band_curr - 1;
	int8_t band_right = band_curr;
	if (band_curr < (BANDS_COUNT - 1))
		band_right = band_curr + 1;
	int32_t fft_freq_position_start = 0;
	int32_t fft_freq_position_stop = 0;
	for (uint16_t band = band_left; band <= band_right; band++)
	{
		//regions
		for (uint16_t region = 0; region < BANDS[band].regionsCount; region++)
		{
			uint16_t region_color = COLOR->BANDMAP_SSB;
			if (BANDS[band].regions[region].mode == TRX_MODE_CW_L || BANDS[band].regions[region].mode == TRX_MODE_CW_U)
				region_color = COLOR->BANDMAP_CW;
			else if (BANDS[band].regions[region].mode == TRX_MODE_DIGI_L || BANDS[band].regions[region].mode == TRX_MODE_DIGI_U)
				region_color = COLOR->BANDMAP_DIGI;
			else if (BANDS[band].regions[region].mode == TRX_MODE_NFM || BANDS[band].regions[region].mode == TRX_MODE_WFM)
				region_color = COLOR->BANDMAP_FM;
			else if (BANDS[band].regions[region].mode == TRX_MODE_AM)
				region_color = COLOR->BANDMAP_AM;

			fft_freq_position_start = getFreqPositionOnFFT(BANDS[band].regions[region].startFreq);
			fft_freq_position_stop = getFreqPositionOnFFT(BANDS[band].regions[region].endFreq);
			if (fft_freq_position_start != -1 && fft_freq_position_stop == -1)
				fft_freq_position_stop = LAY_FFT_PRINT_SIZE;
			if (fft_freq_position_start == -1 && fft_freq_position_stop != -1)
				fft_freq_position_start = 0;
			if (fft_freq_position_start == -1 && fft_freq_position_stop == -1 && BANDS[band].regions[region].startFreq < CurrentVFO()->Freq && BANDS[band].regions[region].endFreq > CurrentVFO()->Freq)
			{
				fft_freq_position_start = 0;
				fft_freq_position_stop = LAY_FFT_PRINT_SIZE;
			}

			if (fft_freq_position_start != -1 && fft_freq_position_stop != -1)
				for (int32_t pixel_counter = fft_freq_position_start; pixel_counter < fft_freq_position_stop; pixel_counter++)
					bandmap_line_tmp[(uint16_t)pixel_counter] = region_color;
		}
	}

	LCDDriver_SetCursorAreaPosition(0, LAY_FFT_FFTWTF_POS_Y - 4, LAY_FFT_PRINT_SIZE - 1, LAY_FFT_FFTWTF_POS_Y - 3);
	for (uint8_t r = 0; r < 2; r++)
		for (uint32_t pixel_counter = 0; pixel_counter < LAY_FFT_PRINT_SIZE; pixel_counter++)
			LCDDriver_SendData(bandmap_line_tmp[pixel_counter]);

	// display the waterfall using DMA
	print_wtf_yindex = 0;
	FFT_printWaterfallDMA();
}

// waterfall output
void FFT_printWaterfallDMA(void)
{
	//wtf area
	if (print_wtf_yindex == 0)
		LCDDriver_SetCursorAreaPosition(0, LAY_FFT_FFTWTF_POS_Y + LAY_FFT_HEIGHT, LAY_FFT_PRINT_SIZE - 1, LAY_FFT_FFTWTF_POS_Y + LAY_FFT_HEIGHT + (uint16_t)LAY_WTF_HEIGHT - 1);

//print waterfall line
	if (print_wtf_yindex < LAY_WTF_HEIGHT)
	{
		// calculate offset
		float32_t freq_diff = (((float32_t)currentFFTFreq - (float32_t)wtf_buffer_freqs[print_wtf_yindex]) / FFT_HZ_IN_PIXEL) * (float32_t)TRX.FFT_Zoom;
		float32_t freq_diff_part = fmodf(freq_diff, 1.0f);
		int32_t margin_left = 0;
		if (freq_diff < 0)
			margin_left = -floorf(freq_diff);
		if (margin_left > LAY_FFT_PRINT_SIZE)
			margin_left = LAY_FFT_PRINT_SIZE;
		int32_t margin_right = 0;
		if (freq_diff > 0)
			margin_right = ceilf(freq_diff);
		if (margin_right > LAY_FFT_PRINT_SIZE)
			margin_right = LAY_FFT_PRINT_SIZE;
		if ((margin_left + margin_right) > LAY_FFT_PRINT_SIZE)
			margin_right = 0;
		//rounding
		int32_t body_width = LAY_FFT_PRINT_SIZE - margin_left - margin_right;
		
		if (body_width <= 0)
		{
			memset(&wtf_output_line, BG_COLOR, sizeof(wtf_output_line));
		}
		else
		{
			if (margin_left == 0 && margin_right == 0)
			{
				for (uint32_t wtf_x = 0; wtf_x < LAY_FFT_PRINT_SIZE; wtf_x++)
					if (wtf_x >= bw_line_start && wtf_x <= bw_line_end) //print bw bar
						wtf_output_line[wtf_x] = palette_bw_fft_colors[indexed_wtf_buffer[print_wtf_yindex][wtf_x]];
					else
						wtf_output_line[wtf_x] = palette_fft[indexed_wtf_buffer[print_wtf_yindex][wtf_x]];
			}
			else if (margin_left > 0)
			{
				memset(&wtf_output_line, BG_COLOR, (uint32_t)(margin_left * 2)); // fill the space to the left
				for (uint32_t wtf_x = 0; wtf_x < (LAY_FFT_PRINT_SIZE - margin_left); wtf_x++)
					if ((margin_left + wtf_x) >= bw_line_start && (margin_left + wtf_x) <= bw_line_end) //print bw bar
						wtf_output_line[margin_left + wtf_x] = palette_bw_fft_colors[indexed_wtf_buffer[print_wtf_yindex][wtf_x]];
					else
						wtf_output_line[margin_left + wtf_x] = palette_fft[indexed_wtf_buffer[print_wtf_yindex][wtf_x]];
			}
			if (margin_right > 0)
			{
				memset(&wtf_output_line[(LAY_FFT_PRINT_SIZE - margin_right)], BG_COLOR, (uint32_t)(margin_right * 2)); // fill the space to the right
				for (uint32_t wtf_x = 0; wtf_x < (LAY_FFT_PRINT_SIZE - margin_right); wtf_x++)
					if (wtf_x >= bw_line_start && wtf_x <= bw_line_end) //print bw bar
						wtf_output_line[wtf_x] = palette_bw_fft_colors[indexed_wtf_buffer[print_wtf_yindex][wtf_x + margin_right]];
					else
						wtf_output_line[wtf_x] = palette_fft[indexed_wtf_buffer[print_wtf_yindex][wtf_x + margin_right]];
			}
		}

		//print scale lines
		if (TRX.FFT_Grid >= 2)
			for (int8_t i = 0; i < FFT_MAX_GRID_NUMBER; i++)
				if (grid_lines_pos[i] > 0)
					wtf_output_line[grid_lines_pos[i]] = palette_fft[LAY_FFT_HEIGHT * 3 / 4]; //mixColors(wtf_output_line[grid_lines_pos[i]], palette_fft[fftHeight / 2], FFT_SCALE_LINES_BRIGHTNESS);

		//Gauss filter center
		if (TRX.CW_GaussFilter && (CurrentVFO()->Mode == TRX_MODE_CW_L || CurrentVFO()->Mode == TRX_MODE_CW_U))
			wtf_output_line[bw_line_center] = palette_fft[LAY_FFT_HEIGHT / 2]; //mixColors(fft_output_buffer[fft_y][bw_line_center], palette_fft[fftHeight / 2], FFT_SCALE_LINES_BRIGHTNESS);
				
		//center line
		wtf_output_line[LAY_FFT_PRINT_SIZE / 2] = palette_fft[LAY_FFT_HEIGHT / 2]; //mixColors(wtf_output_line[LAY_FFT_PRINT_SIZE / 2], palette_fft[fftHeight / 2], FFT_SCALE_LINES_BRIGHTNESS);

		//draw the line
		HAL_DMA_Start_IT(&hdma_memtomem_dma2_stream6, (uint32_t)&wtf_output_line[0], LCD_FSMC_DATA_ADDR, LAY_FFT_PRINT_SIZE);
		print_wtf_yindex++;
	}
	else
	{
		FFT_FPS++;
		lastWTFFreq = currentFFTFreq;
		FFT_need_fft = true;
		LCD_busy = false;
	}
}

// shift the waterfall
static void FFT_move(int32_t _freq_diff)
{
	if (_freq_diff == 0)
		return;
	float32_t old_x_true = 0.0f;
	int32_t old_x_l = 0;
	int32_t old_x_r = 0;
	float32_t freq_diff = ((float32_t)_freq_diff / FFT_HZ_IN_PIXEL) * (float32_t)TRX.FFT_Zoom;
	float32_t old_x_part_r = fmodf(freq_diff, 1.0f);
	float32_t old_x_part_l = 1.0f - old_x_part_r;
	if (freq_diff < 0.0f)
	{
		old_x_part_l = fmodf(-freq_diff, 1.0f);
		old_x_part_r = 1.0f - old_x_part_l;
	}

	//Move mean Buffer
	for (int32_t x = 0; x < LAY_FFT_PRINT_SIZE; x++)
	{
		old_x_true = (float32_t)x + freq_diff;
		old_x_l = (int32_t)(floorf(old_x_true));
		old_x_r = (int32_t)(ceilf(old_x_true));

		FFTInput_tmp[x] = 0;

		if ((old_x_true >= LAY_FFT_PRINT_SIZE) || (old_x_true < 0.0f))
			continue;
		if ((old_x_l < LAY_FFT_PRINT_SIZE) && (old_x_l >= 0))
			FFTInput_tmp[x] += (FFTOutput_mean[old_x_l] * old_x_part_l);
		if ((old_x_r < LAY_FFT_PRINT_SIZE) && (old_x_r >= 0))
			FFTInput_tmp[x] += (FFTOutput_mean[old_x_r] * old_x_part_r);

		//sides
		if (old_x_r >= LAY_FFT_PRINT_SIZE)
			FFTInput_tmp[x] = FFTOutput_mean[old_x_l];
	}
	//save results
	memcpy(&FFTOutput_mean, &FFTInput_tmp, sizeof FFTOutput_mean);
	
	//clean charge buffer
	if(TRX.FFT_Zoom > 1)
		memset(&FFTInputCharge[0], 0x00, sizeof(float32_t) * (FFT_SIZE - zoomed_width) * 2);
}

// get color from signal strength
static uint16_t getFFTColor(uint_fast8_t height) // Get FFT color warmth (blue to red)
{
	uint_fast8_t red = 0;
	uint_fast8_t green = 0;
	uint_fast8_t blue = 0;
	if (COLOR->WTF_BG_WHITE)
	{
		red = 255;
		green = 255;
		blue = 255;
	}
	//blue -> yellow -> red
	if (TRX.FFT_Color == 1)
	{
		// r g b
		// 0 0 0
		// 0 0 255
		// 255 255 0
		// 255 0 0
		// contrast of each of the 3 zones, the total should be 1.0f
		const float32_t contrast1 = 0.02f;
		const float32_t contrast2 = 0.45f;
		const float32_t contrast3 = 0.53f;

		if (height < LAY_FFT_HEIGHT * contrast1)
		{
			blue = (uint_fast8_t)(height * 255 / (LAY_FFT_HEIGHT * contrast1));
			if (COLOR->WTF_BG_WHITE)
			{
				red -= blue;
				green -= blue;
			}
		}
		else if (height < LAY_FFT_HEIGHT * (contrast1 + contrast2))
		{
			green = (uint_fast8_t)((height - LAY_FFT_HEIGHT * contrast1) * 255 / ((LAY_FFT_HEIGHT - LAY_FFT_HEIGHT * contrast1) * (contrast1 + contrast2)));
			red = green;
			blue = 255 - green;
		}
		else
		{
			red = 255;
			blue = 0;
			green = (uint_fast8_t)(255 - (height - (LAY_FFT_HEIGHT * (contrast1 + contrast2))) * 255 / ((LAY_FFT_HEIGHT - (LAY_FFT_HEIGHT * (contrast1 + contrast2))) * (contrast1 + contrast2 + contrast3)));
		}
		return rgb888torgb565(red, green, blue);
	}
	//black -> yellow -> red
	if (TRX.FFT_Color == 2)
	{
		// r g b
		// 0 0 0
		// 255 255 0
		// 255 0 0
		// contrast of each of the 2 zones, the total should be 1.0f
		float32_t contrast1 = 0.5f;
		float32_t contrast2 = 0.5f;
		if (COLOR->WTF_BG_WHITE)
		{
			contrast1 = 0.2f;
			contrast2 = 0.8f;
		}

		if (height < LAY_FFT_HEIGHT * contrast1)
		{
			if (!COLOR->WTF_BG_WHITE)
			{
				red = (uint_fast8_t)(height * 255 / (LAY_FFT_HEIGHT * contrast1));
				green = (uint_fast8_t)(height * 255 / (LAY_FFT_HEIGHT * contrast1));
				blue = 0;
			}
			else
			{
				red = 255;
				green = 255;
				blue = 255 - (uint_fast8_t)(height * 255 / (LAY_FFT_HEIGHT * contrast1));
			}
		}
		else
		{
			red = 255;
			blue = 0;
			green = (uint_fast8_t)(255 - (height - (LAY_FFT_HEIGHT * (contrast1))) * 255 / ((LAY_FFT_HEIGHT - (LAY_FFT_HEIGHT * (contrast1))) * (contrast1 + contrast2)));
			if (COLOR->WTF_BG_WHITE)
			{
				blue = green;
			}
		}
		return rgb888torgb565(red, green, blue);
	}
	//black -> yellow -> green
	if (TRX.FFT_Color == 3)
	{
		// r g b
		// 0 0 0
		// 255 255 0
		// 0 255 0
		// contrast of each of the 2 zones, the total should be 1.0f
		float32_t contrast1 = 0.5f;
		float32_t contrast2 = 0.5f;
		if (COLOR->WTF_BG_WHITE)
		{
			contrast1 = 0.2f;
			contrast2 = 0.8f;
		}

		if (height < LAY_FFT_HEIGHT * contrast1)
		{
			if (!COLOR->WTF_BG_WHITE)
			{
				red = (uint_fast8_t)(height * 255 / (LAY_FFT_HEIGHT * contrast1));
				green = (uint_fast8_t)(height * 255 / (LAY_FFT_HEIGHT * contrast1));
				blue = 0;
			}
			else
			{
				red = 255;
				green = 255;
				blue = 255 - (uint_fast8_t)(height * 255 / (LAY_FFT_HEIGHT * contrast1));
			}
		}
		else
		{
			green = 255;
			blue = 0;
			red = (uint_fast8_t)(255 - (height - (LAY_FFT_HEIGHT * (contrast1))) * 255 / ((LAY_FFT_HEIGHT - (LAY_FFT_HEIGHT * (contrast1))) * (contrast1 + contrast2)));
			if (COLOR->WTF_BG_WHITE)
			{
				green = red;
			}
		}
		return rgb888torgb565(red, green, blue);
	}
	//black -> red
	if (TRX.FFT_Color == 4)
	{
		// r g b
		// 0 0 0
		// 255 0 0

		if (height <= LAY_FFT_HEIGHT)
		{
			red = (uint_fast8_t)(height * 255 / (LAY_FFT_HEIGHT));
			if (COLOR->WTF_BG_WHITE)
			{
				green -= (uint_fast8_t)(height * 255 / (LAY_FFT_HEIGHT));
				blue -= (uint_fast8_t)(height * 255 / (LAY_FFT_HEIGHT));
				red = 255;
			}
		}
		return rgb888torgb565(red, green, blue);
	}
	//black -> green
	if (TRX.FFT_Color == 5)
	{
		// r g b
		// 0 0 0
		// 0 255 0

		if (height <= LAY_FFT_HEIGHT)
		{
			green = (uint_fast8_t)(height * 255 / (LAY_FFT_HEIGHT));
			if (COLOR->WTF_BG_WHITE)
			{
				green = 255;
				blue -= (uint_fast8_t)(height * 255 / (LAY_FFT_HEIGHT));
				red -= (uint_fast8_t)(height * 255 / (LAY_FFT_HEIGHT));
			}
		}
		return rgb888torgb565(red, green, blue);
	}
	//black -> blue
	if (TRX.FFT_Color == 6)
	{
		// r g b
		// 0 0 0
		// 0 0 255

		if (height <= LAY_FFT_HEIGHT)
		{
			blue = (uint_fast8_t)(height * 255 / (LAY_FFT_HEIGHT));
			if (COLOR->WTF_BG_WHITE)
			{
				green -= (uint_fast8_t)(height * 255 / (LAY_FFT_HEIGHT));
				blue = 255;
				red -= (uint_fast8_t)(height * 255 / (LAY_FFT_HEIGHT));
			}
		}
		return rgb888torgb565(red, green, blue);
	}
	//black -> white
	if (TRX.FFT_Color == 7)
	{
		// r g b
		// 0 0 0
		// 255 255 255

		if (height <= LAY_FFT_HEIGHT)
		{
			red = (uint_fast8_t)(height * 255 / (LAY_FFT_HEIGHT));
			green = red;
			blue = red;
			if (COLOR->WTF_BG_WHITE)
			{
				red = 255 - red;
				green = 255 - green;
				blue = 255 - blue;
			}
		}
		return rgb888torgb565(red, green, blue);
	}
	//unknown
	return COLOR_WHITE;
}

static uint16_t getBGColor(uint_fast8_t height) // Get FFT background gradient
{
	float32_t fftheight = LAY_FFT_HEIGHT;
	float32_t step_red = (float32_t)(COLOR->FFT_GRADIENT_END_R - COLOR->FFT_GRADIENT_START_R) / fftheight;
	float32_t step_green = (float32_t)(COLOR->FFT_GRADIENT_END_G - COLOR->FFT_GRADIENT_START_G) / fftheight;
	float32_t step_blue = (float32_t)(COLOR->FFT_GRADIENT_END_B - COLOR->FFT_GRADIENT_START_B) / fftheight;

	uint_fast8_t red = (uint_fast8_t)(COLOR->FFT_GRADIENT_START_R + (float32_t)height * step_red);
	uint_fast8_t green = (uint_fast8_t)(COLOR->FFT_GRADIENT_START_G + (float32_t)height * step_green);
	uint_fast8_t blue = (uint_fast8_t)(COLOR->FFT_GRADIENT_START_B + (float32_t)height * step_blue);

	return rgb888torgb565(red, green, blue);
}

// prepare the color palette
static void FFT_fill_color_palette(void) // Fill FFT Color Gradient On Initialization
{
	for (uint_fast8_t i = 0; i <= LAY_FFT_HEIGHT; i++)
	{
		palette_fft[i] = getFFTColor(LAY_FFT_HEIGHT - i);
		palette_bg_gradient[i] = getBGColor(LAY_FFT_HEIGHT - i);
		palette_bw_fft_colors[i] = addColor(palette_fft[i], FFT_BW_BRIGHTNESS, FFT_BW_BRIGHTNESS, FFT_BW_BRIGHTNESS);
		palette_bw_bg_colors[i] = addColor(palette_bg_gradient[i], FFT_BW_BRIGHTNESS, FFT_BW_BRIGHTNESS, FFT_BW_BRIGHTNESS);
	}
}

// reset FFT
void FFT_Reset(void) // clear the FFT
{
	FFT_new_buffer_ready = false;
	memset(FFTInput_I_A, 0x00, sizeof FFTInput_I_A);
	memset(FFTInput_Q_A, 0x00, sizeof FFTInput_Q_A);
	memset(FFTInput_I_B, 0x00, sizeof FFTInput_I_B);
	memset(FFTInput_Q_B, 0x00, sizeof FFTInput_Q_B);
	memset(FFTInputCharge, 0x00, sizeof FFTInputCharge);
	memset(FFTInput, 0x00, sizeof FFTInput);
	memset(FFTOutput_mean, 0x00, sizeof FFTOutput_mean);
	FFT_buff_index = 0;
}

static inline int32_t getFreqPositionOnFFT(uint32_t freq)
{
	int32_t pos = (int32_t)((float32_t)LAY_FFT_PRINT_SIZE / 2 + (float32_t)((float32_t)freq - (float32_t)CurrentVFO()->Freq) / FFT_HZ_IN_PIXEL * (float32_t)TRX.FFT_Zoom);
	if (pos < 0 || pos >= LAY_FFT_PRINT_SIZE)
		return -1;
	return pos;
}

uint32_t getFreqOnFFTPosition(uint16_t position)
{
	return (uint32_t)((int32_t)CurrentVFO()->Freq + (int32_t)(-((float32_t)LAY_FFT_PRINT_SIZE * (FFT_HZ_IN_PIXEL  / (float32_t)TRX.FFT_Zoom) / 2.0f) + (float32_t)position * (FFT_HZ_IN_PIXEL / (float32_t)TRX.FFT_Zoom)));
}