/*---------------------------------------------------------------------------*/
/* freqaecf.cpp                                                              */
/* copyright (c) innovaphone 2013                                            */
/*                                                                           */
/*---------------------------------------------------------------------------*/

#define FREQAECF_CPP_

#include "platform.h"
#include "fft.h"
#include "sdsputil.h"
#include "freqaecf.h"
#ifdef __ANDROID__
void sdsp_printf(const char *format,...);
#endif

#define FILE_ "freqaecf.cpp"

#define FREQAEC_TRACE_

NAMESPACE_BEGIN

/*----------------------------------------------------------------------------*/

#define FREQAEC_MU                      ((float) 40)
#define FREQAEC_RIN_ENERGY_BIAS         ((float)(400000 >> 1))

#define FREQAEC_MAX_LOOP_GAIN_MARGIN    0x7fff
#define FREQAEC_MIN_LOSS_LIMIT          0x0400

#define FREQAEC_PROPORTIONATE_EXP       4
#define FREQAEC_PROPORTIONATE_BIAS      ((float)(((float) 0x2000) / 32768.0))

#define FREQAEC_LOSS_ENHANCE_SLOPE      0x1000
#define FREQAEC_LOSS_ENHANCE_SLOPE_RANGE_EXP 6
#define FREQAEC_SQR_STEP_SIZE_CONSTANT  ((float)(((float) 0x2000) / 32768.0))
#define FREQAEC_DOUBLE_TALK_PENALTY_EXP 6
#define FREQAEC_DOUBLE_TALK_HANGOVER    2

#define FREQAEC_MIN_NLP_RETURN_LOSS     0x3000
#define FREQAEC_MIN_NLP_LOSS_LIMIT      0x1000
#define FREQAEC_SQR_NLP_LOSS_CONSTANT   ((float)(((float) 0x1000) / 32768.0))
#define FREQAEC_MAX_NLP_ATTENUATION_EXP 8
#define FREQAEC_MAX_NOISE_ATTENUATION_EXP 5
#define FREQAEC_NOISE_SHAPE_GAIN        0x0800
#define FREQAEC_ATTENUATION_SHAPE_GAIN  0x1000
#define FREQAEC_NLP_SHAPE_GAIN          0x4000
#define FREQAEC_MUSIC_SHAPE_GAIN        0x1000
#define FREQAEC_FILTER_MASKING_THRESHOLD 0x0040

#define FREQAEC_PEER_LOSS_ENHANCE       0x00008000
#define FREQAEC_LOSS_TRACE_GRAIN        16

#define FREQAEC_ENERGY_SHIFT            7
#define FREQAEC_GAUGE_GAIN              0x0100
#define FREQAEC_GAUGE_MARGIN            0x7fff


void freqaec_init (t_freqaec *aec, word frame_length, word tail_frames, word options,
    dword nominal_return_loss, word max_loop_gain, word noise_suppress_level)
{
	unsigned int i;
    float f;
    dword d;

    aec->frame_length = frame_length;
    aec->tail_frames = tail_frames;
    aec->options = options;
    aec->rin_sample_counter = 0;
	aec->rin_buffer_counter = 0;
    aec->min_sqr_loss_enhance = ((float) 1.0) / ((float)(((float)(((float) nominal_return_loss) / ((float) 32768.0))) * ((float)(((float) nominal_return_loss) / ((float) 32768.0)))));
    aec->max_sqr_loss_enhance = aec->min_sqr_loss_enhance / ((float)(((float)(((float) FREQAEC_MIN_LOSS_LIMIT) / ((float) 32768.0))) * ((float)(((float) FREQAEC_MIN_LOSS_LIMIT) / ((float) 32768.0)))));
    aec->max_sqr_nlp_loss_enhance = aec->min_sqr_loss_enhance / ((float)(((float)(((float) FREQAEC_MIN_NLP_LOSS_LIMIT) / ((float) 32768.0))) * ((float)(((float) FREQAEC_MIN_NLP_LOSS_LIMIT) / ((float) 32768.0)))));
    f = ((float)(((float) nominal_return_loss) / ((float) 32768.0))) * ((float)(((float) FREQAEC_MIN_NLP_RETURN_LOSS) / ((float) 32768.0)));
    aec->min_sqr_nlp_return_loss = f * f;
    for (i = 0; i < aec->frame_length; i++)
        aec->sqr_loss_per_freq_buffer[i] = aec->min_sqr_loss_enhance;
    aec->max_loop_gain = (word)(((dword)(((dword) max_loop_gain) * ((dword) FREQAEC_MAX_LOOP_GAIN_MARGIN))) >> 15);
    aec->noise_suppress_level = noise_suppress_level;
    d = ((dword)(((dword) max_loop_gain) * ((dword)(sdsp_sqrt((dword)(aec->min_sqr_loss_enhance * ((float) 32768.0))))))) << 1;
    aec->last_attenuation = (d > 0x00800000) ? 0x8000 : (word)(d >> 8);
    aec->nlp_attenuation = 0;
    aec->random_number_seed = 0;
    for (i = 0; i < aec->tail_frames; i++) {
        aec->filter_masking_schedule[i] = 0;
    }

    freqaec_flush (aec, 0);

    aec->echo_peak_pos = 0;
    aec->echo_peak_amplitude = 0;
    aec->trace_tag = 0;

    FFTInit ();
}


