
// ****************************************************************************
//
//                        Page FFT - spectrum analyzer
//
// ****************************************************************************
// Filter coefficients: https://webaudio.github.io/Audio-EQ-Cookbook/audio-eq-cookbook.html
//
//  fmin = 16 Hz ... minimal frequency
//  fmax = 16 kHz ... maximal frequency
//  fs = 44100 Hz ... sample frequency
//  Q = 10.0 ... filter quality
//  N = 16 ... number of bands
//
//  fc = fmin * pow(fmax/fmin, i/(N-1)) ... middle frequency of the band 'i'
//
//  omega = 2 * pi * fc / fs
//  alpha = sin(omega) / (2*Q)
//
//  b0 = alpha
//  (b1 = 0)
//  (b2 = -alpha = -b0)
//  a0 = 1 + alpha
//  a1 = -2 * cw;
//  a2 = 1 - alpha;
// 
//  Normalize: all coefficients divided by a0

//  y[n] = b0*x[n] + {b1*x[n-1] +} b2*x[n-2] - a1*y[n-1] - a2*y[n-2]

// All calculations and coefficients are in integer Q26 format:
//   bit 31 = sign, bits 26..30 = integer part, bits 0..25 = fractional part

#include "../include.h"

// 1) Disp time: 13 ms
// 2) Loop time: 46 ms (22 fps)
// 3) Process time: 29 ms
// 4) Sample rate: 44126 Hz, record 46 ms (2048 samples)
// 5) wait for DMA: 4 ms

#define FFT_CHECK	0	// DEBUG check: 0=normal mode, 1=disp time [ms], 2=loop time [ms], 3=process time [ms], 4=sample rate [Hz], 5=wait for DMA [ms]

#define N_BANDS		16	// number of bands
#define FFT_BUF_SIZE	(1<<11)	// buffer size

int FFT_Mode = FFT_MODE_FILL;	// current FFT display mode
Bool FFT_Hold = False;		// hold
u8 FFT_BufInx = 0;		// current buffer index to load, ^1 = current buffer index to display
Bool FFT_TitleUpdate = True;	// update title

#if FFT_CHECK	// DEBUG check: 0=normal mode, 1=Draw time, 2=Disp time
int FFT_Check;
#endif

// bands middle frequencies in [Hz] ... 100Hz band 4, 1kHz band 9, 10kHz band 14
const s32 FFT_F[N_BANDS] = {
	16,	25,	40,	64,	100,	160,	250,	400,
	640,	1000,	1600,	2500,	4000,	6400,	10000,	16000,
};

// biquad filter (structure must correspond to the definition in page_asm.S)
typedef struct {
	s32	b0;		// 0x00: b1 = 0 not used, b2 = -b1
	s32	a1;		// 0x04: a0 = 1.0 not used (coefficients are already normalized)
	s32	a2;		// 0x08:
	s32	x1;		// 0x0C: previous inputs
	s32	x2;		// 0x10:
	s32	y1;		// 0x14: previous outputs
	s32	y2;		// 0x18:
	s32	max;		// 0x1C: maximum y
	s32	level;		// 0x20: current level
	s16	peaklive;	// 0x24: peak live counter
	u8	peak;		// 0x26: peak	
	u8	disp;		// 0x27: display level
} sBiquad;
STATIC_ASSERT(sizeof(sBiquad) == 0x28, "Incorrect sBiquad!");

// biquad filters
sBiquad FFT_Bands[N_BANDS] = {
	{ 7648, -134202083, 67093568, },
	{ 11950, -134192978, 67084965, },
	{ 19117, -134177315, 67070630, },
	{ 30582, -134150987, 67047700, },
	{ 48249, -134107344, 67012367, },
	{ 76397, -134030100, 66956069, },
	{ 121184, -133887640, 66866496, },
	{ 191531, -133615207, 66725803, },
	{ 302741, -133062352, 66503383, },
	{ 477760, -131884831, 66153344, },
	{ 749831, -129284535, 65609202, },
	{ 1165575, -123370852, 64777715, },
	{ 1770118, -109833049, 63568628, },
	{ 2543895, -79500329, 62021075, },
	{ 3168971, -16894859, 60770922, },
	{ 2454077, 84172318, 62200711, },
};

