/*************************************************************************
 *
 *  FUNCTION:  agc
 *
 *  PURPOSE: Scales the postfilter output on a subframe basis by automatic
 *           control of the subframe gain.
 *
 *  DESCRIPTION:
 *   sig_out[n] = sig_out[n] * gain[n];
 *   where gain[n] is the gain at the nth sample given by
 *     gain[n] = agc_fac * gain[n-1] + (1 - agc_fac) g_in/g_out
 *   g_in/g_out is the square root of the ratio of energy at the input
 *   and output of the postfilter.
 *
 *************************************************************************/

#include "typedef.h"
#include "basic_op.h"
#include "sig_proc.h"
#include "cnst.h"

Word16 past_gain;               /* initial value of past_gain = 1.0  */

void agc (
    Word16 *sig_in,             /* (i)     : postfilter input signal  */
    Word16 *sig_out,            /* (i/o)   : postfilter output signal */
    Word16 agc_fac,             /* (i)     : AGC factor               */
    Word16 l_trm                /* (i)     : subframe size            */
)
{
    Word16 i, exp;
    Word16 gain_in, gain_out, g0, gain;
    Word32 s;

    Word16 temp;

    /* calculate gain_out with exponent */

		s = 0;
    for (i = 0; i < l_trm; i++)
    {
        temp = sig_out[i] >> 2;
        s += (Word32)temp * (Word32)temp;
    }

    if (s == 0)
    {
        past_gain = 0;
        return;
    }
		s <<= 1;
    exp = norm_l (s) - 1;
    gain_out = (Word16)(((s << exp) + (1 << 15)) >> 16);

    /* calculate gain_in with exponent */

		s = 0;
    for (i = 0; i < l_trm; i++)
    {
        temp = sig_in[i] >> 2;
        s += (Word32)temp * (Word32)temp;
    }

    if (s == 0)
    {
        g0 = 0;
    }
    else
    {
			  s <<= 1;
        i = norm_l (s);
        gain_in = (Word16)(((s << i) + (1 << 15)) >> 16);
        exp -= i;

        /*---------------------------------------------------*
         *  g0 = (1-agc_fac) * sqrt(gain_in/gain_out);       *
         *---------------------------------------------------*/

        s = ((Word32)gain_out << 15) / gain_in;
        s <<= 7;       /* s = gain_out / gain_in */
				if (exp < 0)     /* add exponent */
					s <<= -exp;
				else
					s >>= exp;

        s = Inv_sqrt (s);
        i = (Word16)((s + (1 << 6)) >> 7);

        /* g0 = i * (1-agc_fac) */
        g0 = (Word16)(((Word32)i *  (Word32)(32767 - agc_fac)) >> 15);
    }

    /* compute gain[n] = agc_fac * gain[n-1]
                        + (1-agc_fac) * sqrt(gain_in/gain_out) */
    /* sig_out[n] = gain[n] * sig_out[n]                        */

    gain = past_gain;

    for (i = 0; i < l_trm; i++)
    {
        gain = (Word16)(((Word32)gain * (Word32)agc_fac) >> 15);
        gain += g0;
        sig_out[i] = (Word16)(((Word32)sig_out[i] * (Word32)gain) >> 12);
    }

    past_gain = gain;
}

void agc2 (
 Word16 *sig_in,        /* (i)     : postfilter input signal  */
 Word16 *sig_out        /* (i/o)   : postfilter output signal */
)
{
    Word16 i, exp;
    Word16 gain_in, gain_out, g0;
    Word32 s;

    Word16 temp;

    /* calculate gain_out with exponent */

    s = 0;
    for (i = 0; i < L_SUBFR; i++)
    {
        temp = sig_out[i] >> 2;
        s += (Word32)(temp * temp);
    }

    if (s == 0)
    {
        return;
    }
		s <<= 1;
    exp = norm_l (s) - 1;
    gain_out = (Word16)(((s << exp) + (1 << 15)) >> 16);


    /* calculate gain_in with exponent */

    s = 0;
    for (i = 0; i < L_SUBFR; i++)
    {
        temp = sig_in[i] >> 2;
        s += (Word32)(temp * temp);
    }

    if (s == 0)
    {
        g0 = 0;
    }
    else
    {
			  s <<= 1;
        i = norm_l (s);
        gain_in = (Word16)(((s << i) + (1 << 15)) >> 16);
        exp -= i;

        /*---------------------------------------------------*
         *  g0 = sqrt(gain_in/gain_out);                     *
         *---------------------------------------------------*/

				s = ((Word32)gain_out << 15) / gain_in;
        s <<= 7;       /* s = gain_out / gain_in */
				if (exp < 0)     /* add exponent */
					s <<= -exp;
				else
					s >>= exp;

        s = Inv_sqrt (s);
        g0 = (Word16)((s + (1 << 6)) >> 7);
    }

    /* sig_out(n) = gain(n) sig_out(n) */

    for (i = 0; i < L_SUBFR; i++)
    {
        sig_out[i] = (Word16)((Word32)(sig_out[i] * g0) >> 12);
    }
}