/*
   ITU-T G.729 Speech Coder with Annex B    ANSI-C Source Code
   Version 1.3    Last modified: August 1997

   Copyright (c) 1996,
   AT&T, France Telecom, NTT, Universite de Sherbrooke, Lucent Technologies,
   Rockwell International
   All rights reserved.
*/

/************************************************************************/
/*      Post - filtering : short term + long term                       */
/************************************************************************/
#include <stdio.h>
#include <stdlib.h>

/**** Prototypes                                                        */
#include "typedef.h"
#include "ld8k.h"
#include "basic_op.h"
#include "oper_32b.h"


/* Prototypes for local functions */
/**********************************/

static void pst_ltp(Word16 t0, Word16 *ptr_sig_in,
    Word16 *ptr_sig_pst0, Word16 *vo);
static void search_del(Word16 t0, Word16 *ptr_sig_in, Word16 *ltpdel,
    Word16 *phase, Word16 *num_gltp, Word16 *den_gltp, Word16 *sh_num_gltp,
    Word16 *sh_den_gltp, Word16 *y_up, Word16 *off_yup);
static void filt_plt( Word16 *s_in, Word16 *s_ltp,
    Word16 *s_out, Word16 gain_plt);
static void compute_ltp_l(Word16 *s_in, Word16 ltpdel, Word16 phase,
    Word16 *y_up, Word16 *num, Word16 *den, Word16 *sh_num, Word16 *sh_den);
static Word16 select_ltp(Word16 num1, Word16 den1, Word16 sh_num1,
    Word16 sh_den1, Word16 num2, Word16 den2, Word16 sh_num2, Word16 sh_den2);
static void calc_st_filt(Word16 *apond2, Word16 *apond1, Word16 *parcor0,
    Word16 *signal_ltp0_ptr);
static void filt_mu(Word16 *sig_in, Word16 *sig_out, Word16 parcor0);
static void calc_rc0_h(Word16 *h, Word16 *rc0);
static void scale_st(Word16 *sig_in, Word16 *sig_out, Word16 *gain_prec);

/* Static arrays and variables */
/*******************************/

/* Arrays */
static Word16 apond2[LONG_H_ST];    /* s.t. numerator coeff.        */
static Word16 mem_stp[M];           /* s.t. postfilter memory       */
static Word16 mem_zero[M];          /* null memory to compute h_st  */
static Word16 res2[SIZ_RES2];       /* A(gamma2) residual           */

/* pointers */
Word16 *res2_ptr;
Word16 *ptr_mem_stp;

/* Variables */
Word16 gain_prec;

/************************************************************************/
/****   Short term postfilter :                                     *****/
/*      Hst(z) = Hst0(z) Hst1(z)                                        */
/*      Hst0(z) = 1/g0 A(gamma2)(z) / A(gamma1)(z)                      */
/*      if {hi} = i.r. filter A(gamma2)/A(gamma1) (truncated)           */
/*      g0 = SUM(|hi|) if > 1                                           */
/*      g0 = 1. else                                                    */
/*      Hst1(z) = 1/(1 - |mu|) (1 + mu z-1)                             */
/*      with mu = k1 * gamma3                                           */
/*      k1 = 1st parcor calculated on {hi}                              */
/*      gamma3 = gamma3_minus if k1<0, gamma3_plus if k1>0              */
/****   Long term postfilter :                                      *****/
/*      harmonic postfilter :   H0(z) = gl * (1 + b * z-p)              */
/*      b = gamma_g * gain_ltp                                          */
/*      gl = 1 / 1 + b                                                  */
/*      computation of delay p on A(gamma2)(z) s(z)                     */
/*      sub optimal search                                              */
/*      1. search around 1st subframe delay (3 integer values)          */
/*      2. search around best integer with fract. delays (1/8)          */
/************************************************************************/

/*----------------------------------------------------------------------------
 * Init_Post_Filter -  Initialize postfilter functions
 *----------------------------------------------------------------------------
 */
void Init_Post_Filter(
    void
)
{
    int i;

    /* Initialize arrays and pointers */

    /* res2 =  A(gamma2) residual */
    for(i=0; i<MEM_RES2; i++) res2[i] = 0;
    res2_ptr = res2 + MEM_RES2;

    /* 1/A(gamma1) memory */
    for(i=0; i<M; i++) mem_stp[i] = 0;
    ptr_mem_stp = mem_stp + M - 1;

    /* fill apond2[M+1->LONG_H_ST-1] with zeroes */
    for(i=MP1; i<LONG_H_ST; i++) apond2[i] = 0;

    /* null memory to compute i.r. of A(gamma2)/A(gamma1) */
    for(i=0; i<M; i++) mem_zero[i] = 0;

    /* for gain adjustment */
    gain_prec = 16384;

    return;
}