/*
// Add samples to filters (takes 35 ms)
// - This function has an alternative variant in "page_asm.S" that
//   is slightly faster. However, this C variant can also be used.
void NOFLASH(FFT_AddSamples)()
{
	int i, j;
	s32 x, y;
	s64 acc;
	sBiquad* f;
	u8* s;
	u8* s0 = (u8*)Buf + FFT_BufInx*FFT_BUF_SIZE;

	// loop filters
	f = FFT_Bands;
	for (i = N_BANDS; i > 0; i--)
	{
		// process samples - 1st part, do not update MAX
		s = s0;
		for (j = FFT_BUF_SIZE/2; j > 0; j--)
		{
			// konvert sample to Q26 format
			x = (s32)(*s++ - 128) << (26-7); // convert to Q26

			// get accumulator (b2 = -b0)
			//  y[n] = b0*(x[n] - x[n-2]) - a1*y[n-1] - a2*y[n-2]
			acc = (s64)f->b0 * (x - f->x2);
			acc -= (s64)f->a1 * f->y1;
			acc -= (s64)f->a2 * f->y2;

			// get result y
			acc += (1 << 25);	// rounding
			y = acc >> 26;		// convert to Q26

			// shift state
			f->x2 = f->x1;
			f->x1 = x;
			f->y2 = f->y1;
			f->y1 = y;
		}

		// process samples - 2nd part, update MAX
		for (j = FFT_BUF_SIZE/2; j > 0; j--)
		{
			// konvert sample to Q26 format
			x = (s32)(*s++ - 128) << (26-7); // convert to Q26

			// get accumulator (b2 = -b0)
			//  y[n] = b0*(x[n] - x[n-2]) - a1*y[n-1] - a2*y[n-2]
			acc = (s64)f->b0 * (x - f->x2);
			acc -= (s64)f->a1 * f->y1;
			acc -= (s64)f->a2 * f->y2;

			// get result y
			acc += (1 << 25);	// rounding
			y = acc >> 26;		// convert to Q26

			// shift state
			f->x2 = f->x1;
			f->x1 = x;
			f->y2 = f->y1;
			f->y1 = y;

			// get max. value (positive half-wave is enough)
			if (y > f->max) f->max = y;
		}

		// shift to next filter
		f++;
	}
}
*/

// FFT initialize
void FFT_Init()
{
	// setup input pin
	GPIO_Mode(IN_GPIO, GPIO_MODE_AIN);

	// enable DMA1 clock
	RCC_DMA1ClkEnable();

	// Enable Timer clock source
	TIM1_ClkEnable();

	// enable clock for ADC
	RCC_ADC1ClkEnable();

	// reset ADC module
	RCC_ADC1Reset();
}

// FFT terminate
void FFT_Term()
{
	// reset Timer
	TIM1_Reset();

	// stop DMA
	DMA_ChanDisable(DMA1_Chan(FFT_DMA));

	// Timer clock disable
	RCC_TIM1ClkDisable();

	// ADC disable
	ADC1_Disable();

	// reset input pin
	GPIO_PinReset(IN_GPIO);
}

// setup ADC
void FFT_SetupADC()
{
	// ADC disable
	ADC1_Disable();

	// set ADC divider ... Need ADC speed 44ksps or faster
	//  Divider 8, sampling time 3: 71.5+12.5=84 clock; real speed 48000/8/84 = 71ksps
	RCC_ADCDivEnable();
	RCC_ADCDiv(RCC_ADCCLK_DIV8);

	// set sampling time
	ADC1_SampTime(IN_ADC, 6);

	// select channel to be converted
	ADC1_RSeq(1, IN_ADC);

	// set number of channels to be converted in a regular channel conversion sequence
	ADC1_RNum(1);

	// align data left (we need top 8 bits)
	ADC1_AlignLeft();

	// scan disable
	ADC1_ScanDisable();

	// select trigger of rule channel conversion - trigger with Timer TRGO
	ADC1_ExtSel(ADC_EXT_T1_TRGO);
	ADC1_Single();

	// enable externally triggered conversion for rule channels
	ADC1_ExtTrigEnable();

	// enable DMA transfer
	ADC1_DMAEnable();
}