void freqaec_flush (t_freqaec *aec, word preserve_filter_frames)
{
	unsigned int filter_offs, delay_counter, i;
/*
    float e;
*/

    aec->rin_dc_offset = 0;
    aec->sin_dc_offset = 0;
    memset (aec->rin_frame_buffer, 0, aec->frame_length * sizeof(short));
	memset (aec->sin_frame_buffer, 0, aec->frame_length * sizeof(short));
	memset (aec->sout_frame_buffer, 0, aec->frame_length * sizeof(short));
	for (i = 0; i < aec->tail_frames * aec->frame_length; i++)
	{
		aec->rin_delay_buffer[i].Re = (float) 0;
		aec->rin_delay_buffer[i].Im = (float) 0;
        aec->rin_energy_buffer[i] = (float) 0;
	}
/*
    for (i = 0; i < aec->frame_length; i++)
    {
        e = (float) 0;
        for (delay_counter = 0; delay_counter + preserve_filter_frames < aec->tail_frames; delay_counter++)
        {
    	    filter_offs = delay_counter * aec->frame_length;
            if (aec->filter_buffer[filter_offs + i].Re >= 0) {
                e += aec->filter_buffer[filter_offs + i].Re;
            }
            else {
                e -= aec->filter_buffer[filter_offs + i].Re;
            }
            if (aec->filter_buffer[filter_offs + i].Im >= 0) {
                e += aec->filter_buffer[filter_offs + i].Im;
            }
            else {
                e -= aec->filter_buffer[filter_offs + i].Im;
            }
            aec->filter_buffer[filter_offs + i].Re = (float) 0;
		    aec->filter_buffer[filter_offs + i].Im = (float) 0;
	    }
        if (e != 0) {
            e *= 2.0 * aec->frame_length;
            e = ((float) 1.0) / (e * e);
            if (e < aec->sqr_loss_per_freq_buffer[i]) {
                aec->sqr_loss_per_freq_buffer[i] = e;
                if (aec->sqr_loss_per_freq_buffer[i] < aec->min_sqr_loss_enhance)
                    aec->sqr_loss_per_freq_buffer[i] = aec->min_sqr_loss_enhance;
            }
        }
    }
*/
    for (delay_counter = 0; delay_counter + preserve_filter_frames < aec->tail_frames; delay_counter++)
    {
	    filter_offs = delay_counter * aec->frame_length;
        for (i = 0; i < aec->frame_length; i++)
        {
            aec->filter_buffer[filter_offs + i].Re = (float) 0;
		    aec->filter_buffer[filter_offs + i].Im = (float) 0;
	    }
    }
    for (i = 0; i < aec->frame_length; i++)
        aec->sqr_loss_per_freq_buffer[i] = aec->min_sqr_loss_enhance;
    for (i = 0; i < aec->frame_length; i++)
        aec->last_dt[i] = 0;

    aec->sout_energy = 0;
    aec->gauge_threshold = 0;
    aec->rin_energy = 0;
    aec->rout_attenuation = 0x8000;
    aec->rout_gain = 0x80000000;
}


