/*
***********************************************************************
*
* 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 "typedef.h"
#include "basic_op.h"
#include "sig_proc.h"

#define L_CODE    40
#define NB_TRACK  5
#define NB_PULSE  3
#define STEP      5


/* locals functions */

void cor_h_x(
    Word16 h[],  /* (i)  : impulse response of weighted synthesis filter */
    Word16 x[],  /* (i)  : correlation between target and h[]            */
    Word16 dn[]  /* (o)  : correlation between target and h[]            */
);
void set_sign(
    Word16 dn[],   /* (i/o) : correlation between target and h[]   */
    Word16 sign[], /* (o)   : sign of dn[]                         */
    Word16 dn2[]   /* (o)   : maximum of corr. in each track.      */
);
void cor_h(
    Word16 h[],    /* (i) : impulse response of weighted synthesis filter */
    Word16 sign[], /* (o) : sign of dn[]                                  */
    Word16 rr[][L_CODE] /* (o) : matrix of autocorrelation                */
);
void search_3i40(
    Word16 dn[],   /* (i) : correlation between target and h[]             */
    Word16 dn2[],       /* (i) : maximum of corr. in each track.           */
    Word16 rr[][L_CODE],/* (i) : matrix of autocorrelation                 */
    Word16 codvec[]     /* (o) : algebraic codebook vector                 */
);
Word16 build_code(
    Word16 codvec[],    /* (i) : algebraic codebook vector                     */
    Word16 dn_sign[],   /* (i) : sign of dn[]                                  */
    Word16 cod[],       /* (o) : algebraic (fixed) codebook excitation         */
    Word16 h[],         /* (i) : impulse response of weighted synthesis filter */
    Word16 y[],         /* (o) : filtered fixed codebook excitation            */
    Word16 sign[]       /* (o) : index of 3 pulses (position+sign+ampl)*3      */
);

/*************************************************************************
 *
 *  FUNCTION:  code_3i40_14bits()
 *
 *  PURPOSE:  Searches a 14 bit algebraic codebook containing 3 pulses
 *            in a frame of 40 samples.
 *
 *  DESCRIPTION:
 *    The code length is 40, containing 3 nonzero pulses: i0...i2.
 *    All pulses can have two possible amplitudes: +1 or -1.
 *    Pulse i0 can have 8 possible positions, pulses i1 and i2 can have
 *    2x8=16 positions.
 *
 *       i0 :  0, 5, 10, 15, 20, 25, 30, 35.
 *       i1 :  1, 6, 11, 16, 21, 26, 31, 36.
 *             3, 8, 13, 18, 23, 28, 33, 38.
 *       i2 :  2, 7, 12, 17, 22, 27, 32, 37.
 *             4, 9, 14, 19, 24, 29, 34, 39.
 *
 *************************************************************************/

Word16 code_3i40_14bits(
    Word16 x[], /* (i)   : target vector                                 */
    Word16 h[], /* (i)   : impulse response of weighted synthesis filter */
                /*         h[-L_subfr..-1] must be set to zero.          */
    Word16 T0,  /* (i)   : Pitch lag                                     */
    Word16 pitch_sharp, /* (i)   : Last quantized pitch gain             */
    Word16 code[],      /* (o)   : Innovative codebook                   */
    Word16 y[],         /* (o)   : filtered fixed codebook excitation    */
    Word16 * sign       /* (o)   : Signs of 3 pulses                     */
)
{
    Word16 codvec[NB_PULSE];
    Word16 dn[L_CODE], dn2[L_CODE], dn_sign[L_CODE];
    Word16 rr[L_CODE][L_CODE];
    Word16 i, index, sharp;

    sharp = shl(pitch_sharp, 1);
    if (sub(T0, L_CODE) < 0)
        for (i = T0; i < L_CODE; i++) {
            h[i] = add(h[i], mult(h[i - T0], sharp));
        }
    cor_h_x(h, x, dn);
    set_sign(dn, dn_sign, dn2);
    cor_h(h, dn_sign, rr);
    search_3i40(dn, dn2, rr, codvec);
    index = build_code(codvec, dn_sign, code, h, y, sign);

  /*-----------------------------------------------------------------*
  * Compute innovation vector gain.                                 *
  * Include fixed-gain pitch contribution into code[].              *
  *-----------------------------------------------------------------*/

    if (sub(T0, L_CODE) < 0)
        for (i = T0; i < L_CODE; i++) {
            code[i] = add(code[i], mult(code[i - T0], sharp));
        }
    return index;
}


