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

		SV_AGC.c

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



#include"SV_AGC.h"

#include <math.h>



#ifdef Debug_File_Write_C

#include <stdio.h>

extern FILE *fp_agc;

#endif



// tuning variables +++++++++++++++++++++++

const int xknee_peak_preAGC[PEAKPreAGC_KNEETABLE_LEN] =

{ -90, -3, 0, 6 };

const int yknee_peak_preAGC[PEAKPreAGC_KNEETABLE_LEN] =

{ -90, -3, -1, -1 };



const float attack_peakPreAGC[ATTACK_RELEASE_LEN] =

{ 2.0f, 30.0f, 10.0f };

const float release_peakPreAGC[ATTACK_RELEASE_LEN] =

{ 5.0f, 500.0f, 10.0f };



// Processing

INT64 iir_updown_int64(INT64 in_data, INT64 state, int tau_up, int tau_down)

{

	int tau;

	if (in_data > state)

		tau = tau_up;

	else

		tau = tau_down;



	INT64 s = ((INT64)state * tau +

		(INT64)in_data * ((1 << QTAU) - tau)) >> QTAU;



	return s;

}



int preAGC_MaxABS(const int* in, int frame_n)

{

	int maxabs = 0;



	do {

		int data = (*in++);

		if (data < 0) data = -data;

		if (maxabs < data) maxabs = data;

	} while (--frame_n > 0);



	return maxabs;

}



INT64 preAGC_MaxABS_int64(const INT64* in, int frame_n)

{

	INT64 maxabs = 0;



	do

	{

		INT64 data = (*in++);

		if (data < 0) data = -data;

		if (maxabs < data) maxabs = data;

	} while (--frame_n > 0);



	return maxabs;

}



float pieceLinear(float in_dB, SV_AGC_T* p_struct)

{

	if (p_struct->kneeLen <= 0)

		return in_dB;



	// look up first i which is greater than xknee

	int i = 0;

	while ((in_dB > p_struct->xknee_tbl[i]) && (i < p_struct->kneeLen))

	{

		i++;

	}



	if (i == 0)

		return ((in_dB - p_struct->xknee_tbl[0]) + p_struct->yknee_tbl[0]);

	if (i > p_struct->kneeLen-1)

		return (float)p_struct->yknee_tbl[p_struct->kneeLen - 1];



	int x0 = p_struct->xknee_tbl[i - 1], x1 = p_struct->xknee_tbl[i];

	int y0 = p_struct->yknee_tbl[i - 1], y1 = p_struct->yknee_tbl[i];

	return ((float)(y1 - y0) / (x1 - x0) * (in_dB - x0) + y0);

}



int GetGainForPeakPreAGC(const int* in1, const int* in2, int frame_n, SV_AGC_T* p_struct)

{

	int gain_dec_peak = p_struct->gain_dec_sm;

	int gain_dec_peak_prev = p_struct->gain_dec_sm_prev;

	float in_dB;

	int curr_maxabs1 = preAGC_MaxABS(in1, frame_n);

	int curr_maxabs2 = preAGC_MaxABS(in2, frame_n);

	int curr_maxabs = MAX(curr_maxabs1, curr_maxabs2);

	if (curr_maxabs <= 0)

		in_dB = -90.0f;

	else

		in_dB = 20 * Log10(curr_maxabs / (float)(1 << (QINPCM + NOISEBITS)));



	in_dB = MAX(-90.0f, in_dB);



	// peak based  long term agc

	p_struct->longtermVal = iir_updown_int64(curr_maxabs,

		p_struct->longtermVal,

		p_struct->tau_release_longterm,

		p_struct->tau_attack_longterm);



	p_struct->longterm_buf[(p_struct->longterm_idx)++] = p_struct->longtermVal;



	if (p_struct->longterm_idx >= p_struct->longtermBuflen)

		p_struct->longterm_idx = 0;



	INT64 longterm_maxval = preAGC_MaxABS_int64(p_struct->longterm_buf, p_struct->longtermBuflen);

	float in_longterm_maxval_dB;

	if (longterm_maxval <= 0)

		in_longterm_maxval_dB = -90;

	else

		in_longterm_maxval_dB = 20 * Log10((float)longterm_maxval / (float)((INT64)1 << (QINPCM + NOISEBITS)));



	in_longterm_maxval_dB = MAX(-90, in_longterm_maxval_dB);



	float out_longterm_dB = pieceLinear(in_longterm_maxval_dB, p_struct);

	int gain_longterm_times = (int)(pow(10.0f, (out_longterm_dB - in_longterm_maxval_dB) / 20.0f)*(1 << QGAIN_L));



	int gain_cmb = gain_longterm_times;

	int gain_var = MIN(gain_cmb, p_struct->gain_dec_sm_prev);



	// MadOnion : should be checked !!!

	// p_struct->gain_dec_sm_prev = gain_var; 

	p_struct->gain_dec_sm_prev = gain_cmb; // always update



#ifdef Debug_File_Write_C

	fprintf(fp_agc, "%f %f %f %f\n", (float)longterm_maxval, in_longterm_maxval_dB, out_longterm_dB, (float)gain_var);

#endif		



	return gain_var;

}



