/*
***********************************************************************
*
* Copyright (C) 1999 Ericsson Radio Systems AB
* 
* The Association of Radio Industries and Businesses (ARIB) has the
* licence to reproduce, distribute and publish the Bit Exact C Code
* programs in this CD-ROM together with the other parts of the RCR
* STD-27 (Personal Digital Cellular Telecommunication System)
* specification.
* 
* Ericsson has declared Option 1 (royalty-free licence) to ARIB for
* its Essential Industrial Property Rights related to the ACELP
* specification and the Bit Exact C Code programs. The royalty-free
* licence is limited to the development and manufacturing of
* products complying with the RCR STD-27H specification (or
* subsequent versions) for sale and use in Japan. Copying of the
* program(s) for any other purpose requires the prior written
* permission from Ericsson.
* 
* The user of the Bit Exact C Code hereby acknowledges the
* following:
* 
* 1.  Ericsson does not provide the user with any warranties,
*     whether express or implied, including but not limited to
*     warranties of merchantability and fitness for a particular
*     purpose relating to the Bit Exact C Code.
* 
* 2.  Ericsson shall have no responsibility whatsoever to indemnify
*     the user against actions or claims for infringement of any
*     intellectual property rights, e.g. patents, copyrights,
*     registered designs or any other rights, by reason of the
*     user's use of the Bit Exact C Code.
* 
***********************************************************************
*/

/*************************************************************************
 *
 *  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"


/* LOCAL PROGRAM CODE  */

static Word32 energy_calc( /* o : return energy of signal     */
    Word16 in[],           /* i : input signal (length l_trm) */
    Word16 l_trm           /* i : signal length               */
)
{
    Word32 s;
    Word16 i, temp;
    Word16 ov_save;

    ov_save = Overflow;          /* save overflow flag in case A-code  */
                                 /* below is invoked                   */
    s = L_mult(in[0], in[0]);
    for (i = 1; i < l_trm; i++)
    {
        s = L_mac(s, in[i], in[i]);
    } 
    /* check for overflow */
    if (L_sub(s, MAX_32) == 0L)
    {
        Overflow = ov_save;      /* A; restore overflow flag */
        temp = shr(in[0], 2);
        s = L_mult(temp, temp);
        for (i = 1; i < l_trm; i++) {
            temp = shr(in[i], 2);
            s = L_mac(s, temp, temp);
        }
    }
    else
    {
        s = L_shr(s, 4);
    } 

    return s;
}

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            */
)
{
    static Word16 past_gain = 4096;
    Word16 i, exp;
    Word16 gain_in, gain_out, g0, gain;
    Word32 s;

    /* calculate gain_out with exponent */

    s = energy_calc(sig_out, l_trm);   

    if (s == 0) {
        past_gain = 0;
        return;
    }
    exp = sub(norm_l(s), 1);
    gain_out = round(L_shl(s, exp));

    /* calculate gain_in with exponent */

    s = energy_calc(sig_in, l_trm);   

    if (s == 0) {
        g0 = 0;
    } else {
        i = norm_l(s);
        gain_in = round(L_shl(s, i));
        exp = sub(exp, i);

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

        s = L_deposit_l(div_s(gain_out, gain_in));
        s = L_shl(s, 7);        /* s = gain_out / gain_in */
        s = L_shr(s, exp);      /* add exponent */

        s = Inv_sqrt(s);
        i = round(L_shl(s, 9));

        /* g0 = i * (1-agc_fac) */
        g0 = mult(i, sub(32767, agc_fac));
    }

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

    gain = past_gain;
    for (i = 0; i < l_trm; i++) {
        gain = mult(gain, agc_fac);
        gain = add(gain, g0);
        sig_out[i] = extract_h(L_shl(L_mult(sig_out[i], gain), 3));
    }
    past_gain = gain;

    return;
}


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

    /* calculate gain_out with exponent */

    s = energy_calc(sig_out, l_trm);   

    if (s == 0) {
        return;
    }
    exp = sub(norm_l(s), 1);
    gain_out = round(L_shl(s, exp));

    /* calculate gain_in with exponent */

    s = energy_calc(sig_in, l_trm);   

    if (s == 0) {
        g0 = 0;
    } else {
        i = norm_l(s);
        gain_in = round(L_shl(s, i));
        exp = sub(exp, i);

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

        s = L_deposit_l(div_s(gain_out, gain_in));
        s = L_shl(s, 7);        /* s = gain_out / gain_in */
        s = L_shr(s, exp);      /* add exponent */

        s = Inv_sqrt(s);
        g0 = round(L_shl(s, 9));
    }

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

    for (i = 0; i < l_trm; i++) {
        sig_out[i] = extract_h(L_shl(L_mult(sig_out[i], g0), 3));
    }

    return;
}




