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

/***********************************************************************
 *
 * routine:   Q_plsf_3()
 *
 * Quantization of LSF parameters with 1st order MA prediction and
 * split by 3 matrix quantization (split-MQ)
 *
 ***********************************************************************/

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

/* Locals functions */

void Lsf_wt(
    Word16 * lsf,  /* (i)  : LSF vector                   */
    Word16 * wf2); /* (o)  : square of weighting factors  */

Word16 Vq_subvec3(     /* (o)  : return quantization index     */
    Word16 * lsf_r1,   /* (i)  : 1st LSF residual vector       */
    Word16 * dico,     /* (i)  : quantization codebook         */
    Word16 * wf1,      /* (i)  : 1st LSF weighting factors     */
    Word16 dico_size); /* (i)  : size of quantization codebook */

Word16 Vq_subvec4(      /* (o)  : return quantization index     */
    Word16 * lsf_r1,    /* (i)  : 1st LSF residual vector       */
    Word16 * dico,      /* (i)  : quantization codebook         */
    Word16 * wf1,       /* (i)  : 1st LSF weighting factors     */
    Word16 dico_size);  /* (i)  : size of quantization codebook */


/* M  ->order of linear prediction filter                      */
/* LSF_GAP  -> Minimum distance between LSF after quantization */
/*             50 hz = 205                                     */

#define M         10
#define LSF_GAP   205

#include "q_plsf_3.tab"        /* Codebooks of LSF prediction residual */

 /* Past quantized prediction error */

static Word16 past_rq[M] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

void Q_plsf_3(
    Word16 * lsp1,      /* (i)  : 1st LSP vector                     */
    Word16 vox,         /* (i)  : vox mode                           */
    Word16 *lsp1_q,     /* (o)  : quantized 1st LSP vector           */
    Word16 *indice,     /* (o)  : quantization indices of 3 matrices */
    Word16 *pred_init_i /* (i)  : init index for MA predictor in vox mode */
)
{
    Word16 i,j, index, index2, *p_dico;
    Word16 lsf1[M], wf1[M], lsf_p[M], lsf_r1[M];
    Word16 lsf1_q[M];

    Word16 temp_r1[M];
    Word16 temp_p[M];
    Word32 L_pred_init_err;
    Word32 L_min_pred_init_err;

    /* convert LSFs to normalize frequency domain 0..16384 */

    Lsp_lsf(lsp1, lsf1, M);

    /* compute LSF weighting factors */

    Lsf_wt(lsf1, wf1);

    /* Compute predicted LSF and prediction error */
    if (vox == 0) {
       /* speech mode, normal computation */
       for (i = 0; i < M; i++) {
          lsf_p[i] = add(mean_lsf[i], mult(past_rq[i], pred_fac[i]));
          lsf_r1[i] = sub(lsf1[i], lsf_p[i]);
       }
    } else {
       /* vox mode, search the init vector that    */
       /* yields lowest prediction residual energy */
       *pred_init_i = 0;
       L_min_pred_init_err = MAX_32;
       for (j = 0; j < PAST_RQ_INIT_SIZE; j++) {

          L_pred_init_err = 0;
          index2 = add(j,add(j,j));

          index = past_rq_init[index2++];
          p_dico = &dico1_lsf[add(index, add(index, index))];            
          for (i = 0; i < 3; i++) {
             temp_p[i] = add(mean_lsf[i], *p_dico++);
             temp_r1[i] = sub(lsf1[i],temp_p[i]);
             L_pred_init_err = L_mac(L_pred_init_err, temp_r1[i], temp_r1[i]);
          }
           
          index = past_rq_init[index2++];
          p_dico = &dico2_lsf[add(index, add(index, index))];   
          for (i = 3; i < 6; i++) {
             temp_p[i] = add(mean_lsf[i], *p_dico++);
             temp_r1[i] = sub(lsf1[i],temp_p[i]);
             L_pred_init_err = L_mac(L_pred_init_err, temp_r1[i], temp_r1[i]);
          }

          index = past_rq_init[index2];
          p_dico = &dico3_lsf[shl(index,2)];            
          for (i = 6; i < M; i++) {
             temp_p[i] = add(mean_lsf[i], *p_dico++);
             temp_r1[i] = sub(lsf1[i],temp_p[i]);
             L_pred_init_err = L_mac(L_pred_init_err, temp_r1[i], temp_r1[i]);
          }

          if (L_sub(L_pred_init_err, L_min_pred_init_err) < (Word32) 0) {
             L_min_pred_init_err = L_pred_init_err;

             Copy(temp_r1, lsf_r1, M);
             Copy(temp_p, lsf_p, M);
             *pred_init_i = j;
          }
       }
    }

    /*---- Split-VQ of prediction error ----*/

    indice[0] = Vq_subvec3(&lsf_r1[0], dico1_lsf, &wf1[0], DICO1_SIZE);

    indice[1] = Vq_subvec3(&lsf_r1[3], dico2_lsf, &wf1[3], DICO2_SIZE);

    indice[2] = Vq_subvec4(&lsf_r1[6], dico3_lsf, &wf1[6], DICO3_SIZE);

    /* Compute quantized LSFs and update the past quantized residual */

    for (i = 0; i < M; i++) {
        lsf1_q[i] = add(lsf_r1[i], lsf_p[i]);
        past_rq[i] = lsf_r1[i];
    }

    /* verification that LSFs has mimimum distance of LSF_GAP Hz */

    Reorder_lsf(lsf1_q, LSF_GAP, M);

    /*  convert LSFs to the cosine domain */

    Lsf_lsp(lsf1_q, lsp1_q, M);

    return;
}