/*----------------------------------------------------------------------------
 * Post - adaptive postfilter main function
 *----------------------------------------------------------------------------
 */
void Post(
 Word16 t0,             /* input : pitch delay given by coder */
 Word16 *signal_ptr,    /* input : input signal (pointer to current subframe */
 Word16 *coeff,         /* input : LPC coefficients for current subframe */
 Word16 *sig_out,       /* output: postfiltered output */
 Word16 *vo,            /* output: voicing decision 0 = uv,  > 0 delay */
 Word16 Vad             /* input : frame type */
)
{

    /* Local variables and arrays */
    Word16 apond1[MP1];             /* s.t. denominator coeff.      */
    Word16 sig_ltp[L_SUBFRP1];      /* H0 output signal             */
    Word16 *sig_ltp_ptr;
    Word16 parcor0;

    /* Compute weighted LPC coefficients */
    Weight_Az(coeff, GAMMA1_PST, M, apond1);
    Weight_Az(coeff, GAMMA2_PST, M, apond2);

    /* Compute A(gamma2) residual */
    Residu(apond2, signal_ptr, res2_ptr, L_SUBFR);

    /* Harmonic filtering */
    sig_ltp_ptr = sig_ltp + 1;
    if (sub(Vad, 1) == 0)
      pst_ltp(t0, res2_ptr, sig_ltp_ptr, vo);
    else{
      *vo = 0;
      Copy(res2_ptr, sig_ltp_ptr, L_SUBFR);
    }

    /* Save last output of 1/A(gamma1)  */
    /* (from preceding subframe)        */
    sig_ltp[0] = *ptr_mem_stp;

    /* Controls short term pst filter gain and compute parcor0   */
    calc_st_filt(apond2, apond1, &parcor0, sig_ltp_ptr);

    /* 1/A(gamma1) filtering, mem_stp is updated */
    Syn_filt(apond1, sig_ltp_ptr, sig_ltp_ptr, L_SUBFR, mem_stp, 1);

    /* Tilt filtering */
    filt_mu(sig_ltp, sig_out, parcor0);

    /* Gain control */
    scale_st(signal_ptr, sig_out, &gain_prec);

    /**** Update for next subframe */
    Copy(&res2[L_SUBFR], &res2[0], MEM_RES2);

    return;
}

/*----------------------------------------------------------------------------
 *  pst_ltp - harmonic postfilter
 *----------------------------------------------------------------------------
 */