// setup Timer
void FFT_SetupTimer()
{
	// Reset timer to default setup
	TIM1_Reset();

	// select input from internal clock CK_INT
	TIM1_InMode(TIM_INMODE_INT);

	// set prescaler to 2 - clock will be 48/2 = 24MHz
	TIM1_Presc(2-1);

	// set reload value to get 44100 Hz (48000000/2/544 = 44117Hz)
	TIM1_Load(544-1);

	// reload immediately
	TIM1_Update();

	// reset counter
	TIM1_Cnt(0);

	// generate TRGO signal to other peripherals
	TIM1_MSSync(TIM_MSSYNC_UPDATE);
};

// setup DMA channel
void FFT_SetupDMA()
{
	// get address of DMA1 channel
	DMAchan_t* chan = DMA1_Chan(FFT_DMA);

	// set source address - ADC regular data register (as peripheral address)
	DMA_PerAddr(chan, ((volatile u8*)&ADC1->RDATAR) + 1);

	// set destination addres (as memory address)
	DMA_MemAddr(chan, (u8*)Buf + FFT_BufInx*FFT_BUF_SIZE);

	// set number of entries
	DMA_Cnt(chan, FFT_BUF_SIZE);

	// configure DMA channel
	DMA_Cfg(chan,
	//	DMA_CFG_EN |			// channel enable
	//	DMA_CFG_COMPINT |		// completion interrupt enable
	//	DMA_CFG_HALFINT |		// over half interrupt enable
	//	DMA_CFG_TRANSERRINT |		// transmission error interrupt enable
	//	DMA_CFG_DIRFROMMEM |		// transfer direction from memory
		DMA_CFG_DIRFROMPER |		// ... transfer direction from peripheral
	//	DMA_CFG_CIRC |			// circular mode enabled
	//	DMA_CFG_PERINC |		// peripheral address increment
		DMA_CFG_MEMINC |		// memory address increment
	//	DMA_CFG_PSIZE(size) |		// peripheral data size DMA_SIZE_*
		DMA_CFG_PSIZE_8 |		// ... peripheral data size 8 bits
	//	DMA_CFG_PSIZE_16 |		// ... peripheral data size 16 bits
	//	DMA_CFG_PSIZE_32 |		// ... peripheral data size 32 bits
	//	DMA_CFG_MSIZE(size) |		// memory data size DMA_SIZE_*
		DMA_CFG_MSIZE_8 |		// ... memory data size 8 bits
	//	DMA_CFG_MSIZE_16 |		// ... memory data size 16 bits
	//	DMA_CFG_MSIZE_32 |		// ... memory data size 32 bits
	//	DMA_CFG_PRIOR(prior) |		// channel priority 0..3
	//	DMA_CFG_PRIOR_LOW |		// ... channel priority 0 low
	//	DMA_CFG_PRIOR_MED |		// ... channel priority 1 medium
	//	DMA_CFG_PRIOR_HIGH |		// ... channel priority 2 high
		DMA_CFG_PRIOR_VERYHIGH |	// ... channel priority 3 very high
	//	DMA_CFG_MERM2MEM |		// memory-to-memory mode enable
		0);

	// clear trasmission completion flag ... must be cleared after ADC setup
	DMA1_CompClr(FFT_DMA);
}

// start new transfer, flip buffer
void FFT_Start()
{
	// setup Timer
	FFT_SetupTimer();

	// setup DMA channel ... must be prepared before ADC setup
	FFT_SetupDMA();

	// setup ADC ... must be prepared after DMA setup
	FFT_SetupADC();

	// clear trasmission completion flag ... must be cleared after ADC setup, buf before DMA start transfer
	DMA1_CompClr(FFT_DMA);

	// start DMA transfer
	DMA_ChanEnable(DMA1_Chan(FFT_DMA));

	// ADC enable and wake-up
	ADC1_Enable();
	WaitUs(500);
	ADC1_Enable();	// 2nd ON to start CONT conversion (or it will not start)

	// enable timer
	TIM1_Enable();

	// flip to next buffer
	FFT_BufInx ^= 1;
}