short freqaec_exec (t_freqaec *aec, short Rin, short Sin)
{
    float filter_energy_buffer[FREQAEC_MAX_TAIL_FRAMES];
    float unbiased_sqr_proportionate_buffer[FREQAEC_MAX_TAIL_FRAMES];
    float proportionate_buffer[FREQAEC_MAX_TAIL_FRAMES];
#ifdef FREQAEC_TRACE_
    static char hex_digit_table[] = {'0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F'};
    unsigned int proportionate_exp_buffer[FREQAEC_MAX_TAIL_FRAMES];
    char proportionate_string[FREQAEC_MAX_TAIL_FRAMES + 1];
    char loss_per_freq_string[FREQAEC_LOSS_TRACE_GRAIN + 1];
#endif
	unsigned int rin_offs, filter_offs, delay_counter, dt_detects;
    float e, f, g, h, max_filter_energy, max_filter_energy2, bias, rin_energy_bias, rin_energy, unbiased_rin_energy, max_rin, energy_quot;
    float unbiased_energy_quot, filter_masking_max, filter_masking_error;
    short s, Sout;
    word x, attenuation, max_loop_gain, max_loop_gain_peer;
    dword d, least_attenuation, attenuation_sum;
    int32 l, dc_offset;
	unsigned int i, j, k, two_frame_length;
    byte dt, nlp;

    two_frame_length = 2 * aec->frame_length;
    Sout = aec->sout_frame_buffer[aec->rin_sample_counter];
    aec->sout_frame_buffer[aec->rin_sample_counter] = aec->rin_frame_buffer[aec->rin_sample_counter];
    aec->rin_frame_buffer[aec->rin_sample_counter] = Rin;
    aec->sin_frame_buffer[aec->rin_sample_counter] = Sin;
    (aec->rin_sample_counter)++;
    if (aec->rin_sample_counter == aec->frame_length)
    {
        aec->rin_sample_counter = 0;
        for (i = 0; i < aec->frame_length; i++)
        {
            aec->fft_buffer[two_frame_length - 1 - i].Re = (float)(aec->sout_frame_buffer[i]);
            aec->fft_buffer[two_frame_length - 1 - i].Im = (float) 0;
        }

        dc_offset = aec->rin_dc_offset;
        for (i = 0; i < aec->frame_length; i++)
	    {
            l = aec->rin_frame_buffer[i] - (dc_offset >> 16);
            if (l < -32768)
                l = -32768;
            else if (l > 32767)
                l = 32767;
            dc_offset += l * ((int32)(2 * 0x0080));
            aec->rin_frame_buffer[i] = (short) l;
        }
        aec->rin_dc_offset = dc_offset;

        for (i = 0; i < aec->frame_length; i++)
	    {
		    aec->fft_buffer[aec->frame_length - 1 - i].Re = (float)(aec->rin_frame_buffer[i]);
		    aec->fft_buffer[aec->frame_length - 1 - i].Im = (float) 0;
	    }

		FFTCalc (aec->fft_buffer, two_frame_length);
	
		rin_offs = aec->rin_buffer_counter * aec->frame_length;
		aec->rin_delay_buffer[rin_offs].Re = aec->fft_buffer[0].Re / ((float) 2.0);
		aec->rin_delay_buffer[rin_offs].Im = aec->fft_buffer[aec->frame_length].Re / ((float) 2.0);
		for (i = 1; i < aec->frame_length; i++)
		{
			aec->rin_delay_buffer[rin_offs + i].Re = aec->fft_buffer[i].Re;
			aec->rin_delay_buffer[rin_offs + i].Im = aec->fft_buffer[i].Im;
		}
		for (i = 0; i < aec->frame_length; i++)
		{
            aec->rin_energy_buffer[rin_offs + i] =
                ((float)(aec->rin_delay_buffer[rin_offs + i].Re * aec->rin_delay_buffer[rin_offs + i].Re)) +
                ((float)(aec->rin_delay_buffer[rin_offs + i].Im * aec->rin_delay_buffer[rin_offs + i].Im));
        }
		aec->rin_buffer_counter = (aec->rin_buffer_counter == aec->tail_frames - 1) ? 0 : aec->rin_buffer_counter + 1;

        dc_offset = aec->sin_dc_offset;
        for (i = 0; i < aec->frame_length; i++)
        {
            l = aec->sin_frame_buffer[i] - (dc_offset >> 16);
            if (l < -32768)
                l = -32768;
            else if (l > 32767)
                l = 32767;
            dc_offset += l * ((int32)(2 * 0x0080));
            aec->sin_frame_buffer[i] = (short) l;
        }
        aec->sin_dc_offset = dc_offset;

        for (i = 0; i < aec->frame_length; i++)
        {
            aec->fft_buffer[i].Re = (float) 0;
            aec->fft_buffer[i].Im = (float) 0;
        }
	    for (delay_counter = 0; delay_counter < aec->tail_frames; delay_counter++)
	    {
            rin_offs = aec->rin_buffer_counter + delay_counter;
		    if (rin_offs >= aec->tail_frames)
			    rin_offs -= aec->tail_frames;
		    rin_offs *= aec->frame_length;
		    filter_offs = delay_counter * aec->frame_length;

            aec->fft_buffer[0].Re += aec->filter_buffer[filter_offs].Re * aec->rin_delay_buffer[rin_offs].Re;
		    aec->fft_buffer[0].Im += aec->filter_buffer[filter_offs].Im * aec->rin_delay_buffer[rin_offs].Im;
		    for (i = 1; i < aec->frame_length; i++)
		    {
			    aec->fft_buffer[i].Re +=
				    (float)(((float)(aec->filter_buffer[filter_offs + i].Re * aec->rin_delay_buffer[rin_offs + i].Re)) -
				    ((float)(aec->filter_buffer[filter_offs + i].Im * aec->rin_delay_buffer[rin_offs + i].Im)));
			    aec->fft_buffer[i].Im +=
				    (float)(((float)(aec->filter_buffer[filter_offs + i].Re * aec->rin_delay_buffer[rin_offs + i].Im)) +
				    ((float)(aec->filter_buffer[filter_offs + i].Im * aec->rin_delay_buffer[rin_offs + i].Re)));
		    }
	    }
        aec->fft_buffer[0].Re *= ((float) 2.0);
        aec->fft_buffer[aec->frame_length].Re = aec->fft_buffer[0].Im * ((float) 2.0);
        aec->fft_buffer[0].Im = 0;
        aec->fft_buffer[aec->frame_length].Im = 0;
        for (i = 1; i < aec->frame_length; i++)
        {
            aec->fft_buffer[two_frame_length - i].Re = aec->fft_buffer[i].Re;
            aec->fft_buffer[two_frame_length - i].Im = -aec->fft_buffer[i].Im;
        }

        FFTCalc (aec->fft_buffer, two_frame_length);

        f = (float)(((float)(aec->fft_buffer[0].Re / ((float)(1.0 * 32768.0)))) + ((float) 0.5));
        if (f > ((float) 32767.0))
            aec->filter_result_buffer[0] = 32767;
        else if (f < ((float) -32768.0))
            aec->filter_result_buffer[0] = -32768;
        else
            aec->filter_result_buffer[0] = (short) f;
        for (i = 1; i < aec->frame_length; i++)
        {
            f = (float)(((float)(aec->fft_buffer[i].Re / ((float) 32768.0))) + ((float) 0.5));
            if (f > ((float) 32767.0))
                aec->filter_result_buffer[i] = 32767;
            else if (f < ((float) -32768.0))
                aec->filter_result_buffer[i] = -32768;
            else
                aec->filter_result_buffer[i] = (short) f;
        }

	    for (i = 0; i < aec->frame_length; i++)
	    {
            l = aec->sin_frame_buffer[i] - aec->filter_result_buffer[i];
            if (l > 32767)
                aec->sout_frame_buffer[i] = 32767;
            else if (l < -32768)
                aec->sout_frame_buffer[i] = -32768;
            else
                aec->sout_frame_buffer[i] = (short) l;
        }

        for (i = 0; i < aec->frame_length; i++)
	    {
            aec->fft_buffer[i].Re = (float)(aec->sout_frame_buffer[i]);
		    aec->fft_buffer[i].Im = (float) 0;
		    aec->fft_buffer[aec->frame_length + i].Re = (float) 0;
		    aec->fft_buffer[aec->frame_length + i].Im = (float) 0;
	    }

	    FFTCalc (aec->fft_buffer, two_frame_length);

        aec->eout_freq_buffer[0].Re = aec->fft_buffer[0].Re / ((float) 2.0);
        aec->eout_freq_buffer[0].Im = aec->fft_buffer[aec->frame_length].Re / ((float) 2.0);
        for (i = 1; i < aec->frame_length; i++)
	    {
            aec->eout_freq_buffer[i].Re = aec->fft_buffer[i].Re;
            aec->eout_freq_buffer[i].Im = aec->fft_buffer[i].Im;
        }
        aec->fft_buffer[0].Im = aec->fft_buffer[aec->frame_length].Re;

        max_filter_energy = 0;
        max_filter_energy2 = 0;
        for (delay_counter = 0; delay_counter < aec->tail_frames; delay_counter++)
        {
		    filter_offs = delay_counter * aec->frame_length;
            f = ((float)(((float) 0x0001) / 32768.0));
            for (i = 0; i < aec->frame_length; i++)
	        {
                f += ((float)(aec->filter_buffer[filter_offs + i].Re * aec->filter_buffer[filter_offs + i].Re)) +
                    ((float)(aec->filter_buffer[filter_offs + i].Im * aec->filter_buffer[filter_offs + i].Im));
            }
            filter_energy_buffer[delay_counter] = f;
            if (f > max_filter_energy2) {
                if (f > max_filter_energy) {
                    max_filter_energy2 = max_filter_energy;
                    max_filter_energy = f;
                }
                else {
                    max_filter_energy2 = f;
                }
            }
        }
        unbiased_energy_quot = 1.0 / max_filter_energy;
        h = ((float)(FREQAEC_PROPORTIONATE_BIAS * FREQAEC_PROPORTIONATE_BIAS)) * ((float)(aec->frame_length)) * ((float)(1 << (2 * FREQAEC_PROPORTIONATE_EXP)));
        if (max_filter_energy >= h) {
            if (max_filter_energy < 2.0 * max_filter_energy2) {
                h = max_filter_energy;
            }
            else if (2.0 * max_filter_energy2 > h) {
                h = 2.0 * max_filter_energy2;
            }
        }
        energy_quot = 1.0 / h;
        rin_energy_bias = 0;
        for (delay_counter = 0; delay_counter < aec->tail_frames; delay_counter++)
        {
            if ((filter_energy_buffer[delay_counter] >= h) || ((delay_counter == aec->tail_frames - 1) && (max_filter_energy < h))) {
                proportionate_buffer[delay_counter] = ((float) 0x8000) / ((float) 32768.0);
            }
            else if (((float)(filter_energy_buffer[delay_counter] * ((float)(1 << (2 * FREQAEC_PROPORTIONATE_EXP))))) < h)
            {
                proportionate_buffer[delay_counter] = ((float)(0x8000 >> FREQAEC_PROPORTIONATE_EXP)) / ((float) 32768.0);
            }
            else
            {
                d = (dword)(filter_energy_buffer[delay_counter] * ((float)(energy_quot * ((float)(((float) 32768.0) * ((float) 65536.0))))));
                proportionate_buffer[delay_counter] = ((float)((d >= 0x80000000) ? 0x8000 : sdsp_sqrt(d))) / ((float) 32768.0);
            }
            rin_energy_bias += ((float) FREQAEC_RIN_ENERGY_BIAS) * proportionate_buffer[delay_counter];
            d = (dword)(filter_energy_buffer[delay_counter] * ((float)(unbiased_energy_quot * ((float)(((float) 32768.0) * ((float) 65536.0))))));
            unbiased_sqr_proportionate_buffer[delay_counter] = ((float)((d >= 0x80000000) ? 0x8000 : sdsp_sqrt(d))) / ((float) 32768.0);
            if (unbiased_sqr_proportionate_buffer[delay_counter] > proportionate_buffer[delay_counter])
                unbiased_sqr_proportionate_buffer[delay_counter] = proportionate_buffer[delay_counter];
            unbiased_sqr_proportionate_buffer[delay_counter] = unbiased_sqr_proportionate_buffer[delay_counter] * unbiased_sqr_proportionate_buffer[delay_counter];
#ifdef FREQAEC_TRACE_
            if (aec->trace_tag)
            {
                j = 0xf;
                f = 16.0;
                while ((j != 0) && (f < filter_energy_buffer[delay_counter]))
                {
                    f *= 2.0;
                    j--;
                }
                proportionate_exp_buffer[delay_counter] = j;
            }
#endif
        }
        if (aec->noise_suppress_level != 0) {
            for (i = 0; i < aec->frame_length; i++)
            {
                d = (dword)((aec->eout_freq_buffer[i].Re >= 0) ? aec->eout_freq_buffer[i].Re : -aec->eout_freq_buffer[i].Re) +
                    ((aec->eout_freq_buffer[i].Im >= 0) ? aec->eout_freq_buffer[i].Im : -aec->eout_freq_buffer[i].Im);
                s = (1 - sdsp_dword_leading_zeroes (d));
                if (d > (((dword)(aec->noise_suppress_level)) << 2)) {
                    aec->last_dt[i] |= 0x80;
                    aec->attenuations[i] = 0x8000;
                }
                else if (d > ((dword)(aec->noise_suppress_level + (aec->noise_suppress_level >> FREQAEC_MAX_NOISE_ATTENUATION_EXP)))) {
                    d -= aec->noise_suppress_level;
                    s -= (1 - sdsp_dword_leading_zeroes (d));
                    aec->attenuations[i] = 0x8000 >> s;
                }
                else {
                    aec->attenuations[i] = 0x8000 >> FREQAEC_MAX_NOISE_ATTENUATION_EXP;
                }
            }
            attenuation = 0x8000 >> FREQAEC_MAX_NOISE_ATTENUATION_EXP;
            i = aec->frame_length;
            do
            {
                i--;
                if (attenuation <= aec->attenuations[i])
                {
                    aec->shaped_attenuations[i] = attenuation;
                    attenuation += (word)(((dword)(((dword) attenuation) * ((dword) FREQAEC_NOISE_SHAPE_GAIN) + 0x4000)) >> 15);
                }
                else
                {
                    attenuation -= (word)(((dword)(((dword) attenuation) * ((dword) FREQAEC_NOISE_SHAPE_GAIN) + 0x4000)) >> 15);
                    aec->shaped_attenuations[i] = attenuation;
                }
            } while (i != 0);
            attenuation = 0x8000 >> FREQAEC_MAX_NOISE_ATTENUATION_EXP;
            for (i = 0; i < aec->frame_length; i++)
            {
                if (attenuation <= aec->attenuations[i])
                {
                    if (attenuation > aec->shaped_attenuations[i])
                        aec->shaped_attenuations[i] = attenuation;
                    attenuation += (word)(((dword)(((dword) attenuation) * ((dword) FREQAEC_NOISE_SHAPE_GAIN) + 0x4000)) >> 15);
                }
                else
                {
                    attenuation -= (word)(((dword)(((dword) attenuation) * ((dword) FREQAEC_NOISE_SHAPE_GAIN) + 0x4000)) >> 15);
                    if (attenuation > aec->shaped_attenuations[i])
                        aec->shaped_attenuations[i] = attenuation;
                }
            }
        }
        else {
            for (i = 0; i < aec->frame_length; i++) {
                aec->shaped_attenuations[i] = 0x8000;
            }
        }

        if (aec->rout_gain < 0x80000000) {
            x = (word)(aec->rout_gain >> 16);
            s = 1 - sdsp_word_leading_zeroes(x);
            x <<= -s;
            d = ((dword)(0x3fffffff / ((dword) x))) >> 1;
            d <<= 1 - s;
            d = (((dword)((((dword)(d & 0xffff)) * ((dword)(aec->max_loop_gain))))) >> 15) +
                ((((dword)(d >> 16)) * ((dword)(aec->max_loop_gain))) << 1);
            max_loop_gain = (d > 0x8000) ? 0x8000 : (word) d;
        }
        else {
            max_loop_gain = aec->max_loop_gain;
        }
        d = (((dword)((((dword)(FREQAEC_PEER_LOSS_ENHANCE & 0xffff)) * ((dword)(max_loop_gain))))) >> 15) +
            ((((dword)(FREQAEC_PEER_LOSS_ENHANCE >> 16)) * ((dword)(max_loop_gain))) << 1);
        max_loop_gain_peer = (d > 0x8000) ? 0x8000 : (word) d;
        dt_detects = 0;
        attenuation_sum = 0;
        least_attenuation = 0;
        for (i = 0; i < aec->frame_length; i++)
        {
            unbiased_rin_energy = 0;
            bias = rin_energy_bias * aec->sqr_loss_per_freq_buffer[i];
            rin_energy = bias;
            max_rin = 0;
            for (delay_counter = 0; delay_counter < aec->tail_frames; delay_counter++)
            {
                rin_offs = aec->rin_buffer_counter + delay_counter;
                if (rin_offs >= aec->tail_frames)
	                rin_offs -= aec->tail_frames;
                rin_offs *= aec->frame_length;
                f = aec->rin_energy_buffer[rin_offs + i] * unbiased_sqr_proportionate_buffer[delay_counter];
                if (f > max_rin)
                    max_rin = f;
                unbiased_rin_energy += f;
                rin_energy += aec->rin_energy_buffer[rin_offs + i] * proportionate_buffer[delay_counter];
            }
            aec->rin_energy_sum_buffer[i] = unbiased_rin_energy;
            f = ((float)(aec->eout_freq_buffer[i].Re * aec->eout_freq_buffer[i].Re)) + ((float)(aec->eout_freq_buffer[i].Im * aec->eout_freq_buffer[i].Im));
            energy_quot = 1.0 / rin_energy;

            if (aec->last_dt[i] & 0x80) {
                aec->last_dt[i] &= ~0x80;
                dt = ((f > aec->noise_suppress_level) && (((float)(f * aec->min_sqr_loss_enhance)) >= max_rin));
            }
            else {
                dt = false;
            }
            aec->last_dt[i] = dt ? ((FREQAEC_DOUBLE_TALK_HANGOVER != 0) ? FREQAEC_DOUBLE_TALK_HANGOVER : 1) : ((aec->last_dt[i] != 0) ? aec->last_dt[i] - 1 : 0);
            if (aec->last_dt[i] == 0)
            {
                if (!(aec->options & FREQAEC_FREEZE_COEFFICIENTS))
                {
                    if (unbiased_rin_energy * ((float)(((dword) 1) << FREQAEC_LOSS_ENHANCE_SLOPE_RANGE_EXP)) > bias) {
                        d = (dword)(bias / unbiased_rin_energy);
                        x = FREQAEC_LOSS_ENHANCE_SLOPE >> ((31 + (1 - sdsp_dword_leading_zeroes(d))) >> 1);
                    }
                    else {
                        x = FREQAEC_LOSS_ENHANCE_SLOPE >> (FREQAEC_LOSS_ENHANCE_SLOPE_RANGE_EXP >> 1);
                    }
                    if (unbiased_rin_energy >= ((float)(f * aec->sqr_loss_per_freq_buffer[i])))
                    {
                        aec->sqr_loss_per_freq_buffer[i] *= ((float)(0x8000 + x)) / ((float) 32768.0);
                        if (aec->sqr_loss_per_freq_buffer[i] > aec->max_sqr_loss_enhance)
                            aec->sqr_loss_per_freq_buffer[i] = aec->max_sqr_loss_enhance;
                    }
                    else
                    {
                        aec->sqr_loss_per_freq_buffer[i] *= ((float)(0x8000 - x)) / ((float) 32768.0);
                        if (aec->sqr_loss_per_freq_buffer[i] < aec->min_sqr_loss_enhance)
                            aec->sqr_loss_per_freq_buffer[i] = aec->min_sqr_loss_enhance;
                    }
                }
                h = f;
            }
            else
            {
                dt_detects++;
                h = f * ((float)(1 << FREQAEC_DOUBLE_TALK_PENALTY_EXP));
            }

            d = ((dword)(((dword)((aec->last_dt[i] != 0) ? max_loop_gain_peer : max_loop_gain)) * ((dword)(sdsp_sqrt((dword)(aec->sqr_loss_per_freq_buffer[i] * ((float) 32768.0))))))) << 1;
            d >>= 8;
            if (i < aec->frame_length/2) {
                least_attenuation += d;
            }
            attenuation = (d > 0x00008000) ? 0x8000 : (word) d;
            aec->attenuations[i] = attenuation;

            e = (aec->sqr_loss_per_freq_buffer[i] <= aec->max_sqr_nlp_loss_enhance) ? aec->sqr_loss_per_freq_buffer[i] : aec->max_sqr_nlp_loss_enhance;
            if ((aec->options & FREQAEC_ENABLE_NONLINEAR_PROCESSING)
             && (aec->last_dt[i] == 0)
             && (f < ((float)(unbiased_rin_energy * aec->min_sqr_nlp_return_loss)))
             && (((float)(((float)(f * e)) * FREQAEC_SQR_NLP_LOSS_CONSTANT)) < unbiased_rin_energy))
            {
                aec->last_dt[i] |= 0x80;
/*
                if (aec->options & FREQAEC_ENABLE_COMFORT_NOISE_GENERATION)
                {
                    short noise[2];
                    sdsp_fill_uniform_noise (noise, 2, 0x8000, &aec->random_number_seed);
                    e = aec->fft_buffer[i].Re;
                    if (e < 0)
                        e = -e;
                    if (e < aec->fft_buffer[i].Im)
                        e = aec->fft_buffer[i].Im;
                    if (e < -aec->fft_buffer[i].Im)
                        e = -aec->fft_buffer[i].Im;
                    aec->fft_buffer[i].Re = e * (noise[0] / 32768.0);
                    aec->fft_buffer[i].Im = e * (noise[1] / 32768.0);
                    if (attenuation < aec->attenuations[i]) {
                        aec->attenuations[i] = attenuation;
                    }
                }
*/
            }
            attenuation_sum += aec->attenuations[i];
            aec->eout_energy_buffer[i] = ((float)(h * aec->sqr_loss_per_freq_buffer[i])) * FREQAEC_SQR_STEP_SIZE_CONSTANT;
            f = FREQAEC_MU * energy_quot;
            aec->eout_freq_buffer[i].Re *= f;
            aec->eout_freq_buffer[i].Im *= f;
        }
        aec->last_attenuation = (word)(attenuation_sum >> (14 + (1 - sdsp_word_leading_zeroes(aec->frame_length))));

        attenuation_sum = 0;
        if ((aec->options & FREQAEC_ENABLE_NONLINEAR_PROCESSING) || (aec->max_loop_gain != 0x8000) || (aec->noise_suppress_level != 0))
        {
            attenuation = 0x8000;
            i = aec->frame_length;
            do
            {
                i--;
                attenuation += (word)(((dword)(((dword) attenuation) * ((dword)((attenuation <= aec->attenuations[i]) ?
                    FREQAEC_MUSIC_SHAPE_GAIN : -FREQAEC_ATTENUATION_SHAPE_GAIN)) + 0x4000)) >> 15);
                if (attenuation < aec->shaped_attenuations[i])
                    aec->shaped_attenuations[i] = attenuation;
            } while (i != 0);
            attenuation = 0x8000;
            for (i = 0; i < aec->frame_length; i++)
            {
                attenuation += (word)(((dword)(((dword) attenuation) * ((dword)((attenuation <= aec->attenuations[i]) ?
                    FREQAEC_MUSIC_SHAPE_GAIN : -FREQAEC_ATTENUATION_SHAPE_GAIN)) + 0x4000)) >> 15);
                if (attenuation < aec->shaped_attenuations[i])
                    aec->shaped_attenuations[i] = attenuation;
            }
            if (aec->options & FREQAEC_ENABLE_NONLINEAR_PROCESSING) {
                attenuation = 0x8000 >> FREQAEC_MAX_NLP_ATTENUATION_EXP;
                nlp = false;
                i = aec->frame_length - 1;
                do
                {
                    i--;
                    if ((aec->last_dt[i] & ~0x80) != 0) {
                        attenuation += (word)(((dword)(((dword) attenuation) * ((dword) FREQAEC_NLP_SHAPE_GAIN) + 0x4000)) >> 15);
                        if (attenuation > 0x8000) {
                            attenuation = 0x8000;
                        }
                        nlp = false;
                    }
                    else if (((aec->last_dt[i] & 0x80) && (aec->rin_energy_sum_buffer[i] >= aec->rin_energy_sum_buffer[i+1]) && (aec->rin_energy_sum_buffer[i] >= aec->rin_energy_sum_buffer[i-1]))
                     || (nlp && (aec->rin_energy_sum_buffer[i] <= aec->rin_energy_sum_buffer[i+1]))) {
                        attenuation -= (word)(((dword)(((dword) attenuation) * ((dword) FREQAEC_MUSIC_SHAPE_GAIN) + 0x4000)) >> 15);
                        if (attenuation < (0x8000 >> FREQAEC_MAX_NLP_ATTENUATION_EXP)) {
                            attenuation = 0x8000 >> FREQAEC_MAX_NLP_ATTENUATION_EXP;
                        }
                        nlp = true;
                    }
                    else {
                        attenuation += (word)(((dword)(((dword) attenuation) * ((dword) FREQAEC_MUSIC_SHAPE_GAIN) + 0x4000)) >> 15);
                        if (attenuation > 0x8000) {
                            attenuation = 0x8000;
                        }
                        nlp = false;
                    }
                    aec->attenuations[i] = attenuation;
                } while (i > 1);
                attenuation = 0x8000 >> FREQAEC_MAX_NLP_ATTENUATION_EXP;
                nlp = false;
                for (i = 1; i < aec->frame_length - 1; i++)
                {
                    if ((aec->last_dt[i] & ~0x80) != 0) {
                        attenuation += (word)(((dword)(((dword) attenuation) * ((dword) FREQAEC_NLP_SHAPE_GAIN) + 0x4000)) >> 15);
                        if (attenuation > 0x8000) {
                            attenuation = 0x8000;
                        }
                        nlp = false;
                    }
                    else if (((aec->last_dt[i] & 0x80) && (aec->rin_energy_sum_buffer[i] >= aec->rin_energy_sum_buffer[i+1]) && (aec->rin_energy_sum_buffer[i] >= aec->rin_energy_sum_buffer[i-1]))
                     || (nlp && (aec->rin_energy_sum_buffer[i] <= aec->rin_energy_sum_buffer[i-1]))) {
                        attenuation -= (word)(((dword)(((dword) attenuation) * ((dword) FREQAEC_MUSIC_SHAPE_GAIN) + 0x4000)) >> 15);
                        if (attenuation < (0x8000 >> FREQAEC_MAX_NLP_ATTENUATION_EXP)) {
                            attenuation = 0x8000 >> FREQAEC_MAX_NLP_ATTENUATION_EXP;
                        }
                        nlp = true;
                    }
                    else {
                        attenuation += (word)(((dword)(((dword) attenuation) * ((dword) FREQAEC_MUSIC_SHAPE_GAIN) + 0x4000)) >> 15);
                        if (attenuation > 0x8000) {
                            attenuation = 0x8000;
                        }
                        nlp = false;
                    }
                    if (attenuation > aec->attenuations[i]) {
                        aec->attenuations[i] = attenuation;
                    }
                    if (aec->attenuations[i] < aec->shaped_attenuations[i]) {
                        aec->shaped_attenuations[i] = aec->attenuations[i];
                    }
                    attenuation_sum += aec->attenuations[i];
                    aec->last_dt[i] &= ~0x80;
                }
            }
            for (i = 0; i < aec->frame_length; i++)
            {
                f = ((float)(aec->shaped_attenuations[i])) / 32768.0;
                aec->fft_buffer[i].Re *= f;
                aec->fft_buffer[i].Im *= f;
            }

            aec->fft_buffer[aec->frame_length].Re = aec->fft_buffer[0].Im;
            aec->fft_buffer[0].Im = 0;
            aec->fft_buffer[aec->frame_length].Im = 0;
            for (i = 1; i < aec->frame_length; i++)
            {
                aec->fft_buffer[two_frame_length - i].Re = aec->fft_buffer[i].Re;
                aec->fft_buffer[two_frame_length - i].Im = aec->fft_buffer[i].Im;
                aec->fft_buffer[i].Im = -aec->fft_buffer[i].Im;
            }

            FFTCalc (aec->fft_buffer, two_frame_length);

            f = (float)(((float)(aec->fft_buffer[0].Re / ((float)(1.0 * two_frame_length)))) + ((float) 0.5));
            if (f > ((float) 32767.0))
                aec->sout_frame_buffer[0] = 32767;
            else if (f < ((float) -32768.0))
                aec->sout_frame_buffer[0] = -32768;
            else
                aec->sout_frame_buffer[0] = (short) f;
            for (i = 1; i < aec->frame_length; i++)
            {
                f = (float)(((float)(aec->fft_buffer[i].Re / ((float) two_frame_length))) + ((float) 0.5));
                if (f > ((float) 32767.0))
                    aec->sout_frame_buffer[i] = 32767;
                else if (f < ((float) -32768.0))
                    aec->sout_frame_buffer[i] = -32768;
                else
                    aec->sout_frame_buffer[i] = (short) f;
            }
        }
        aec->nlp_attenuation = (word)(attenuation_sum >> (14 + (1 - sdsp_word_leading_zeroes(aec->frame_length))));

        if (aec->max_loop_gain != 0x8000) {
            least_attenuation /= aec->frame_length/2;
            x = (word)(((dword)(((dword)(aec->rout_gain >> 16)) * ((dword) FREQAEC_GAUGE_MARGIN))) >> 15);
            d = (((dword)((((dword)(least_attenuation & 0xffff)) * ((dword) x)))) >> 15) +
                ((((dword)(least_attenuation >> 16)) * ((dword) x)) << 1);
            aec->rout_attenuation = (d > 0x8000) ? 0x8000 : (word) d;
            for (i = 0; i < aec->frame_length; i++)
	        {
                d = ((int32)(aec->sout_frame_buffer[i])) * ((int32)(aec->sout_frame_buffer[i]));
                aec->sout_energy += ((int32)(d - aec->sout_energy)) >> FREQAEC_ENERGY_SHIFT;
            }
            x = aec->max_loop_gain;
            s = 1 - sdsp_word_leading_zeroes(x);
            x <<= -s;
            x = (word)(((dword)(0x3fffffff / ((dword) x))) >> 1);
            x = (word)(((dword)(((dword) x) * ((dword) x))) >> 15);
            s = (1 - s) << 1;
            d = (((dword)((((dword)(aec->sout_energy & 0xffff)) * ((dword) x)))) >> 15) +
                ((((dword)(aec->sout_energy >> 16)) * ((dword) x)) << 1);
            aec->gauge_threshold = (s > -(1 - sdsp_dword_leading_zeroes(d))) ? 0x7fffffff : d << s;
        }

        if (!(aec->options & FREQAEC_FREEZE_COEFFICIENTS))
        {
            for (delay_counter = 0; delay_counter < aec->tail_frames; delay_counter++)
	        {
                rin_offs = aec->rin_buffer_counter + delay_counter;
		        if (rin_offs >= aec->tail_frames)
			        rin_offs -= aec->tail_frames;
		        rin_offs *= aec->frame_length;
		        filter_offs = delay_counter * aec->frame_length;

                f = aec->rin_energy_buffer[rin_offs] * proportionate_buffer[delay_counter];
                e = aec->eout_energy_buffer[0];
                if (f * ((float)(((dword) 1) << 31)) > e) {
                    d = (dword)(e / f);
                    h = proportionate_buffer[delay_counter] / ((float)(1 << ((31 + (1 - sdsp_dword_leading_zeroes(d))) >> 1)));
                }
                else {
                    h = proportionate_buffer[delay_counter] / ((float) 65536.0);
                }
                g = aec->eout_freq_buffer[0].Re * h;
                h = aec->eout_freq_buffer[0].Im * h;
                aec->filter_buffer[filter_offs].Re += g * aec->rin_delay_buffer[rin_offs].Re;
                aec->filter_buffer[filter_offs].Im += h * aec->rin_delay_buffer[rin_offs].Im;

		        for (i = 1; i < aec->frame_length; i++)
		        {
                    f = aec->rin_energy_buffer[rin_offs + i] * proportionate_buffer[delay_counter];
                    e = aec->eout_energy_buffer[i];
                    if (f * ((float)(((dword) 1) << 31)) > e) {
                        d = (dword)(e / f);
                        h = proportionate_buffer[delay_counter] / ((float)(1 << ((31 + (1 - sdsp_dword_leading_zeroes(d))) >> 1)));
                    }
                    else {
                        h = proportionate_buffer[delay_counter] / ((float) 65536.0);
                    }
                    g = aec->eout_freq_buffer[i].Re * h;
                    h = aec->eout_freq_buffer[i].Im * h;
                    aec->filter_buffer[filter_offs + i].Re += (float)(
                        (((float)(g * aec->rin_delay_buffer[rin_offs + i].Re)) -
                        ((float)(h * aec->rin_delay_buffer[rin_offs + i].Im))));
			        aec->filter_buffer[filter_offs + i].Im -= (float)(
                        (((float)(g * aec->rin_delay_buffer[rin_offs + i].Im)) +
                        ((float)(h * aec->rin_delay_buffer[rin_offs + i].Re))));
		        }
            }

            d = 0;
            k = 0;
            for (i = 0; i < aec->tail_frames; i++) {
                aec->filter_masking_schedule[i] += (word)(proportionate_buffer[i] * ((float) 32768.0));
                if (aec->filter_masking_schedule[i] > d) {
                    d = aec->filter_masking_schedule[i];
                    k = i;
                }
            }
            aec->filter_masking_schedule[k] = 0;

            filter_offs = k * aec->frame_length;
            aec->fft_buffer[0].Re = aec->filter_buffer[filter_offs].Re * ((float) 2.0);
            aec->fft_buffer[aec->frame_length].Re = aec->filter_buffer[filter_offs].Im * ((float) 2.0);
            aec->fft_buffer[0].Im = 0;
            aec->fft_buffer[aec->frame_length].Im = 0;
            for (i = 1; i < aec->frame_length; i++)
            {
                aec->fft_buffer[i].Re = aec->filter_buffer[filter_offs + i].Re;
                aec->fft_buffer[i].Im = aec->filter_buffer[filter_offs + i].Im;
                aec->fft_buffer[two_frame_length - i].Re = aec->fft_buffer[i].Re;
                aec->fft_buffer[two_frame_length - i].Im = aec->fft_buffer[i].Im;
                aec->fft_buffer[i].Im = -aec->fft_buffer[i].Im;
            }

            FFTCalc (aec->fft_buffer, two_frame_length);

            filter_masking_max = 0;
            filter_masking_error = 0;
            for (i = 0; i < aec->frame_length; i++)
            {
                if (aec->fft_buffer[i].Re >= 0) {
                    if (aec->fft_buffer[i].Re > filter_masking_max) {
                        filter_masking_max = aec->fft_buffer[i].Re;
                    }
                }
                else {
                    if (-aec->fft_buffer[i].Re > filter_masking_max) {
                        filter_masking_max = -aec->fft_buffer[i].Re;
                    }
                }
                filter_masking_error += ((aec->fft_buffer[i].Im >= 0) ? aec->fft_buffer[i].Im : -aec->fft_buffer[i].Im) +
                    ((aec->fft_buffer[aec->frame_length + i].Re >= 0) ? aec->fft_buffer[aec->frame_length + i].Re : -aec->fft_buffer[aec->frame_length + i].Re) +
                    ((aec->fft_buffer[aec->frame_length + i].Im >= 0) ? aec->fft_buffer[aec->frame_length + i].Im : -aec->fft_buffer[aec->frame_length + i].Im);
                aec->fft_buffer[i].Im = 0;
                aec->fft_buffer[aec->frame_length + i].Re = 0;
                aec->fft_buffer[aec->frame_length + i].Im = 0;
            }

#ifdef FREQAEC_TRACE_
            if (aec->trace_tag)
            {
                for (i = 0; i < aec->frame_length; i++)
                {
                    j = (aec->fft_buffer[i].Re >= 0) ? ((dword) aec->fft_buffer[i].Re) : ((dword) -aec->fft_buffer[i].Re);
                    if (j > aec->echo_peak_amplitude)
                    {
                        aec->echo_peak_amplitude = j;
                        aec->echo_peak_pos = (aec->tail_frames - k) * aec->frame_length - i;
                    }
                }
                if (aec->rin_buffer_counter == aec->tail_frames - 1)
                {
                    for (delay_counter = 0; delay_counter < aec->tail_frames; delay_counter++)
                        proportionate_string[delay_counter] = hex_digit_table[proportionate_exp_buffer[delay_counter]];
                    proportionate_string[aec->tail_frames] = '\0';
                    for (i = 0; i < FREQAEC_LOSS_TRACE_GRAIN; i++)
	                {
                        f = 0;
                        for (j = 0; j < aec->frame_length / FREQAEC_LOSS_TRACE_GRAIN; j++)
                            f += 1.0 / aec->sqr_loss_per_freq_buffer[i*(aec->frame_length / FREQAEC_LOSS_TRACE_GRAIN) + j];
                        f = ((float)(aec->frame_length / FREQAEC_LOSS_TRACE_GRAIN)) / f;
                        j = 0;
                        while ((j < 0xf) && (((float)(1 << j)) < f))
                            j++;
                        loss_per_freq_string[i] = hex_digit_table[j];
                    }
                    loss_per_freq_string[FREQAEC_LOSS_TRACE_GRAIN] = '\0';
#ifdef __ANDROID__
                    sdsp_printf("%s: AEC %s %s %u %u %04x %04x %04x %04x %04x %u", aec->trace_tag, proportionate_string, loss_per_freq_string, aec->echo_peak_pos, aec->echo_peak_amplitude, aec->max_loop_gain, aec->last_attenuation,aec->nlp_attenuation, aec->rout_attenuation, aec->rout_gain >> 16, dt_detects);
#else
//                    printf ("%s: AEC %s %s %u %u %04x %04x %04x %04x %04x %u\n", aec->trace_tag, proportionate_string, loss_per_freq_string, aec->echo_peak_pos, aec->echo_peak_amplitude, aec->max_loop_gain, aec->last_attenuation, aec->nlp_attenuation, aec->rout_attenuation, aec->rout_gain >> 16, dt_detects);
                    debug->iprintf("%s: AEC %s %s %u %u %04x %04x %04x %04x %04x %u", aec->trace_tag, proportionate_string, loss_per_freq_string, aec->echo_peak_pos, aec->echo_peak_amplitude, aec->max_loop_gain, aec->last_attenuation,aec->nlp_attenuation, aec->rout_attenuation, aec->rout_gain >> 16, dt_detects);
#endif
                    aec->echo_peak_pos = 0;
                    aec->echo_peak_amplitude = 0;
                }
            }
#endif

            if (filter_masking_error >= (((float) filter_masking_max) / ((float) 32768.0)) * (aec->frame_length * (3 * FREQAEC_FILTER_MASKING_THRESHOLD) * 2)) {
                FFTCalc (aec->fft_buffer, two_frame_length);

                aec->filter_buffer[filter_offs].Re = aec->fft_buffer[0].Re / ((float)(2 * two_frame_length));
                aec->filter_buffer[filter_offs].Im = aec->fft_buffer[aec->frame_length].Re / ((float)(2 * two_frame_length));
                for (i = 1; i < aec->frame_length; i++)
                {
                    aec->filter_buffer[filter_offs + i].Re = aec->fft_buffer[i].Re / ((float) two_frame_length);
                    aec->filter_buffer[filter_offs + i].Im = aec->fft_buffer[i].Im / ((float) two_frame_length);
                }
            }
        }
	}
	return (Sout);
}


short freqaec_r_exec (t_freqaec *aec, short Rin)
{
    dword d;
    word w, x;

    if (aec->max_loop_gain != 0x8000) {
        d = ((int32) Rin) * ((int32) Rin);
        aec->rin_energy += ((int32)(d - aec->rin_energy)) >> FREQAEC_ENERGY_SHIFT;
        x = (aec->rin_energy >= aec->gauge_threshold) ? 0x8000 : aec->rout_attenuation;
        w = (word)(aec->rout_gain >> 16);
        if (w < x) {
            aec->rout_gain += (((dword) w) * ((dword) FREQAEC_GAUGE_GAIN)) << 1;
            if (aec->rout_gain > 0x80000000) {
                aec->rout_gain = 0x80000000;
            }
        }
        else {
            aec->rout_gain -= (((dword) w) * ((dword) FREQAEC_GAUGE_GAIN)) << 1;
        }
        return ((short)(((int32)(((int32) Rin) * ((dword) w))) >> 15));
    }
    else {
        return Rin;
    }
}


NAMESPACE_END

/*----------------------------------------------------------------------------*/