static void pst_ltp(
 Word16 t0,             /* input : pitch delay given by coder */
 Word16 *ptr_sig_in,    /* input : postfilter input filter (residu2) */
 Word16 *ptr_sig_pst0,  /* output: harmonic postfilter output */
 Word16 *vo             /* output: voicing decision 0 = uv,  > 0 delay */
)
{

/**** Declare variables                                 */
    int i;
    Word16 temp;
    Word16 ltpdel, phase;
    Word16 num_gltp, den_gltp;
    Word16 num2_gltp, den2_gltp;
    Word16 sh_num, sh_den;
    Word16 sh_num2, sh_den2;
    Word16 gain_plt;
    Word16 y_up[SIZ_Y_UP];
    Word16 *ptr_y_up;
    Word16 off_yup;
    Word16 *ptr_sig;
    Word16 sig_cadr[SIZ_RES2], *ptr_sig_cadr;
    Word16 nb_sh_sig;
    Word32 L_temp;

    /* input signal justified on 13 bits */
    ptr_sig = ptr_sig_in - MEM_RES2;

    temp = 0;
    for(i=0; i<SIZ_RES2; i++) {
        temp |= abs_s(ptr_sig[i]);
    }
    nb_sh_sig = sub(3, norm_s(temp));

    for(i=0; i<SIZ_RES2; i++) {   /* nb_sh_sig may be >0, <0 or =0 */
        sig_cadr[i] = shr( ptr_sig[i], nb_sh_sig);
    }

    ptr_sig_cadr = sig_cadr + MEM_RES2;

    /* Sub optimal delay search */
    search_del(t0, ptr_sig_cadr, &ltpdel, &phase, &num_gltp, &den_gltp,
                            &sh_num, &sh_den, y_up, &off_yup);
    *vo = ltpdel;

    if(num_gltp == 0)   {
        Copy(ptr_sig_in, ptr_sig_pst0, L_SUBFR);
    }

    else {
        if(phase == 0) {
            ptr_y_up = ptr_sig_in - ltpdel;
        }

        else {

            /* Filtering with long filter */
            compute_ltp_l(ptr_sig_cadr, ltpdel, phase, ptr_sig_pst0,
                &num2_gltp, &den2_gltp, &sh_num2, &sh_den2);

            if(select_ltp(num_gltp, den_gltp, sh_num, sh_den,
                num2_gltp, den2_gltp, sh_num2, sh_den2) == 1) {

                /* select short filter */
                temp   = sub(phase, 1);
                L_temp = L_mult(temp, L_SUBFRP1);
                L_temp = L_shr(L_temp, 1);
                temp   = extract_l(L_temp);
                temp   = add(temp, off_yup);
                /* ptr_y_up = y_up + (phase-1) * L_SUBFRP1 + off_yup */
                ptr_y_up = y_up + temp;
            }
            else {
                /* select long filter */
                num_gltp = num2_gltp;
                den_gltp = den2_gltp;
                sh_num   = sh_num2;
                sh_den   = sh_den2;
                ptr_y_up = ptr_sig_pst0;
            }

            /* rescale y_up */
            for(i=0; i<L_SUBFR; i++) {  /* nb_sh_sig may be >0, <0 or =0 */
                ptr_y_up[i] = shl(ptr_y_up[i], nb_sh_sig);
            }
        }

        temp = sub(sh_num,sh_den);
        if(temp >= 0) den_gltp = shr(den_gltp, temp);
        else {
            num_gltp = shl(num_gltp, temp); /*  >> (-temp) */
        }

        if(sub(num_gltp ,den_gltp)>=0) {
            /* beta bounded to 1 */
            gain_plt = MIN_GPLT;
        }
        else {

            /* GAMMA_G = 0.5                                            */
            /* gain_plt = den_gltp x 2**15 / (den_gltp + 0.5 num_gltp)  */
            /* shift 1 bit to avoid overflows in add                    */
            num_gltp = shr(num_gltp, 2);
            den_gltp = shr(den_gltp, 1);
            temp     = add(den_gltp, num_gltp);
            gain_plt = div_s(den_gltp, temp);   /* Q15 */
        }

        /** filtering by H0(z) = harmonic filter **/
        filt_plt(ptr_sig_in, ptr_y_up, ptr_sig_pst0, gain_plt);

    }

    return;
}

/*----------------------------------------------------------------------------
 *  search_del: computes best (shortest) integer LTP delay + fine search
 *----------------------------------------------------------------------------
 */
static void search_del(
 Word16 t0,                /* input : pitch delay given by coder */
 Word16 *ptr_sig_in,       /* input : input signal (with delay line) */
 Word16 *ltpdel,           /* output: delay = *ltpdel - *phase / f_up */
 Word16 *phase,            /* output: phase */
 Word16 *num_gltp,         /* output: 16 bits numerator of LTP gain */
 Word16 *den_gltp,         /* output: 16 bits denominator of LTP gain */
 Word16 *sh_num_gltp,      /* output: justification for num_gltp */
 Word16 *sh_den_gltp,      /* output: justification for den_gltp */
 Word16 *y_up,             /* output: LT delayed signal if fract. delay */
 Word16 *off_yup           /* output: offset in y_up */
)
{

    /* Tables of constants */
    extern Word16 tab_hup_s[SIZ_TAB_HUP_S];

    Word32 L_den0[F_UP_PST-1];
    Word32 L_den1[F_UP_PST-1];
    Word32 *ptr_L_den0, *ptr_L_den1;

    int i, n;
    Word16 *ptr_h;
    Word16 *ptr_sig_past, *ptr_sig_past0;
    Word16 *ptr1, *ptr_y_up;
    Word16 num, den0, den1;
    Word16 den_max, num_max;
    Word32 L_num_int, L_den_int, L_den_max;
    Word16 hi_numsq, hi_numsq_max;
    Word16 lo_numsq, lo_numsq_max;
    Word16 ener;
    Word16 sh_num, sh_den, sh_ener;
    Word16 i_max, lambda, phi, phi_max, ioff;
    Word16 temp;
    Word32 L_temp0, L_temp1;
    Word32 L_acc;
    Word32 L_temp;


    /* Computes energy of current signal */
    /*************************************/

    L_acc = 0L;
    for(i=0; i<L_SUBFR; i++) {
        L_acc = L_mac( L_acc, ptr_sig_in[i] , ptr_sig_in[i]);
    }
    if(L_acc == 0) {
        *num_gltp = 0;
        *den_gltp = 1;
        *ltpdel = 0;
        *phase = 0;
        return;
    }
    sh_ener = sub(16, norm_l(L_acc));