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

#include <stdio.h>
#include <stdlib.h>
#include "typedef.h"
#include "basic_op.h"
#include "cnst.h"
#include "ph_disp.tab"

#define PHDGAINMEMSIZE 5
#define PHDTHR1LTP     9830  /* 0.6 in Q14 */
#define PHDTHR2LTP     14746 /* 0.9 in Q14 */
#define ONFACTPLUS1    16384 /* 2.0 in Q13   */
#define ONLENGTH       2

/*************************************************************************
*
*  Function:   ph_disp
*
*              Adaptive phase dispersion; forming of total excitation
*              (for synthesis part of decoder)
*
**************************************************************************
*/
void ph_disp (
      Word16 x[],          /* i/o Q0  : in:  LTP excitation signal       */
                           /*           out: total excitation signal     */
      Word16 cbGain,       /* i   Q1  : Codebook gain                    */
      Word16 ltpGain,      /* i   Q14 : LTP gain                         */
      Word16 inno[],       /* i/o Q13 : Innovation vector                */
      Word16 pitch_fac,    /* i   Q14 : pitch factor used to scale the
                                        LTP excitation                   */
      Word16 tmp_shift     /* i   Q0  : shift factor applied to sum of   
                                        scaled LTP ex & innov. before
                                        rounding                         */
)
{
   Word16 i, i1;
   Word16 tmp1;
   Word32 L_temp;
   Word16 impNr;           /* indicator for amount of disp./filter used */

   Word16 inno_sav[L_SUBFR];
   Word16 ps_poss[L_SUBFR];
   Word16 j, nze, nPulse, ppos;
   const Word16 *ph_imp;   /* Pointer to phase dispersion filter */

   static Word16 gainMem[PHDGAINMEMSIZE] = {0, 0, 0, 0, 0};
   static Word16 prevState = 0;
   static Word16 prevCbGain = 0;
   static Word16 onset = 0;

   /* Update LTP gain memory */
   for (i = PHDGAINMEMSIZE-1; i > 0; i--)
   {
       gainMem[i] = gainMem[i-1];                    
   }
   gainMem[0] = ltpGain;                                    
   
   /* basic adaption of phase dispersion */
   
   if (sub(ltpGain, PHDTHR2LTP) < 0) {    /* if (ltpGain < 0.9) */
       
       if (sub(ltpGain, PHDTHR1LTP) > 0) { /* if (ltpGain > 0.6 */
           impNr = 1; /* medium dispersion */                      
       }
       else {
           impNr = 0; /* maximum dispersion */                     
       }
   }
   else {
       impNr = 2; /* no dispersion */                              
   }
   
   /* onset indicator */
   /* onset = (cbGain  > onFact * cbGainMem[0]) */
                                                                   
   tmp1 = round(L_shl(L_mult(prevCbGain, ONFACTPLUS1), 2));
   
   if (sub(cbGain, tmp1) > 0)
   {
       onset = ONLENGTH;                                    
   }
   else
   {
        
       if (onset > 0)
       {
           onset = sub (onset, 1);                   
       }
   }
   
   /* if not onset, check ltpGain buffer and use max phase dispersion if
      half or more of the ltpGain-parameters say so */
    
   if (onset == 0)
   {
       /* Check LTP gain memory and set filter accordingly */
       i1 = 0;                                                     
       for (i = 0; i < PHDGAINMEMSIZE; i++)
       {
           
           if (sub(gainMem[i], PHDTHR1LTP) < 0)
           {
               i1 = add (i1, 1);
           }
       }
       
       if (sub(i1, 2) > 0)
       {
           impNr = 0;                                              
       }
       
   }
   /* Restrict decrease in phase dispersion to one step if not onset */
   
   if((sub(impNr, add(prevState, 1)) > 0) && (onset == 0))
   {
       impNr = sub (impNr, 1);
   }
   /* if onset, use one step less phase dispersion */
   
   if((sub(impNr, 2) < 0) && (onset > 0))
   {
       impNr = add (impNr, 1);
   }
   
   /* disable for very low levels */
   
   if(sub(cbGain, 10) < 0)
   {
       impNr = 2;                                                  
   }
   
   /* update static memory */
   prevState = impNr;                                       
   prevCbGain = cbGain;                                     

   /* don't modify the innovation if impNr >=2 (= no phase disp) */
   if(sub(impNr, 2) < 0)
   {
       /* track pulse positions, save innovation,
          and initialize new innovation          */
       nze = 0;                                                    
       for (i = 0; i < L_SUBFR; i++)
       {
           if (inno[i] != 0)
           {
               ps_poss[nze] = i;                                   
               nze = add (nze, 1);
           }
           inno_sav[i] = inno[i];                                  
           inno[i] = 0;                                            
       }
       
       if (impNr == 0)
       {
           ph_imp = ph_imp_low;
       }
       else
       {
           ph_imp = ph_imp_mid;
       }
       
       /* Do phase dispersion of innovation */
       for (nPulse=0; nPulse<nze; nPulse++)
       {
           ppos = ps_poss[nPulse];                                   
           
           /* circular convolution with impulse response */
           j = 0;                                                    
           for (i = ppos; i < L_SUBFR; i++)
           {
               /* inno[i1] += inno_sav[ppos] * ph_imp[i1-ppos] */
               tmp1 = mult(inno_sav[ppos], ph_imp[j++]);
               inno[i] = add(inno[i], tmp1);                         
           }    
           
           for (i = 0; i < ppos; i++)
           {
               /* inno[i] += inno_sav[ppos] * ph_imp[L_SUBFR-ppos+i] */
               tmp1 = mult(inno_sav[ppos], ph_imp[j++]);
               inno[i] = add(inno[i], tmp1);                         
           }
       }
   }
       
   /* compute total excitation for synthesis part of decoder
      (using modified innovation if phase dispersion is active) */
   for (i = 0; i < L_SUBFR; i++)
   {
       /* x[i] = gain_pit*x[i] + cbGain*code[i]; */
       L_temp = L_mult (        x[i],    pitch_fac); /* Q0*Q14 */
       L_temp = L_mac  (L_temp, inno[i], cbGain);    /* Q13*Q1 */
       L_temp = L_shl (L_temp, tmp_shift);           /* Q16    */           
       x[i] = round (L_temp);                                         
   }

   return;
}