/* Quantization of a 3 dimensional subvector */

Word16
Vq_subvec3(             /* (o)  : return quantization index     */
    Word16 * lsf_r1,    /* (i)  : 1st LSF residual vector       */
    Word16 * dico,      /* (i)  : quantization codebook         */
    Word16 * wf1,       /* (i)  : 1st LSF weighting factors     */
    Word16 dico_size)   /* (i)  : size of quantization codebook */
{
    Word16 i, index;
    Word16 *p_dico, temp;
    Word32 dist_min, dist;

    dist_min = MAX_32;
    p_dico = dico;

    for (i = 0; i < dico_size; i++) {
        temp = sub(lsf_r1[0], *p_dico++);
        temp = mult(wf1[0], temp);
        dist = L_mult(temp, temp);

        temp = sub(lsf_r1[1], *p_dico++);
        temp = mult(wf1[1], temp);
        dist = L_mac(dist, temp, temp);

        temp = sub(lsf_r1[2], *p_dico++);
        temp = mult(wf1[2], temp);
        dist = L_mac(dist, temp, temp);

        if (L_sub(dist, dist_min) < (Word32) 0) {
            dist_min = dist;
            index = i;
        }
    }

    /* Reading the selected vector */

    p_dico = &dico[add(index, add(index, index))];
    lsf_r1[0] = *p_dico++;
    lsf_r1[1] = *p_dico++;
    lsf_r1[2] = *p_dico++;

    return index;

}


/* Quantization of a 4 dimensional subvector */

Word16
Vq_subvec4(             /* (o)  : return quantization index     */
    Word16 * lsf_r1,    /* (i)  : 1st LSF residual vector       */
    Word16 * dico,      /* (i)  : quantization codebook         */
    Word16 * wf1,       /* (i)  : 1st LSF weighting factors     */
    Word16 dico_size)   /* (i)  : size of quantization codebook */
{
    Word16 i, index;
    Word16 *p_dico, temp;
    Word32 dist_min, dist;

    dist_min = MAX_32;
    p_dico = dico;

    for (i = 0; i < dico_size; i++) {
        temp = sub(lsf_r1[0], *p_dico++);
        temp = mult(wf1[0], temp);
        dist = L_mult(temp, temp);

        temp = sub(lsf_r1[1], *p_dico++);
        temp = mult(wf1[1], temp);
        dist = L_mac(dist, temp, temp);

        temp = sub(lsf_r1[2], *p_dico++);
        temp = mult(wf1[2], temp);
        dist = L_mac(dist, temp, temp);

        temp = sub(lsf_r1[3], *p_dico++);
        temp = mult(wf1[3], temp);
        dist = L_mac(dist, temp, temp);

        if (L_sub(dist, dist_min) < (Word32) 0) {
            dist_min = dist;
            index = i;
        }
    }

    /* Reading the selected vector */

    p_dico = &dico[shl(index, 2)];
    lsf_r1[0] = *p_dico++;
    lsf_r1[1] = *p_dico++;
    lsf_r1[2] = *p_dico++;
    lsf_r1[3] = *p_dico++;

    return index;

}

/***********************************************************************
 *
 * FUNCTION  Lsf_wt
 *
 * Compute LSF weighting factors
 *
 *  d[i] = lsf[i+1] - lsf[i-1]
 *
 *  The weighting factors are approximated by two line segment
 *
 *  First segment passes by the following 2 points:
 *
 *     d[i] = 0hz     wf[i] = 3.347
 *     d[i] = 450hz   wf[i] = 1.8
 *
 *  Second segment passes by the following 2 points:
 *
 *     d[i] = 450hz   wf[i] = 1.8
 *     d[i] = 1500hz  wf[i] = 1.0
 *
 *  if( d[i] < 450hz )
 *    wf[i] = 3.347 - ( (3.347-1.8) / (450-0)) *  d[i]
 *  else
 *    wf[i] = 1.8 - ( (1.8-1.0) / (1500-450)) *  (d[i] - 450)
 *
 *
 *  if( d[i] < 1843)
 *    wf[i] = 3427 - (28160*d[i])>>15
 *  else
 *    wf[i] = 1843 - (6242*(d[i]-1843))>>15
 *
 ***********************************************************************/

void Lsf_wt(
    Word16 * lsf,   /* (i)  : LSF vector                  */
    Word16 * wf)    /* (o)  : square of weighting factors */
{
    Word16 temp;
    Word16 i;

    wf[0] = lsf[1];               /* wf[0] = lsf[1] - 0  */
    for (i = 1; i < 9; i++) {
        wf[i] = sub(lsf[i + 1], lsf[i - 1]);
    }
    wf[9] = sub(16384, lsf[8]);   /* wf[9] = 0.5 - lsf[8] */

    for (i = 0; i < 10; i++) {
        temp = sub(wf[i], 1843);
        if (temp < 0) {
            wf[i] = sub(3427, mult(wf[i], 28160));
        } else {
            wf[i] = sub(1843, mult(temp, 6242));
        }

        wf[i] = shl(wf[i], 3);
    }

    return;
}
