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

#define FREQAEC_CPP_

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

#define FILE_ "freqaec.cpp"

#define FREQAEC_TRACE_

NAMESPACE_BEGIN

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

#define FREQAEC_MU                      40
#define FREQAEC_RIN_ENERGY_BIAS         400000

#define FREQAEC_MAX_LOOP_GAIN_MARGIN    0x7fff
#define FREQAEC_MIN_LOSS_LIMIT          0x0400

#define FREQAEC_PROPORTIONATE_EXP       4
#define FREQAEC_PROPORTIONATE_BIAS      0x2000

#define FREQAEC_LOSS_ENHANCE_SLOPE      0x1000
#define FREQAEC_LOSS_ENHANCE_SLOPE_RANGE_EXP 6
#define FREQAEC_SQR_STEP_SIZE_CONSTANT  0x2000
#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   0x1000
#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


#define fft_buffer                      fft_buffer_.complex
#define fft_buffer_scalar               fft_buffer_.scalar

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;
    short s;
    word w;
    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;
    w = (word)((((dword)((((dword)(nominal_return_loss & 0xffff)) * ((dword) FREQAEC_MIN_LOSS_LIMIT)))) >> 15) +
        ((((dword)(nominal_return_loss >> 16)) * ((dword) FREQAEC_MIN_LOSS_LIMIT)) << 1));
    s = (1 - sdsp_word_leading_zeroes (w));
    w <<= -s;
    w = (word)(((dword)(0x3fffffff / ((dword) w))) >> 1);
    aec->max_sqr_loss_enhance = ((dword)(((dword) w) * ((dword) w))) >> (15 + ((s - 1) << 1));
    w = (word)(((dword)(((dword) w) * ((dword) FREQAEC_MIN_LOSS_LIMIT))) >> 15);
    aec->min_sqr_loss_enhance = ((dword)(((dword) w) * ((dword) w))) >> (15 + ((s - 1) << 1));

    w = (word)((((dword)((((dword)(nominal_return_loss & 0xffff)) * ((dword) FREQAEC_MIN_NLP_LOSS_LIMIT)))) >> 15) +
        ((((dword)(nominal_return_loss >> 16)) * ((dword) FREQAEC_MIN_NLP_LOSS_LIMIT)) << 1));
    s = (1 - sdsp_word_leading_zeroes (w));
    w <<= -s;
    w = (word)(((dword)(0x3fffffff / ((dword) w))) >> 1);
    aec->max_sqr_nlp_loss_enhance = ((dword)(((dword) w) * ((dword) w))) >> (15 + ((s - 1) << 1));

    w = (word)((((dword)((((dword)(nominal_return_loss & 0xffff)) * ((dword) FREQAEC_MIN_NLP_RETURN_LOSS)))) >> 15) +
        ((((dword)(nominal_return_loss >> 16)) * ((dword) FREQAEC_MIN_NLP_RETURN_LOSS)) << 1));
    aec->min_sqr_nlp_return_loss = (word)(((dword)(((dword) w) * ((dword) w))) >> 15);

    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(aec->min_sqr_loss_enhance))))) << 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;
}


void freqaec_flush (t_freqaec *aec, word preserve_filter_frames)
{
    unsigned int filter_offs, delay_counter, i;
/*
    dword e;
    word w;
    short s, sft, exp_adj;
*/

    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 = 0;
		aec->rin_delay_buffer[i].im = 0;
        aec->rin_energy_buffer[i] = 0;
        aec->rin_energy_exp[i] = -31;
	}
    for (i = 0; i < aec->tail_frames; i++)
        aec->rin_delay_exp[i] = -128;