// stop current transfer
void FFT_Stop()
{
#if FFT_CHECK == 5	// DEBUG check: 0=normal mode, 1=disp time [ms], 2=loop time [ms], 3=process time [ms], 4=sample rate [Hz], 5=wait for DMA [ms]
	u32 t1 = Time();
#endif

	// wait for transfer to complete
	while (!DMA1_Comp(FFT_DMA)) { }

#if FFT_CHECK == 5	// DEBUG check: 0=normal mode, 1=disp time [ms], 2=loop time [ms], 3=process time [ms], 4=sample rate [Hz], 5=wait for DMA [ms]
	u32 t2 = Time();
	FFT_Check = (t2 - t1)/(1000*HCLK_PER_US);
#endif

	// stop Timer
	TIM1_Disable();

	// stop DMA
	DMA_ChanDisable(DMA1_Chan(FFT_DMA));

	// disable DMA transfer
	ADC1_DMADisable();

	// clear complete flag
	DMA1_CompClr(FFT_DMA);

	// ADC disable
	ADC1_Disable();
}

#define ENV_ALPHA_ATTACK	(50 << 16)	// attack speed 0..1024 ... higher number = slower attack
#define ENV_ALPHA_DECAY		(500 << 16)	// decay speed 0..1024 ... higher number = slower decay
#define ENV_ALPHA_DECAYLOW	(900 << 16)	// decay speed of low filters 0..1024

#define FFT_DISP_H	(HEIGHT-TITLE_H)	// display height (= 48 lines)

// FFT update
void FFT_Update()
{
	// stop current transfer
	FFT_Stop();

	// start new transfer, flip buffer
	FFT_Start();

#if FFT_CHECK == 4	// DEBUG check: 0=normal mode, 1=disp time [ms], 2=redraw FPS, 3=process time [ns], 4=sample rate [Hz]
	u32 t1 = Time();
	while (!DMA1_Comp(FFT_DMA)) { }
	u32 t2 = Time();
	u32 dt = t2 - t1;
	FFT_Check = (u32)(((s64)SystemCoreClock * FFT_BUF_SIZE + dt/2) / dt);
#endif

#if FFT_CHECK == 3	// DEBUG check: 0=normal mode, 1=disp time [ms], 2=redraw FPS, 3=process time [ns]
	u32 t1 = Time();
#endif

	// clear filters
	sBiquad* f = FFT_Bands;
	int i;
	for (i = N_BANDS; i > 0; i--)
	{
		f->max = 0;
		f++;
	}

	// process buffer
	FFT_AddSamples();

	// filter level using exponential moving average (EMA) of the sample: level = alpha*level + (1-alpha)*y
	f = FFT_Bands;
	for (i = 0; i < N_BANDS; i++)
	{
		int y = f->max;
		int level = f->level;
		s64 acc;
		if (y > level) // attack
		{
			acc = (s64)ENV_ALPHA_ATTACK * level;
			acc += (s64)(B26 - ENV_ALPHA_ATTACK) * y;	// (1-alpha)*y
		}
		else // decay
		{
			if (i <= 2) // lowest 2 filters
			{
				acc = (s64)ENV_ALPHA_DECAYLOW * level;
				acc += (s64)(B26 - ENV_ALPHA_DECAYLOW) * y;	// (1-alpha)*y
			}
			else
			{
				acc = (s64)ENV_ALPHA_DECAY * level;
				acc += (s64)(B26 - ENV_ALPHA_DECAY) * y;	// (1-alpha)*y
			}
		}

		// get result
		y = acc >> 26;	// y is Q26
		f->level = y;

		// convert to range 0..1023
		y >>= 15;	// y = 0..2048
		y -= 20;	// cut-out some noise
		if (y > 1023) y = 1023; // slightly overdriven to boost weak signals

		// nonlinear conversion for small amplification of weak levels
		if (y >= 1024/4)
			y = (y - 1024/4) / 3 * 2 + 1024/2;
		else
			y *= 2;

		// convert to display range
		int lev = ((y*FFT_DISP_H + 512) >> 10);
		if (lev < 0) lev = 0;
		if (lev > FFT_DISP_H) lev = FFT_DISP_H;
		f->disp = lev;

		f++;
	}

#if FFT_CHECK == 3	// DEBUG check: 0=normal mode, 1=disp time [ms], 2=redraw FPS, 3=process time [ns]
	u32 t2 = Time();
	FFT_Check = (t2 - t1)/(1000*HCLK_PER_US);
#endif
}