/*************************************************************************
 *
 *  FUNCTION:  cor_h_x()
 *
 *  PURPOSE:  Computes correlation between target signal "x[]" and
 *            impulse response"h[]".
 *
 *************************************************************************/

void cor_h_x(
    Word16 h[], /* (i)  : impulse response of weighted synthesis filter */
    Word16 x[], /* (i)  : correlation between target and h[]            */
    Word16 dn[] /* (o)  : correlation between target and h[]            */
)
{
    Word16 i, j, k;
    Word32 s, y32[L_CODE], max, tot;

    /* first keep the result on 32 bits and find absolute maximum */

    tot = 5;

    for (k = 0; k < NB_TRACK; k++) {
        max = 0;
        for (i = k; i < L_CODE; i += STEP) {
            s = 0;
            for (j = i; j < L_CODE; j++)
                s = L_mac(s, x[j], h[j - i]);

            y32[i] = s;

            s = L_abs(s);
            if (L_sub(s, max) > (Word32) 0) {
                max = s;
            }
        }
        tot = L_add(tot, L_shr(max, 1));
    }

    j = sub(norm_l(tot), 1);    /* multiply tot by 2 */

    for (i = 0; i < L_CODE; i++) {
        dn[i] = round(L_shl(y32[i], j));
    }

    return;
}


/*************************************************************************
 *
 *  FUNCTION  set_sign()
 *
 *  PURPOSE: Builds sign[] vector according to "dn[]" and "cn[]".
 *           Also finds the position of maximum of correlation in each track
 *           and the starting position for each pulse.
 *
 *************************************************************************/

void set_sign(
    Word16 dn[],        /* (i/o)  : correlation between target and h[]   */
    Word16 sign[],      /* (o)    : sign of dn[]                         */
    Word16 dn2[]        /* (o)    : maximum of corr. in each track.      */
)
{
    Word16 i, j, k;
    Word16 val, min, pos;

    /* set sign according to dn[] */

    for (i = 0; i < L_CODE; i++) {
        val = dn[i];

        if (val >= 0) {
            sign[i] = 32767;
        } else {
            sign[i] = -32767;
            val = negate(val);
        }
        dn[i] = val;    /* modify dn[] according to the fixed sign */
        dn2[i] = val;
    }

    /* keep 2 maximum positions/8 of each track and store it in dn2[] */

    for (i = 0; i < NB_TRACK; i++) {
        for (k = 0; k < 2; k++) {
            min = 0x7fff;
            for (j = i; j < L_CODE; j += STEP) {
                if (dn2[j] >= 0) {
                    val = sub(dn2[j], min);
                    if (val < 0) {
                        min = dn2[j];
                        pos = j;
                    }
                }
            }
            dn2[pos] = -1;
        }
    }

    return;
}

/*************************************************************************
 *
 *  FUNCTION:  cor_h()
 *
 *  PURPOSE:  Computes correlations of h[] needed for the codebook search;
 *            and include the sign information into the correlations.
 *
 *  DESCRIPTION: The correlations are given by
 *         rr[i][j] = sum_{n=i}^{L-1} h[n-i] h[n-j];   i>=j; i,j=0,...,L-1
 *
 *  and the sign information is included by
 *         rr[i][j] = rr[i][j]*sign[i]*sign[j]
 *
 *************************************************************************/

void cor_h(
    Word16 h[],  /* (i) : impulse response of weighted synthesis filter   */
    Word16 sign[],      /* (o) : sign of dn[]                             */
    Word16 rr[][L_CODE] /* (o) : matrix of autocorrelation                */
)
{
    Word16 i, j, k, dec, h2[L_CODE];
    Word32 s;

    /* Scaling for maximum precision */

    s = 2;
    for (i = 0; i < L_CODE; i++)
        s = L_mac(s, h[i], h[i]);

    j = sub(extract_h(s), 32767);
    if (j == 0)
        for (i = 0; i < L_CODE; i++) {
            h2[i] = shr(h[i], 1);
    } else {
        s = L_shr(s, 1);
        k = extract_h(L_shl(Inv_sqrt(s), 7));
        k = mult(k, 32440);

        for (i = 0; i < L_CODE; i++) {
            h2[i] = round(L_shl(L_mult(h[i], k), 9));
        }
    }

    /* build matrix rr[] */

    s = 0;
    i = L_CODE - 1;
    for (k = 0; k < L_CODE; k++, i--) {
        s = L_mac(s, h2[k], h2[k]);
        rr[i][i] = round(s);
    }

    for (dec = 1; dec < L_CODE; dec++) {
        s = 0;
        j = L_CODE - 1;
        i = sub(j, dec);
        for (k = 0; k < (L_CODE - dec); k++, i--, j--) {
            s = L_mac(s, h2[k], h2[k + dec]);
            rr[i][j] = rr[j][i] = mult(round(s), mult(sign[i], sign[j]));
        }
    }
}