void preAGC_ApplyGain_2ch_Direct(int *inout1, int *inout2, int gain_start, int delta_gain, int n)

{

	int gain = gain_start;



	do {

		int data1 = (*inout1);         // read new data

		int data2 = (*inout2);         // read new data



		// apply gain

		data1 = (int)(((INT64)data1*gain) >> QGAIN_L);

		data2 = (int)(((INT64)data2*gain) >> QGAIN_L);

		

		(*inout1++) = data1;

		(*inout2++) = data2;



		// update gain

		gain += delta_gain;

	} while (--n > 0);



	return;

}



void SV_AGC_2ch_Exe(int* inout1, int* inout2, int frame_n, SV_AGC_T* p_struct)

{



	int gain_start = p_struct->gain_var;



	// In case of considering RMS, put codes here

	int newgain_var = GetGainForPeakPreAGC(inout1, inout2, frame_n, p_struct);

	int delta_gain = (newgain_var - gain_start) / frame_n;



	preAGC_ApplyGain_2ch_Direct(inout1, inout2, gain_start, delta_gain, frame_n);



	p_struct->gain_var = newgain_var;



	return;

}



#define	PREAGC_FRAME				256

#define QTAU						(24-8)



// Init

int Rounding(float d)

{

	return (int)(d + 0.5f);

}



int preAGC_Rounding(float time, int fs)

{

	return Rounding(expf(-2.2f * PREAGC_FRAME / fs / (time * 1e-3f)) * (1 << QTAU));

}



void init_attack_release(int* tau_att, int* tau_rel, int tbl_idx, SV_AGC_T* p_struct)

{

	int fs = p_struct->samplerate;

	float attack_time = p_struct->attack_tbl[tbl_idx];

	float release_time = p_struct->release_tbl[tbl_idx];



	*(tau_att) = preAGC_Rounding(attack_time, fs);

	*(tau_rel) = preAGC_Rounding(release_time, fs);



	return;

}



extern void SV_AGC_Init(int Fs, SV_AGC_T* p_struct)

{

	int i;



	p_struct->samplerate = Fs;

	p_struct->kneeLen = PEAKPreAGC_KNEETABLE_LEN;

	p_struct->lookForwardTime = LOOKFORWARD_TIME;



	// setup agc knee table

	for (i = 0; i < PEAKPreAGC_KNEETABLE_LEN; i++)

	{

		p_struct->xknee_tbl[i] = xknee_peak_preAGC[i];

		p_struct->yknee_tbl[i] = yknee_peak_preAGC[i];

	}



	// set up attack release time 

	for (i = 0; i < ATTACK_RELEASE_LEN; i++)

	{

		p_struct->attack_tbl[i] = attack_peakPreAGC[i];

		p_struct->release_tbl[i] = release_peakPreAGC[i];

	}



	// only longterm tau used, will be removed soon.

	init_attack_release(&(p_struct->tau_attack), &(p_struct->tau_release), 0, p_struct);



	p_struct->gain_dec_sm_prev = 1 << QGAIN_L;

	p_struct->gain_var = 1 << QGAIN_L;

	p_struct->gain_dec_sm = 1 << QGAIN_L;



	// delay buffer setup +++++++++++++++++++++++++++++++++++++++++++

	// DelayBuf_T.dly, DelayBuf_T.p_wrptr, DelayBuf_T.dly_buf

	int dly_in_samples = Rounding(p_struct->samplerate * p_struct->lookForwardTime / 1e3f);

	p_struct->delay_buf_struct.dly = dly_in_samples;



	memset(p_struct->delay_buf_struct.dly_buf, 0, sizeof(p_struct->delay_buf_struct.dly_buf));

	p_struct->delay_buf_struct.p_wrptr = p_struct->delay_buf_struct.dly_buf;



	// longterm setup +++++++++++++++++++++++++++++++++++++++++++++++

	// longtermBuflen, longtermVal, longterm_idx

	// tau_attack_longterm, tau_release_longterm

	p_struct->longterm_idx = 0;

	p_struct->longtermBuflen = LONGTERMBUFLEN_MAX * p_struct->samplerate / MAX_FS;

	for (i = 0; i < p_struct->longtermBuflen; i++)

	{

		p_struct->longterm_buf[i] = 1 << (8);

	}

	init_attack_release(&(p_struct->tau_attack_longterm), &(p_struct->tau_release_longterm), 2, p_struct);



	return;

}