// FFT display title
void FFT_DispTitle()
{
	// update title
	FFT_TitleUpdate = False;

	// repaint page header
	PageHead();

	// draw HOLD flag
	if (FFT_Hold)
	{
		SelFont12();
		DrawRectClr(PageX, ROW0_Y, WIDTH-1-PageX, FONTH);
		DrawText("HOLD", (WIDTH-1-PageX-4*8)/2+PageX, ROW0_Y);
	}

#if FFT_CHECK	// DEBUG check: 0=normal mode, 1=Disp time [ms]
	SelFont12();
	DrawRectClr(PageX, ROW0_Y, WIDTH-1-PageX, FONTH);
	int len = DecNum(DecNumBuf, FFT_Check, '\'');
	DrawText(DecNumBuf, (WIDTH-1-PageX-len*8)/2+PageX, ROW0_Y);
#endif
}

#define FFT_DISP_DX	8	// X increment (total 16*8 = 128 pixels)

#if FFT_CHECK	// DEBUG check: 0=normal mode, 1=Disp time [ms]
int FFT_CheckUpdate = 0;
#endif

// FFT display (takes up to 57 ms)
void FFT_Disp()
{
	int x, y, h, i, oldx, oldy;
	u8* d;

#if FFT_CHECK	// DEBUG check: 0=normal mode, 1=Disp time [ms]
	FFT_CheckUpdate++;
	if (FFT_CheckUpdate > 10)
	{
		FFT_TitleUpdate = True;
		FFT_CheckUpdate = 0;
	}
#endif

#if FFT_CHECK == 1	// DEBUG check: 0=normal mode, 1=Draw time, 2=Disp time
	u32 t1 = Time();
#endif

	// FFT display title
	if (FFT_TitleUpdate) FFT_DispTitle();

	// clear display
	memset(&FrameBuf[TITLE_H*WIDTHBYTE], 0, FRAMESIZE-TITLE_H*WIDTHBYTE);

	// display marks: 100Hz band 4, 1kHz band 9, 10kHz band 14
	d = &FrameBuf[WIDTHBYTE + FRAMESIZE];
	for (i = FFT_DISP_H/3; i > 0; i--)
	{
		d -= 3*WIDTHBYTE;
		d[4] = 0x10;
		d[9] = 0x10;
		d[14] = 0x10;
	}

	// pointer to filters
	sBiquad* f = FFT_Bands;

	// line mode
	if (FFT_Mode == FFT_MODE_LINE)
	{
		// 16 points (in middle of the bands) with distance 8 pixels = total width 121 pixels, 7 pixels left
		x = 3;
		for (i = N_BANDS; i > 0; i--)
		{
			// get Y coordinate
			y = f->disp;
			y = HEIGHT - 1 - y;

			// draw line - not for 1st sample
			if (x > 3)
			{
				DrawLineSetFast(oldx, oldy, x, y);
			}

			// save coordinates
			oldx = x;
			oldy = y;

			// shift to next filter
			x += FFT_DISP_DX;
			f++;
		}
	}

	// filled mode
	else if (FFT_Mode == FFT_MODE_FILL)
	{
		// 16 bars of width 7 pixels + 1 pixel space = 127 pixels, 1 pixel left
		for (i = 0; i < N_BANDS; i++)
		{
			// get bar height
			h = f->disp;

			// draw rectangle
			d = &FrameBuf[i + FRAMESIZE];
			for (; h > 0; h--)
			{
				d -= WIDTHBYTE;
				*d = 0xfe;
			}

			// shift to next filter
			f++;
		}
	}

	// bar mode
	else // FFT_MODE_BARS
	{
		// 16 bars of width 7 pixels + 1 pixel space = 127 pixels, 1 pixel left
		for (i = 0; i < N_BANDS; i++)
		{
			// get bar height, rounded to muptiply of 3 lines (max. height 16 segments, 48 lines)
			h = f->disp/3;
			if (h < 0) h = 0;
			if (h > FFT_DISP_H/3) h = FFT_DISP_H/3;
			y = h * 3;

			// draw rectangle
			d = &FrameBuf[i + FRAMESIZE + WIDTHBYTE];
			for (; h > 0; h--)
			{
				d -= 3*WIDTHBYTE;
				*d = 0xfe;
				d[WIDTHBYTE] = 0xfe;
			}

			// peak
			if (y > FFT_DISP_H) y = FFT_DISP_H;
			if ((y >= f->peak) || (f->peaklive == 0))
			{
				f->peak = y;
				f->peaklive = 20;
			}
			else
			{
				if (f->peaklive > 0)
				{
					y = f->peak;
					d = &FrameBuf[i + FRAMESIZE - y*WIDTHBYTE];
					*d = 0x54;
					f->peaklive--;
				}
			}

			// shift to next filter
			f++;
		}
	}

	// display update
	DispUpdate();

#if FFT_CHECK == 1	// DEBUG check: 0=normal mode, 1=Disp time [ms]
	u32 t2 = Time();
	FFT_Check = (t2 - t1 + HCLK_PER_US*500)/(HCLK_PER_US*1000);
#endif
}