/*
    exp_adj = 15 + (1 - sdsp_word_leading_zeroes(aec->frame_length));
    for (i = 0; i < aec->frame_length; i++)
    {
        e = 0;
        for (delay_counter = 0; delay_counter + preserve_filter_frames < aec->tail_frames; delay_counter++)
        {
	        filter_offs = delay_counter * aec->frame_length;
            sft = aec->filter_exp[delay_counter] + exp_adj;
            if (sft >= 0) {
                if (sft < 16) {
                    s = aec->filter_buffer[filter_offs + i].re;
                    e += ((dword)((s >= 0) ? s : -s)) << sft;
                    s = aec->filter_buffer[filter_offs + i].im;
                    e += ((dword)((s >= 0) ? s : -s)) << sft;
                }
            }
            else {
                if (sft > -16) {
                    s = aec->filter_buffer[filter_offs + i].re;
                    e += ((word)((s >= 0) ? s : -s)) >> -sft;
                    s = aec->filter_buffer[filter_offs + i].im;
                    e += ((word)((s >= 0) ? s : -s)) >> -sft;
                }
            }
		    aec->filter_buffer[filter_offs + i].re = 0;
		    aec->filter_buffer[filter_offs + i].im = 0;
	    }
        if (e >= 0x0100) {
            w = (e <= 0x7fff) ? (word) e : 0x7fff;
            s = (1 - sdsp_word_leading_zeroes (w));
            w <<= -s;
            w = (word)(((dword)(0x3fffffff / ((dword) w))) >> 1);
            e = ((dword)(((dword) w) * ((dword) w))) >> (15 + ((s - 1) << 1));
            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++)
    {
        aec->filter_exp[delay_counter] = -128;
    }
*/
    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 = 0;
		    aec->filter_buffer[filter_offs + i].im = 0;
        }
        aec->filter_exp[delay_counter] = -128;
    }
    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)
{
    word filter_energy_buffer[FREQAEC_MAX_TAIL_FRAMES];
    short filter_energy_exp[FREQAEC_MAX_TAIL_FRAMES];
    word unbiased_sqr_proportionate_buffer[FREQAEC_MAX_TAIL_FRAMES];
    short unbiased_proportionate_exp[FREQAEC_MAX_TAIL_FRAMES];
    word proportionate_buffer[FREQAEC_MAX_TAIL_FRAMES];
    short proportionate_exp[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;
    word bias, rin_energy_bias, rin_energy, unbiased_rin_energy, max_rin;
    word max_filter_energy, max_filter_energy2, energy_quot, unbiased_energy_quot;
    word f, h;
    short fft_exp, bias_exp, rin_energy_bias_exp, rin_energy_exp, unbiased_rin_energy_exp, max_rin_exp;
    short eout_freq_exp, max_filter_energy_exp, max_filter_energy2_exp, energy_quot_exp;
    short f_exp, h_exp, filter_masking_exp;
    short Sout, s, t, re, im, re2, im2;
    word attenuation, sft, w, x, max_loop_gain, max_loop_gain_peer;
    dword d, e, least_attenuation, attenuation_sum, filter_masking_error;
    int32 l, m, dc_offset;
	unsigned int i, j, k, n, 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 = aec->sout_frame_buffer[i];
            aec->fft_buffer[two_frame_length - 1 - i].im = 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 = aec->rin_frame_buffer[i];
		    aec->fft_buffer[aec->frame_length - 1 - i].im = 0;
	    }

        fft_exp = 0;
        sbfpfft_normalize (aec->fft_buffer, two_frame_length, &fft_exp, 0);

        sbfpfft_execute (aec->fft_buffer, two_frame_length, &fft_exp);

		rin_offs = aec->rin_buffer_counter * aec->frame_length;
		aec->rin_delay_buffer[rin_offs].re = (aec->fft_buffer[0].re + 1) >> 1;
		aec->rin_delay_buffer[rin_offs].im = (aec->fft_buffer[aec->frame_length].re + 1) >> 1;
		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;
		}
        aec->rin_delay_exp[aec->rin_buffer_counter] = fft_exp;

		for (i = 0; i < aec->frame_length; i++)
		{
            d = ((int32)(aec->rin_delay_buffer[rin_offs + i].re)) * ((int32)(aec->rin_delay_buffer[rin_offs + i].re)) +
                ((int32)(aec->rin_delay_buffer[rin_offs + i].im)) * ((int32)(aec->rin_delay_buffer[rin_offs + i].im));
            s = (1 - sdsp_dword_leading_zeroes (d));
            d <<= -s;
            aec->rin_energy_buffer[rin_offs + i] = (word)(d >> 16);
            aec->rin_energy_exp[rin_offs + i] = s + (fft_exp << 1) + 1;
        }
        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_scalar[i] = 0;
            aec->fft_buffer_scalar[i+FREQAEC_MAX_FRAME_LENGTH] = 0;
        }
        fft_exp = -128;
	    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;
            n = rin_offs;
            s = aec->filter_exp[delay_counter] + aec->rin_delay_exp[n];
            if (s > fft_exp)
                fft_exp = s;
        }
	    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;
            n = rin_offs;
            rin_offs *= aec->frame_length;
		    filter_offs = delay_counter * aec->frame_length;

            sft = fft_exp - (aec->filter_exp[delay_counter] + aec->rin_delay_exp[n]) + 7;
            if (sft < 32)
            {
                aec->fft_buffer_scalar[0] += ((int32)(((int32)(aec->filter_buffer[filter_offs].re)) * ((int32)(aec->rin_delay_buffer[rin_offs].re)))) >> sft;
		        aec->fft_buffer_scalar[FREQAEC_MAX_FRAME_LENGTH] += ((int32)(((int32)(aec->filter_buffer[filter_offs].im)) * ((int32)(aec->rin_delay_buffer[rin_offs].im)))) >> sft;
		        for (i = 1; i < aec->frame_length; i++)
		        {
			        aec->fft_buffer_scalar[i] +=
				        (((int32)(((int32)(aec->filter_buffer[filter_offs + i].re)) * ((int32)(aec->rin_delay_buffer[rin_offs + i].re)))) >> sft) -
				        (((int32)(((int32)(aec->filter_buffer[filter_offs + i].im)) * ((int32)(aec->rin_delay_buffer[rin_offs + i].im)))) >> sft);
			        aec->fft_buffer_scalar[i+FREQAEC_MAX_FRAME_LENGTH] +=
				        (((int32)(((int32)(aec->filter_buffer[filter_offs + i].re)) * ((int32)(aec->rin_delay_buffer[rin_offs + i].im)))) >> sft) +
				        (((int32)(((int32)(aec->filter_buffer[filter_offs + i].im)) * ((int32)(aec->rin_delay_buffer[rin_offs + i].re)))) >> sft);
		        }
            }
	    }
        aec->fft_buffer_scalar[0] <<= 1;
        aec->fft_buffer_scalar[FREQAEC_MAX_FRAME_LENGTH] <<= 1;
        d = 0;
        for (i = 0; i < aec->frame_length; i++)
        {
            l = aec->fft_buffer_scalar[i];
            if (l >= 0)
            {
                if (((dword) l) > d)
                    d = l;
            }
            else
            {
                if (((dword) ~l) > d)
                    d = (dword) ~l;
            }
            l = aec->fft_buffer_scalar[i+FREQAEC_MAX_FRAME_LENGTH];
            if (l >= 0)
            {
                if (((dword) l) > d)
                    d = l;
            }
            else
            {
                if (((dword) ~l) > d)
                    d = (dword) ~l;
            }
        }
        s = (1 - sdsp_dword_leading_zeroes (d + (d >> 16)));
        if (s > -16)
        {
            sft = s + 16;
            l = ((dword) 1) << (sft - 1);
            for (i = 0; i < aec->frame_length; i++)
            {
                aec->fft_buffer[i].re = (short)((aec->fft_buffer_scalar[i] + l) >> sft);
                aec->fft_buffer[i].im = (short)((aec->fft_buffer_scalar[i+FREQAEC_MAX_FRAME_LENGTH] + l) >> sft);
            }
        }
        else
        {
            sft = -16 - s;
            for (i = 0; i < aec->frame_length; i++)
            {
                aec->fft_buffer[i].re = (short)(aec->fft_buffer_scalar[i] << sft);
                aec->fft_buffer[i].im = (short)(aec->fft_buffer_scalar[i+FREQAEC_MAX_FRAME_LENGTH] << sft);
            }
        }
        fft_exp += s + 8;

        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;
        }

        sbfpfft_execute (aec->fft_buffer, two_frame_length, &fft_exp);

        sbfpfft_denormalize (aec->fft_buffer, two_frame_length, fft_exp, 0);

        for (i = 0; i < aec->frame_length; i++)
            aec->filter_result_buffer[i] = aec->fft_buffer[i].re;

        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 = aec->sout_frame_buffer[i];
		    aec->fft_buffer[i].im = 0;
		    aec->fft_buffer[aec->frame_length + i].re = 0;
		    aec->fft_buffer[aec->frame_length + i].im = 0;
	    }

        eout_freq_exp = 0;
        sbfpfft_normalize (aec->fft_buffer, two_frame_length, &eout_freq_exp, 0);

        sbfpfft_execute (aec->fft_buffer, two_frame_length, &eout_freq_exp);

        aec->eout_freq_buffer[0].re = (aec->fft_buffer[0].re + 1) >> 1;
        aec->eout_freq_buffer[0].im = (aec->fft_buffer[aec->frame_length].re + 1) >> 1;
        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 = 0x7fff;
        max_filter_energy_exp = (-128 << 1) - 32;
        max_filter_energy2 = 0x7fff;
        max_filter_energy2_exp = (-128 << 1) - 32;
        for (delay_counter = 0; delay_counter < aec->tail_frames; delay_counter++)
        {
		    filter_offs = delay_counter * aec->frame_length;
            d = 1;
            for (i = 0; i < aec->frame_length; i++)
	        {
                d += (((dword)(((int32)(aec->filter_buffer[filter_offs + i].re)) * ((int32)(aec->filter_buffer[filter_offs + i].re)))) >> (1 + 1 + FREQAEC_MAX_FRAME_LENGTH_EXP)) +
                    (((dword)(((int32)(aec->filter_buffer[filter_offs + i].im)) * ((int32)(aec->filter_buffer[filter_offs + i].im)))) >> (1 + 1 + FREQAEC_MAX_FRAME_LENGTH_EXP));
            }
            s = (1 - sdsp_dword_leading_zeroes (d));
            w = (word)((d << -s) >> 16);
            filter_energy_buffer[delay_counter] = w;
            s += (aec->filter_exp[delay_counter] << 1) + 1 + (1 + 1 + FREQAEC_MAX_FRAME_LENGTH_EXP);
            filter_energy_exp[delay_counter] = s;
            if ((s > max_filter_energy2_exp) || ((s == max_filter_energy2_exp) && (w > max_filter_energy2)))
            {
                if ((s > max_filter_energy_exp) || ((s == max_filter_energy_exp) && (w > max_filter_energy)))
                {
                    max_filter_energy2_exp = max_filter_energy_exp;
                    max_filter_energy2 = max_filter_energy;
                    max_filter_energy_exp = s;
                    max_filter_energy = w;
                }
                else
                {
                    max_filter_energy2_exp = s;
                    max_filter_energy2 = w;
                }
            }
        }
        unbiased_energy_quot = (word)(((dword)(0x3fffffff / ((dword) max_filter_energy))) >> 1);
        h_exp = 0;
        h = FREQAEC_PROPORTIONATE_BIAS;
        s = (1 - sdsp_word_leading_zeroes (h));
        h <<= -s;
        h_exp += s;
        h = (word)(((dword)(((dword) h) * ((dword) h))) >> 15);
        h_exp = 2 * (h_exp + FREQAEC_PROPORTIONATE_EXP);
        d = ((dword) h) * ((dword)(aec->frame_length));
        s = (1 - sdsp_dword_leading_zeroes (d));
        h = (word)((d << -s) >> 16);
        h_exp += 16 + s - 30;
        if ((max_filter_energy_exp > h_exp) || ((max_filter_energy_exp == h_exp) && (max_filter_energy > h)))
        {
            if ((max_filter_energy_exp < max_filter_energy2_exp + 1) || ((max_filter_energy_exp == max_filter_energy2_exp + 1) && (max_filter_energy < max_filter_energy2))) {
                h_exp = max_filter_energy_exp;
                h = max_filter_energy;
            }
            else if ((max_filter_energy2_exp + 1 > h_exp) || ((max_filter_energy2_exp + 1 == h_exp) && (max_filter_energy2 > h))) {
                h_exp = max_filter_energy2_exp + 1;
                h = max_filter_energy2;
            }
        }
        energy_quot = (word)(((dword)(0x3fffffff / ((dword) h))) >> 1);
        e = 0;
        for (delay_counter = 0; delay_counter < aec->tail_frames; delay_counter++)
        {
            if ((filter_energy_exp[delay_counter] > h_exp) || ((filter_energy_exp[delay_counter] == h_exp) && (filter_energy_buffer[delay_counter] >= h))
             || ((delay_counter == aec->tail_frames - 1)
              && ((max_filter_energy_exp < h_exp) || ((max_filter_energy_exp == h_exp) && (max_filter_energy < h))))) {
                w = 0x8000;
                proportionate_exp[delay_counter] = 0;
                proportionate_buffer[delay_counter] = 0x7fff;
            }
            else if ((filter_energy_exp[delay_counter] + 2 * FREQAEC_PROPORTIONATE_EXP < h_exp)
             || ((filter_energy_exp[delay_counter] + 2 * FREQAEC_PROPORTIONATE_EXP == h_exp)
              && (filter_energy_buffer[delay_counter] < h)))
            {
                w = 0x8000 >> FREQAEC_PROPORTIONATE_EXP;
                proportionate_exp[delay_counter] = -FREQAEC_PROPORTIONATE_EXP;
                proportionate_buffer[delay_counter] = 0x7fff;
            }
            else
            {
                d = ((dword)((((dword)(filter_energy_buffer[delay_counter])) * ((dword) energy_quot)) << 2)) >> (h_exp - filter_energy_exp[delay_counter]);
                w = sdsp_sqrt(d);
                proportionate_exp[delay_counter] = (1 - sdsp_word_leading_zeroes (w));
                proportionate_buffer[delay_counter] = w << -proportionate_exp[delay_counter];
            }
            e += (((dword)((((dword)(FREQAEC_RIN_ENERGY_BIAS & 0xffff)) * ((dword) w)))) >> 15) +
                ((((dword)(FREQAEC_RIN_ENERGY_BIAS >> 16)) * ((dword) w)) << 1);
            if (max_filter_energy_exp - filter_energy_exp[delay_counter] < 32) {
                d = ((dword)((((dword)(filter_energy_buffer[delay_counter])) * ((dword) unbiased_energy_quot)) << 2)) >> (max_filter_energy_exp - filter_energy_exp[delay_counter]);
                w = sdsp_sqrt(d);
                unbiased_proportionate_exp[delay_counter] = (1 - sdsp_word_leading_zeroes (w));
                unbiased_sqr_proportionate_buffer[delay_counter] = w << -unbiased_proportionate_exp[delay_counter];
                if ((unbiased_proportionate_exp[delay_counter] > proportionate_exp[delay_counter])
                 || ((unbiased_proportionate_exp[delay_counter] == proportionate_exp[delay_counter])
                  && (unbiased_sqr_proportionate_buffer[delay_counter] > proportionate_buffer[delay_counter]))) {
                     unbiased_proportionate_exp[delay_counter] = proportionate_exp[delay_counter];
                     unbiased_sqr_proportionate_buffer[delay_counter] = proportionate_buffer[delay_counter];
                }
                unbiased_proportionate_exp[delay_counter] *= 2;
                unbiased_sqr_proportionate_buffer[delay_counter] = (word)(((dword)(((dword)(unbiased_sqr_proportionate_buffer[delay_counter])) * ((dword)(unbiased_sqr_proportionate_buffer[delay_counter])))) >> 15);
                if (unbiased_sqr_proportionate_buffer[delay_counter] <= 0x3fff)
                {
                    unbiased_sqr_proportionate_buffer[delay_counter] <<= 1;
                    (unbiased_proportionate_exp[delay_counter])--;
                }
            }
            else {
                unbiased_proportionate_exp[delay_counter] = -16;
                unbiased_sqr_proportionate_buffer[delay_counter] = 0x7fff;
            }
#ifdef FREQAEC_TRACE_
            if (aec->trace_tag)
            {
                s = -15 + 4 - filter_energy_exp[delay_counter];
                proportionate_exp_buffer[delay_counter] = (s < 0) ? 0 : ((s < 0xf) ? s : 0xf);
            }
#endif
        }
        s = (1 - sdsp_dword_leading_zeroes (e));
        rin_energy_bias = (word)((e << -s) >> 16);
        rin_energy_bias_exp = s;

        if (aec->noise_suppress_level != 0) {
            for (i = 0; i < aec->frame_length; i++)
            {
                d = ((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);
                if (eout_freq_exp >= 0) {
                    d <<= eout_freq_exp;
                }
                else {
                    if (eout_freq_exp >= -31) {
                        d >>= -eout_freq_exp;
                    }
                    else {
                        d = 0;
                    }
                }
                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 = 0x7fff;
            unbiased_rin_energy_exp = -128;
            d = (((dword)((((dword)(aec->sqr_loss_per_freq_buffer[i] & 0xffff)) * ((dword) rin_energy_bias)))) >> 15) +
                ((((dword)(aec->sqr_loss_per_freq_buffer[i] >> 16)) * ((dword) rin_energy_bias)) << 1);
            bias_exp = (1 - sdsp_dword_leading_zeroes (d));
            bias = (word)((d << -bias_exp) >> 16);
            bias_exp += rin_energy_bias_exp + 16;
            rin_energy = bias;
            rin_energy_exp = bias_exp;
            max_rin = 0x7fff;
            max_rin_exp = -128;
            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;
                n = rin_offs;
                rin_offs *= aec->frame_length;
                w = (word)(((dword)(((dword)(aec->rin_energy_buffer[rin_offs + i])) * ((dword)(unbiased_sqr_proportionate_buffer[delay_counter])))) >> 15);
                s = aec->rin_energy_exp[rin_offs + i] + unbiased_proportionate_exp[delay_counter];
                if (w <= 0x3fff)
                {
                    w <<= 1;
                    s--;
                }
                if ((s > max_rin_exp) || ((s == max_rin_exp) && (w > max_rin)))
                {
                    max_rin_exp = s;
                    max_rin = w;
                }
                if (s > unbiased_rin_energy_exp)
                {
                    sft = s - unbiased_rin_energy_exp;
                    unbiased_rin_energy = (sft < 16) ? unbiased_rin_energy >> sft : 0;
                    unbiased_rin_energy_exp = s;
                    sft = 0;
                }
                else
                {
                    sft = unbiased_rin_energy_exp - s;
                }
                if (sft < 16)
                {
                    unbiased_rin_energy += w >> sft;
                    if (unbiased_rin_energy > 0x7fff)
                    {
                        unbiased_rin_energy >>= 1;
                        unbiased_rin_energy_exp++;
                    }
                }
                w = (word)(((dword)(((dword)(aec->rin_energy_buffer[rin_offs + i])) * ((dword)(proportionate_buffer[delay_counter])))) >> 15);
                s = aec->rin_energy_exp[rin_offs + i] + proportionate_exp[delay_counter];
                if (w <= 0x3fff)
                {
                    w <<= 1;
                    s--;
                }
                if (s > rin_energy_exp)
                {
                    sft = s - rin_energy_exp;
                    rin_energy = (sft < 16) ? rin_energy >> sft : 0;
                    rin_energy_exp = s;
                    sft = 0;
                }
                else
                {
                    sft = rin_energy_exp - s;
                }
                if (sft < 16)
                {
                    rin_energy += w >> sft;
                    if (rin_energy > 0x7fff)
                    {
                        rin_energy >>= 1;
                        rin_energy_exp++;
                    }
                }
            }
            aec->rin_energy_sum_buffer[i] = unbiased_rin_energy;
            aec->rin_energy_sum_exp[i] = unbiased_rin_energy_exp;

            d = ((int32)(aec->eout_freq_buffer[i].re)) * ((int32)(aec->eout_freq_buffer[i].re)) +
                ((int32)(aec->eout_freq_buffer[i].im)) * ((int32)(aec->eout_freq_buffer[i].im));
            s = (1 - sdsp_dword_leading_zeroes (d));
            f = (word)((d << -s) >> 16);
            f_exp = s + (eout_freq_exp << 1) + 1;
            energy_quot_exp = -rin_energy_exp;
            energy_quot = (word)(((dword)(0x3fffffff / ((dword) rin_energy))) >> 1);

            d = (((dword)((((dword)(aec->min_sqr_loss_enhance & 0xffff)) * ((dword) f)))) >> 15) +
                ((((dword)(aec->min_sqr_loss_enhance >> 16)) * ((dword) f)) << 1);
            s = (1 - sdsp_dword_leading_zeroes (d));
            w = (word)((d << -s) >> 16);
            s += f_exp + 16;

            if (aec->last_dt[i] & 0x80) {
                aec->last_dt[i] &= ~0x80;
                dt = ((s > max_rin_exp) || ((s == max_rin_exp) && (w >= 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))
                {
                    d = (((dword)((((dword)(aec->sqr_loss_per_freq_buffer[i] & 0xffff)) * ((dword) f)))) >> 15) +
                        ((((dword)(aec->sqr_loss_per_freq_buffer[i] >> 16)) * ((dword) f)) << 1);
                    s = (1 - sdsp_dword_leading_zeroes (d));
                    w = (word)((d << -s) >> 16);
                    s += f_exp + 16;
                    if (unbiased_rin_energy_exp + FREQAEC_LOSS_ENHANCE_SLOPE_RANGE_EXP > bias_exp) {
                        if (bias_exp >= unbiased_rin_energy_exp) {
                            x = FREQAEC_LOSS_ENHANCE_SLOPE >> ((bias_exp - unbiased_rin_energy_exp + 1) >> 1);
                        }
                        else {
                            x = FREQAEC_LOSS_ENHANCE_SLOPE;
                        }
                    }
                    else {
                        x = FREQAEC_LOSS_ENHANCE_SLOPE >> (FREQAEC_LOSS_ENHANCE_SLOPE_RANGE_EXP >> 1);
                    }
                    if ((unbiased_rin_energy_exp > s) || ((unbiased_rin_energy_exp == s) && (unbiased_rin_energy >= w)))
                    {
                        aec->sqr_loss_per_freq_buffer[i] = (((dword)((((dword)(aec->sqr_loss_per_freq_buffer[i] & 0xffff)) * ((dword)(0x8000 + x))))) >> 15) +
                            ((((dword)(aec->sqr_loss_per_freq_buffer[i] >> 16)) * ((dword)(0x8000 + x))) << 1);
                        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] = (((dword)((((dword)(aec->sqr_loss_per_freq_buffer[i] & 0xffff)) * ((dword)(0x8000 - x))))) >> 15) +
                            ((((dword)(aec->sqr_loss_per_freq_buffer[i] >> 16)) * ((dword)(0x8000 - x))) << 1);
                        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;
                h_exp = f_exp;
            }
            else
            {
                dt_detects++;
                h = f;
                h_exp = f_exp + FREQAEC_DOUBLE_TALK_PENALTY_EXP;
            }

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

            d = ((dword)(((dword) unbiased_rin_energy) * ((dword)(aec->min_sqr_nlp_return_loss)))) << 1;
            s = (1 - sdsp_dword_leading_zeroes (d));
            w = (word)((d << -s) >> 16);
            s += unbiased_rin_energy_exp;
            x = (word)(((dword)(((dword) f) * ((dword) FREQAEC_SQR_NLP_LOSS_CONSTANT))) >> 15);
            d = (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;
            d = (((dword)((((dword)(d & 0xffff)) * ((dword) x)))) >> 15) + ((((dword)(d >> 16)) * ((dword) x)) << 1);
            t = (1 - sdsp_dword_leading_zeroes (d));
            x = (word)((d << -t) >> 16);
            t += f_exp + 16;

            if ((aec->options & FREQAEC_ENABLE_NONLINEAR_PROCESSING)
             && (aec->last_dt[i] == 0)
             && ((f_exp < s) || ((f_exp == s) && (f < w)))
             && ((t < unbiased_rin_energy_exp) || ((t == unbiased_rin_energy_exp) && (x < 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];

            x = (word)(((dword)(((dword) h) * ((dword) FREQAEC_SQR_STEP_SIZE_CONSTANT))) >> 15);
            d = (((dword)((((dword)(aec->sqr_loss_per_freq_buffer[i] & 0xffff)) * ((dword) x)))) >> 15) +
                ((((dword)(aec->sqr_loss_per_freq_buffer[i] >> 16)) * ((dword) x)) << 1);
            aec->eout_energy_exp[i] = (1 - sdsp_dword_leading_zeroes (d)) + h_exp + 16;

            re = aec->eout_freq_buffer[i].re;
            im = aec->eout_freq_buffer[i].im;
            w = (re >= 0) ? re : ~re;
            x = (im >= 0) ? im : ~im;
            if (x > w)
                w = x;
            t = (1 - sdsp_word_leading_zeroes (w));
            d = ((dword)(((dword) FREQAEC_MU) * ((dword) energy_quot))) << 1;
            s = (1 - sdsp_dword_leading_zeroes (d)) + 1;
            w = (word)((d << -s) >> 16);
            s += energy_quot_exp + 16 + 1 - 16;
            aec->eout_freq_buffer[i].re = (short)(((int32)(((int32)(re << -t)) * ((int32) w))) >> 15);
            aec->eout_freq_buffer[i].im = (short)(((int32)(((int32)(im << -t)) * ((int32) w))) >> 15);
            aec->eout_freq_exp[i] = eout_freq_exp + s + t;
        }
        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_exp[i] > aec->rin_energy_sum_exp[i+1])
                       || ((aec->rin_energy_sum_exp[i] == aec->rin_energy_sum_exp[i+1]) && (aec->rin_energy_sum_buffer[i] >= aec->rin_energy_sum_buffer[i+1])))
                      && ((aec->rin_energy_sum_exp[i] > aec->rin_energy_sum_exp[i-1])
                       || ((aec->rin_energy_sum_exp[i] == aec->rin_energy_sum_exp[i-1]) && (aec->rin_energy_sum_buffer[i] >= aec->rin_energy_sum_buffer[i-1]))))
                     || (nlp
                      && ((aec->rin_energy_sum_exp[i] < aec->rin_energy_sum_exp[i+1])
                       || ((aec->rin_energy_sum_exp[i] == aec->rin_energy_sum_exp[i+1]) && (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_exp[i] > aec->rin_energy_sum_exp[i+1])
                       || ((aec->rin_energy_sum_exp[i] == aec->rin_energy_sum_exp[i+1]) && (aec->rin_energy_sum_buffer[i] >= aec->rin_energy_sum_buffer[i+1])))
                      && ((aec->rin_energy_sum_exp[i] > aec->rin_energy_sum_exp[i-1])
                       || ((aec->rin_energy_sum_exp[i] == aec->rin_energy_sum_exp[i-1]) && (aec->rin_energy_sum_buffer[i] >= aec->rin_energy_sum_buffer[i-1]))))
                     || (nlp
                      && ((aec->rin_energy_sum_exp[i] < aec->rin_energy_sum_exp[i-1])
                       || ((aec->rin_energy_sum_exp[i] == aec->rin_energy_sum_exp[i-1]) && (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;
                }
            }
            sbfpfft_denorm (aec->fft_buffer, two_frame_length, aec->shaped_attenuations);

            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;
            }

            fft_exp = eout_freq_exp;
            sbfpfft_normalize (aec->fft_buffer, two_frame_length, &fft_exp, 0);

            sbfpfft_execute (aec->fft_buffer, two_frame_length, &fft_exp);

            sbfpfft_denormalize (aec->fft_buffer, two_frame_length, fft_exp, 14 + (1 - sdsp_word_leading_zeroes (two_frame_length)));

            for (i = 0; i < aec->frame_length; i++)
                aec->sout_frame_buffer[i] = aec->fft_buffer[i].re;
        }
        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;
                n = rin_offs;
		        rin_offs *= aec->frame_length;
		        filter_offs = delay_counter * aec->frame_length;

                re = aec->rin_delay_buffer[rin_offs].re;
                im = aec->rin_delay_buffer[rin_offs].im;
                t = (aec->rin_energy_exp[rin_offs] - (aec->rin_delay_exp[n] << 1) + 1) >> 1;
                if (t <= 0)
                {
                    re <<= -t;
                    im <<= -t;
                }
                else
                {
                    re >>= 1;
                    im >>= 1;
                }
                re2 = (short)(((int32)(((int32) re) * ((int32)(proportionate_buffer[delay_counter])))) >> 15);
                im2 = (short)(((int32)(((int32) im) * ((int32)(proportionate_buffer[delay_counter])))) >> 15);
                t += aec->rin_delay_exp[n] + proportionate_exp[delay_counter];

                w = (word)(((dword)(((dword)(aec->rin_energy_buffer[rin_offs])) * ((dword)(proportionate_buffer[delay_counter])))) >> 15);
                s = aec->rin_energy_exp[rin_offs] + proportionate_exp[delay_counter];
                if (w <= 0x3fff)
                    s--;

                if (s < aec->eout_energy_exp[0])
                    t += (s - aec->eout_energy_exp[0]) >> 1;

                re2 = (short)(((int32)(((int32)(aec->eout_freq_buffer[0].re)) * ((int32) re2))) >> 15);
                im2 = (short)(((int32)(((int32)(aec->eout_freq_buffer[0].im)) * ((int32) im2))) >> 15);
                t += aec->eout_freq_exp[0];

                re = aec->filter_buffer[filter_offs].re;
                im = aec->filter_buffer[filter_offs].im;
                w = (re >= 0) ? re : ~re;
                x = (im >= 0) ? im : ~im;
                if (x > w)
                    w = x;
                s = (1 - sdsp_word_leading_zeroes (w));
                re <<= -s;
                im <<= -s;
                s += aec->filter_exp[delay_counter];

                if (t > s)
                {
                    sft = t - s;
                    if (sft < 16)
                    {
                        re >>= sft;
                        im >>= sft;
                    }
                    else
                    {
                        re = 0;
                        im = 0;
                    }
                    s = t;
                    sft = 0;
                }
                else
                {
                    sft = s - t;
                }
                if (sft < 16)
                {
                    l = re + (re2 >> sft);
                    m = im + (im2 >> sft);
                    if ((l > 32767) || (l < -32768) || (m > 32767) || (m < -32768))
                    {
                        l >>= 1;
                        m >>= 1;
                        s++;
                    }
                    aec->filter_buffer[filter_offs].re = (short) l;
                    aec->filter_buffer[filter_offs].im = (short) m;
                }
                else
                {
                    aec->filter_buffer[filter_offs].re = re;
                    aec->filter_buffer[filter_offs].im = im;
                }
                aec->filter_freq_exp[0] = s;
                fft_exp = s;

		        for (i = 1; i < aec->frame_length; i++)
		        {
                    re = aec->rin_delay_buffer[rin_offs + i].re;
                    im = aec->rin_delay_buffer[rin_offs + i].im;
                    t = (aec->rin_energy_exp[rin_offs + i] - (aec->rin_delay_exp[n] << 1) + 1) >> 1;
                    if (t <= 0)
                    {
                        re <<= -t;
                        im <<= -t;
                    }
                    else
                    {
                        re >>= 1;
                        im >>= 1;
                    }
                    re2 = (short)(((int32)(((int32) re) * ((int32)(proportionate_buffer[delay_counter])))) >> 15);
                    im2 = (short)(((int32)(((int32) im) * ((int32)(proportionate_buffer[delay_counter])))) >> 15);
                    t += aec->rin_delay_exp[n] + proportionate_exp[delay_counter];

                    w = (word)(((dword)(((dword)(aec->rin_energy_buffer[rin_offs + i])) * ((dword)(proportionate_buffer[delay_counter])))) >> 15);
                    s = aec->rin_energy_exp[rin_offs + i] + proportionate_exp[delay_counter];
                    if (w <= 0x3fff)
                        s--;

                    if (s < aec->eout_energy_exp[i])
                        t += (s - aec->eout_energy_exp[i]) >> 1;

                    s = re2;
                    re2 = (short)(((int32)(((int32)(aec->eout_freq_buffer[i].re)) * ((int32) s) -
                        ((int32)(aec->eout_freq_buffer[i].im)) * ((int32) im2))) >> 15);
                    im2 = -(short)(((int32)(((int32)(aec->eout_freq_buffer[i].re)) * ((int32) im2) +
                        ((int32)(aec->eout_freq_buffer[i].im)) * ((int32) s))) >> 15);
                    t += aec->eout_freq_exp[i];

                    re = aec->filter_buffer[filter_offs + i].re;
                    im = aec->filter_buffer[filter_offs + i].im;
                    w = (re >= 0) ? re : ~re;
                    x = (im >= 0) ? im : ~im;
                    if (x > w)
                        w = x;
                    s = (1 - sdsp_word_leading_zeroes (w));
                    re <<= -s;
                    im <<= -s;
                    s += aec->filter_exp[delay_counter];

                    if (t > s)
                    {
                        sft = t - s;
                        if (sft < 16)
                        {
                            re >>= sft;
                            im >>= sft;
                        }
                        else
                        {
                            re = 0;
                            im = 0;
                        }
                        s = t;
                        sft = 0;
                    }
                    else
                    {
                        sft = s - t;
                    }
                    if (sft < 16)
                    {
                        l = re + (re2 >> sft);
                        m = im + (im2 >> sft);
                        if ((l > 32767) || (l < -32768) || (m > 32767) || (m < -32768))
                        {
                            l >>= 1;
                            m >>= 1;
                            s++;
                        }
                        aec->filter_buffer[filter_offs + i].re = (short) l;
                        aec->filter_buffer[filter_offs + i].im = (short) m;
                    }
                    else
                    {
                        aec->filter_buffer[filter_offs + i].re = re;
                        aec->filter_buffer[filter_offs + i].im = im;
                    }
                    aec->filter_freq_exp[i] = s;
                    if (s > fft_exp)
                        fft_exp = s;
		        }

                aec->filter_exp[delay_counter] = fft_exp;
		        for (i = 0; i < aec->frame_length; i++)
		        {
                    sft = fft_exp - aec->filter_freq_exp[i];
                    if (sft < 16)
                    {
                        if (sft != 0) {
                            s = 1 << (sft - 1);
                            aec->filter_buffer[filter_offs + i].re = (aec->filter_buffer[filter_offs + i].re + s) >> sft;
                            aec->filter_buffer[filter_offs + i].im = (aec->filter_buffer[filter_offs + i].im + s) >> sft;
                        }
                    }
                    else
                    {
                        aec->filter_buffer[filter_offs + i].re = 0;
                        aec->filter_buffer[filter_offs + i].im = 0;
                    }
                }
            }

            d = 0;
            k = 0;
            for (i = 0; i < aec->tail_frames; i++) {
                aec->filter_masking_schedule[i] += (proportionate_buffer[i] >> -proportionate_exp[i]);
                if (aec->filter_masking_schedule[i] > d) {
                    d = aec->filter_masking_schedule[i];
                    k = i;
                }
            }
            aec->filter_masking_schedule[k] = 0;
            filter_masking_exp = aec->filter_exp[k];

            filter_offs = k * aec->frame_length;
            if ((aec->filter_buffer[filter_offs].re > 16383) || (aec->filter_buffer[filter_offs].re < -16384)
             || (aec->filter_buffer[filter_offs].im > 16383) || (aec->filter_buffer[filter_offs].im < -16384)) {
                filter_masking_exp++;
                aec->fft_buffer[0].re = aec->filter_buffer[filter_offs].re;
                aec->fft_buffer[aec->frame_length].re = aec->filter_buffer[filter_offs].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[i].re = (aec->filter_buffer[filter_offs + i].re + 1) >> 1;
                    aec->fft_buffer[i].im = (aec->filter_buffer[filter_offs + i].im + 1) >> 1;
                    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;
                }
            }
            else {
                aec->fft_buffer[0].re = aec->filter_buffer[filter_offs].re << 1;
                aec->fft_buffer[aec->frame_length].re = aec->filter_buffer[filter_offs].im << 1;
                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;
                }
            }

            sbfpfft_normalize (aec->fft_buffer, two_frame_length, &filter_masking_exp, 0);

            sbfpfft_execute (aec->fft_buffer, two_frame_length, &filter_masking_exp);

            filter_masking_error = 0;
            for (i = 0; i < aec->frame_length; i++)
            {
                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)
            {
                sft = filter_masking_exp;
                if (filter_masking_exp > 0)
                {
                    sft = filter_masking_exp;
                    if (sft < 16)
                    {
                        for (i = 0; i < aec->frame_length; i++)
                        {
                            d = ((dword)((aec->fft_buffer[i].re >= 0) ? aec->fft_buffer[i].re : -aec->fft_buffer[i].re)) << sft;
                            if (d > aec->echo_peak_amplitude)
                            {
                                aec->echo_peak_amplitude = d;
                                aec->echo_peak_pos = (aec->tail_frames - k) * aec->frame_length - i;
                            }
                        }
                    }
                }
                else
                {
                    sft = -filter_masking_exp;
                    if (sft < 16)
                    {
                        for (i = 0; i < aec->frame_length; i++)
                        {
                            d = ((word)((aec->fft_buffer[i].re >= 0) ? aec->fft_buffer[i].re : -aec->fft_buffer[i].re)) >> sft;
                            if (d > aec->echo_peak_amplitude)
                            {
                                aec->echo_peak_amplitude = d;
                                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++)
	                {
                        e = 0;
                        for (j = 0; j < aec->frame_length / FREQAEC_LOSS_TRACE_GRAIN; j++)
                        {
                            d = aec->sqr_loss_per_freq_buffer[i*(aec->frame_length / FREQAEC_LOSS_TRACE_GRAIN) + j];
                            s = (1 - sdsp_dword_leading_zeroes (d));
                            w = (word)((d << -s) >> 16);
                            d = (((dword)(0x3fffffff / ((dword) w))) >> 1) << (16 - 1);
                            e += d >> (31 + s);
                        }
                        e >>= (1 - sdsp_word_leading_zeroes (aec->frame_length)) - (1 - sdsp_word_leading_zeroes (FREQAEC_LOSS_TRACE_GRAIN));
                        j = (e <= 0x7fff) ? -(1 - sdsp_word_leading_zeroes ((word) e)) : 0;
                        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 >= aec->frame_length * (3 * FREQAEC_FILTER_MASKING_THRESHOLD)) {
                sbfpfft_execute (aec->fft_buffer, two_frame_length, &filter_masking_exp);
                filter_masking_exp -= 14 + (1 - sdsp_word_leading_zeroes (two_frame_length));

                aec->filter_buffer[filter_offs].re = (aec->fft_buffer[0].re + 1) >> 1;
                aec->filter_buffer[filter_offs].im = (aec->fft_buffer[aec->frame_length].re + 1) >> 1;
                for (i = 1; i < aec->frame_length; i++)
                {
                    aec->filter_buffer[filter_offs + i].re = aec->fft_buffer[i].re;
                    aec->filter_buffer[filter_offs + i].im = aec->fft_buffer[i].im;
                }
                aec->filter_exp[k] = filter_masking_exp;
            }
        }
	}
	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

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