/*************************************************************************
 *
 *  FUNCTION  search_3i40()
 *
 *  PURPOSE: Search the best codevector; determine positions of the 3 pulses
 *           in the 40-sample frame.
 *
 *************************************************************************/


#define _1_2    (Word16)(32768L/2)
#define _1_4    (Word16)(32768L/4)
#define _1_8    (Word16)(32768L/8)
#define _1_16   (Word16)(32768L/16)

void search_3i40(
    Word16 dn[],  /* (i) : correlation between target and h[]        */
    Word16 dn2[], /* (i) : maximum of corr. in each track.           */
    Word16 rr[][L_CODE], /* (i) : matrix of autocorrelation          */
    Word16 codvec[]      /* (o) : algebraic codebook vector          */
)
{
    Word16 i0, i1, i2;
    Word16 i, pos, ix, track1, track2, ipos[NB_PULSE];
    Word16 psk, ps, ps0, ps1, sq, sq1;
    Word16 alpk, alp, alp_16;
    Word32 s, alp0, alp1;

    psk = -1;
    alpk = 1;
    codvec[0] = 0;
    codvec[1] = 1;
    codvec[2] = 2;

    for (track1 = 1; track1 < 4; track1 += 2) {
       for (track2 = 2; track2 < 5; track2 += 2) {		
          /* fix starting position */

          ipos[0] = 0;       
          ipos[1] = track1;  
          ipos[2] = track2;  

          /*------------------------------------------------------------------*
           * main loop: try 3 tracks.                                         *
           *------------------------------------------------------------------*/

          for (i = 0; i < NB_PULSE; i++) {
             
             /*----------------------------------------------------------------*
              * i0 loop: try 8 positions.                                      *
              *----------------------------------------------------------------*/
             
              /* account for ptr. init. (rr[io]) */
             for (i0 = ipos[0]; i0 < L_CODE; i0 += STEP) {
                
                if (dn2[i0] >= 0) {

                   ps0 = dn[i0];  
                   alp0 = L_mult(rr[i0][i0], _1_4);

                   /*----------------------------------------------------------------*
                    * i1 loop: 8 positions.                                          *
                    *----------------------------------------------------------------*/

                   sq = -1;  
                   alp = 1;  
                   ps = 0;
                   ix = ipos[1];

                   /* initialize 4 index for next loop. */
                   /*-------------------------------------------------------------------*
                    *  These index have low complexity address computation because      *
                    *  they are, in fact, pointers with fixed increment.  For example,  *
                    *  "rr[i0][i2]" is a pointer initialized to "&rr[i0][ipos[2]]"      *
                    *  and incremented by "STEP".                                       *
                    *-------------------------------------------------------------------*/
                    /* account for ptr. init. (rr[i1]) */
                    /* account for ptr. init. (rr[io]) */
                   for (i1 = ipos[1]; i1 < L_CODE; i1 += STEP) {
                      ps1 = add(ps0, dn[i1]);   /* idx increment = STEP */
                      
                      /* alp1 = alp0 + rr[i0][i1] + 1/2*rr[i1][i1]; */
                      
                      alp1 = L_mac(alp0, rr[i1][i1], _1_4); /* idx incr = STEP */
                      alp1 = L_mac(alp1, rr[i0][i1], _1_2); /* idx incr = STEP */
                      
                      sq1 = mult(ps1, ps1);
                      
                      alp_16 = round(alp1);
                      
                      s = L_msu(L_mult(alp, sq1), sq, alp_16);
                      
                      
                      if (s > 0) {
                         sq = sq1;      
                         ps = ps1;      
                         alp = alp_16;  
                         ix = i1;       
                      }
		   }
                   i1 = ix;             

                   /*----------------------------------------------------------------*
                    * i2 loop: 8 positions.                                          *
                    *----------------------------------------------------------------*/

                   ps0 = ps;            
                   alp0 = L_mult(alp, _1_4);

                   sq = -1;             
                   alp = 1;             
                   ps = 0;
                   ix = ipos[2];

                   /* initialize 4 index for next loop (see i1 loop) */

                     /* account for ptr. init. (rr[i2]) */
                     /* account for ptr. init. (rr[i1]) */
                     /* account for ptr. init. (rr[io]) */
                    for (i2 = ipos[2]; i2 < L_CODE; i2 += STEP) {
                       ps1 = add(ps0, dn[i2]); /* index increment = STEP */
                       
                       /* alp1 = alp0 + rr[i0][i2] + rr[i1][i2] + 1/2*rr[i2][i2]; */
                       
                       alp1 = L_mac(alp0, rr[i2][i2], _1_16); /* idx incr = STEP */
                       alp1 = L_mac(alp1, rr[i1][i2], _1_8);  /* idx incr = STEP */
                       alp1 = L_mac(alp1, rr[i0][i2], _1_8);  /* idx incr = STEP */
                       
                       sq1 = mult(ps1, ps1);
                       
                       alp_16 = round(alp1);
                       
                       s = L_msu(L_mult(alp, sq1), sq, alp_16);
                       
                       
                       if (s > 0) {
                          sq = sq1;      
                          ps = ps1;      
                          alp = alp_16;  
                          ix = i2;       
                       }
		   }
                   i2 = ix;             

                   /*----------------------------------------------------------------*
                    * memorise codevector if this one is better than the last one.   *
                    *----------------------------------------------------------------*/
                   s = L_msu(L_mult(alpk, sq), psk, alp);

                   if (s > 0) {
                      psk = sq;         
                      alpk = alp;       
                      codvec[0] = i0;   
                      codvec[1] = i1;   
                      codvec[2] = i2;   
                   }
		}
	     }

             /*----------------------------------------------------------------*
             * Cyclic permutation of i0, i1 and i2.                           *
             *----------------------------------------------------------------*/
             
             pos = ipos[2];          
             ipos[2] = ipos[1];      
             ipos[1] = ipos[0];      
             ipos[0] = pos;          
	  }
       }
    }
    return;
}