// Page FFT (returns key PREV/NEXT)
u8 PageFFT()
{
	int i;
	u8 key;
	u32 t1, t2;

	// clear data buffer, to start with zero signal
	memset(Buf, 0, BUF_SIZE);

	// clear filters
	FFT_Hold = False;
	sBiquad* f = FFT_Bands;
	for (i = N_BANDS; i > 0; i--)
	{
		f->x1 = 0;
		f->x2 = 0;
		f->y1 = 0;
		f->y2 = 0;
		f->level = 0;
		f->peak = 0;
		f->peaklive = 0;
		f++;
	}

	// FFT initialize
	FFT_Init();

	// start new transfer, flip buffer
	FFT_Start();

	// FFT display
	FFT_TitleUpdate = True;	// update title
	FFT_Disp();

#if FFT_CHECK == 2	// DEBUG check: 0=normal mode, 1=disp time [ms], 2=redraw FPS
	t1 = Time();
#endif

	while (True)
	{
		// reload watchdog counter
		IWDG_Reload();

#if FFT_CHECK == 2	// DEBUG check: 0=normal mode, 1=disp time [ms], 2=redraw FPS
		t2 = Time();
		FFT_Check = (t2 - t1)/(1000*HCLK_PER_US);
		t1 = t2;
#endif

		// update if not holding
		if (!FFT_Hold)
		{
			// FFT display
			FFT_Disp();

			// FFT update
			FFT_Update();
		}

		// keynoard 
		key = KeyGet();
		switch (key)
		{
		// Prev
		case KEY_PREV:
		// Next
		case KEY_NEXT:
			// stop current transfer
			FFT_Stop();
			// FFT terminate
			FFT_Term();
			return key;

		// Fast - next mode
		case KEY_FAST:
			i = FFT_Mode+1;
			if (i >= FFT_MODE_NUM) i = 0;
			FFT_Mode = i;
			FFT_TitleUpdate = True;	// update title
			FFT_Disp();
			break;

		// Slow - previous mode
		case KEY_SLOW:
			i = FFT_Mode-1;
			if (i < 0) i = FFT_MODE_NUM-1;
			FFT_Mode = i;
			FFT_TitleUpdate = True;	// update title
			FFT_Disp();
			break;

		// Hold
		case KEY_HOLD:
			FFT_Hold = !FFT_Hold;
			// do not redraw display, hold current display content - update only title
			FFT_DispTitle();
			DispUpdate();
			break;
		}
	}
}