/*************************************************************************
 *
 *  FUNCTION:  build_code()
 *
 *  PURPOSE: Builds the codeword, the filtered codeword and index of the
 *           codevector, based on the signs and positions of 3 pulses.
 *
 *************************************************************************/

Word16
build_code(
    Word16 codvec[],  /* (i) : position of pulses                            */
    Word16 dn_sign[], /* (i) : sign of pulses                                */
    Word16 cod[],     /* (o) : innovative code vector                        */
    Word16 h[],       /* (i) : impulse response of weighted synthesis filter */
    Word16 y[],       /* (o) : filtered innovative code                      */
    Word16 sign[]     /* (o) : index of 3 pulses                             */
)
{
    Word16 i, j, k, track, index, _sign[NB_PULSE], indx, rsign;
    Word16 *p0, *p1, *p2;
    Word32 s;

    for (i = 0; i < L_CODE; i++) {
        cod[i] = 0;
    }

    indx = 0;
    rsign = 0;
    for (k = 0; k < NB_PULSE; k++) {
        i = codvec[k];    /* read pulse position */
        j = dn_sign[i];   /* read sign          */

        index = mult(i, 6554);    /* index = pos/5 */
        /* track = pos%5 */
        track = sub(i, extract_l(L_shr(L_mult(index, 5), 1)));

        if (sub(track, 1) == 0)
          index = shl(index, 4);
        else if (sub(track, 2) == 0) {
           track = 2;                          
           index = shl(index, 8);
        } else if (sub(track, 3) == 0) {
           track = 1;                                   
           index = add(shl(index, 4), 8);
        } else if (sub(track, 4) == 0) {
           track = 2;                          
           index = add(shl(index, 8), 128);
        }
       
        if (j > 0) {
            cod[i] = 8191;
            _sign[k] = 32767;
            rsign = add(rsign, shl(1, track));
        } else {
            cod[i] = -8192;
            _sign[k] = (Word16) - 32768L;
        }

        indx = add(indx, index);
    }
    *sign = rsign;

    p0 = h - codvec[0];
    p1 = h - codvec[1];
    p2 = h - codvec[2];

    for (i = 0; i < L_CODE; i++) {
        s = 0;
        s = L_mac(s, *p0++, _sign[0]);
        s = L_mac(s, *p1++, _sign[1]);
        s = L_mac(s, *p2++, _sign[2]);
        y[i] = round(s);
    }

    return indx;
}

