--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/libtwamr/q_plsf_3.c	Mon May 06 04:00:14 2024 +0000
@@ -0,0 +1,312 @@
+/*
+*****************************************************************************
+*
+*      GSM AMR-NB speech codec   R98   Version 7.6.0   December 12, 2001
+*                                R99   Version 3.3.0                
+*                                REL-4 Version 4.1.0                
+*
+****************************************************************************
+*
+*      File             : q_plsf_3.c
+*      Purpose          : Quantization of LSF parameters with 1st order MA
+*                         prediction and split by 3 vector quantization
+*                         (split-VQ)
+*
+*****************************************************************************
+*/
+
+/*
+*****************************************************************************
+*                         MODULE INCLUDE FILE AND VERSION ID
+*****************************************************************************
+*/
+#include "namespace.h"
+#include "q_plsf.h"
+ 
+/*
+*****************************************************************************
+*                         INCLUDE FILES
+*****************************************************************************
+*/
+#include "typedef.h"
+#include "basic_op.h"
+#include "no_count.h"
+#include "lsp_lsf.h"
+#include "reorder.h"
+#include "lsfwt.h"
+#include "memops.h"
+#include "q_plsf3_tab.h"
+
+/*
+*****************************************************************************
+*                         LOCAL VARIABLES AND TABLES
+*****************************************************************************
+*/
+#define PAST_RQ_INIT_SIZE 8
+/*
+*****************************************************************************
+*                         LOCAL PROGRAM CODE
+*****************************************************************************
+*/
+/* Quantization of a 4 dimensional subvector */
+
+static Word16
+Vq_subvec4(                   /* o: quantization index,            Q0  */
+    Word16 * lsf_r1,          /* i/o: 1st LSF residual vector,     Q15 */
+    const Word16 * dico,      /* i: quantization codebook,         Q15 */
+    const Word16 * wf1,       /* i: 1st LSF weighting factors,     Q13 */
+    Word16 dico_size)         /* i: size of quantization codebook, Q0  */  
+{
+    Word16 i, index = 0, temp;
+    const Word16 *p_dico;
+    Word32 dist_min, dist;
+
+    dist_min = MAX_32;                                              move32 ();
+    p_dico = dico;                                                  move16 ();
+
+    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);
+
+        test ();
+	if (L_sub (dist, dist_min) < (Word32) 0)
+        {
+            dist_min = dist;                                       move32 ();
+            index = i;                                             move16 ();
+        }
+    }
+
+    /* Reading the selected vector */
+
+    p_dico = &dico[shl (index, 2)];                                move16 ();
+    lsf_r1[0] = *p_dico++;                                         move16 ();
+    lsf_r1[1] = *p_dico++;                                         move16 ();
+    lsf_r1[2] = *p_dico++;                                         move16 ();
+    lsf_r1[3] = *p_dico++;                                         move16 ();
+
+    return index;
+}
+
+/* Quantization of a 3 dimensional subvector */
+
+static Word16
+Vq_subvec3(                   /* o: quantization index,            Q0  */
+    Word16 * lsf_r1,          /* i/o: 1st LSF residual vector,     Q15 */
+    const Word16 * dico,      /* i: quantization codebook,         Q15 */
+    const Word16 * wf1,       /* i: 1st LSF weighting factors,     Q13 */
+    Word16 dico_size,         /* i: size of quantization codebook, Q0  */
+    Flag use_half)            /* i: use every second entry in codebook */
+{
+    Word16 i, index = 0, temp;
+    const Word16 *p_dico;
+    Word32 dist_min, dist;
+
+    dist_min = MAX_32;                                             move32 ();
+    p_dico = dico;                                                 move16 ();
+
+    test ();
+    if (use_half == 0) {
+       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);
+
+          test ();
+          if (L_sub(dist, dist_min) < (Word32) 0) {
+             dist_min = dist;                                   move32 ();
+             index = i;                                         move16 ();
+          }
+       }
+       p_dico = &dico[add(index, add(index, index))];             move16 ();
+    }
+    else
+    {
+       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);
+          
+          test ();
+          if (L_sub(dist, dist_min) < (Word32) 0)
+          {
+             dist_min = dist;                                   move32 ();
+             index = i;                                         move16 ();
+          }
+          p_dico = p_dico + 3; add(0,0);
+       }
+       p_dico = &dico[shl(add(index, add(index, index)),1)];      move16 ();
+    }
+
+    /* Reading the selected vector */
+    lsf_r1[0] = *p_dico++;                                         move16 ();
+    lsf_r1[1] = *p_dico++;                                         move16 ();
+    lsf_r1[2] = *p_dico++;                                         move16 ();
+
+    return index;
+}
+
+/*
+*****************************************************************************
+*                         PUBLIC PROGRAM CODE
+*****************************************************************************
+*/
+
+/***********************************************************************
+ *
+ * routine:   Q_plsf_3()
+ *
+ * Quantization of LSF parameters with 1st order MA prediction and
+ * split by 3 vector quantization (split-VQ)
+ *
+ ***********************************************************************/
+void Q_plsf_3(
+    Q_plsfState *st,    /* i/o: state struct                             */
+    enum Mode mode,     /* i  : coder mode                               */
+    Word16 *lsp1,       /* i  : 1st LSP vector                      Q15  */
+    Word16 *lsp1_q,     /* o  : quantized 1st LSP vector            Q15  */
+    Word16 *indice,     /* o  : quantization indices of 3 vectors   Q0   */
+    Word16 *pred_init_i /* o  : init index for MA prediction in DTX mode */
+)
+{
+    Word16 i, j;
+    Word16 lsf1[M], wf1[M], lsf_p[M], lsf_r1[M];
+    Word16 lsf1_q[M];
+    
+    Word32 L_pred_init_err;
+    Word32 L_min_pred_init_err;
+    Word16 temp_r1[M];
+    Word16 temp_p[M];
+
+    /* convert LSFs to normalize frequency domain 0..16384 */
+
+    Lsp_lsf(lsp1, lsf1, M);
+
+    /* compute LSF weighting factors (Q13) */
+
+    Lsf_wt(lsf1, wf1);
+
+    /* Compute predicted LSF and prediction error */
+    if (test(), sub(mode, MRDTX) != 0)
+    {
+       for (i = 0; i < M; i++)
+       {
+          lsf_p[i] = add(mean_lsf3[i], 
+                         mult(st->past_rq[i], 
+                              pred_fac[i]));                            move16 ();
+          lsf_r1[i] = sub(lsf1[i], lsf_p[i]);                           move16 ();
+      }
+    }
+    else
+    {
+       /* DTX mode, search the init vector that yields */
+       /* lowest prediction resuidual energy           */
+       *pred_init_i = 0;                                                move16 ();
+       L_min_pred_init_err = 0x7fffffff; /* 2^31 - 1 */                 move32 ();
+       for (j = 0; j < PAST_RQ_INIT_SIZE; j++)
+       {
+          L_pred_init_err = 0;                                          move32 ();
+          for (i = 0; i < M; i++)
+          {
+             temp_p[i] = add(mean_lsf3[i], past_rq_init[j*M+i]);
+             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]);
+          }  /* next i */
+
+          test ();
+          if (L_sub(L_pred_init_err, L_min_pred_init_err) < (Word32) 0)
+          {
+             L_min_pred_init_err = L_pred_init_err;                     move32 (); 
+             Copy(temp_r1, lsf_r1, M);
+             Copy(temp_p, lsf_p, M);
+             
+             /* Set zerom */
+             Copy(&past_rq_init[j*M], st->past_rq, M);
+             *pred_init_i = j;                                          move16 ();            
+          } /* endif */
+       } /* next j */
+    } /* endif MRDTX */
+    
+    /*---- Split-VQ of prediction error ----*/
+    if (sub (mode, MR475) == 0 || sub (mode, MR515) == 0)
+    {   /* MR475, MR515 */
+      test (); test (); 
+                          
+      indice[0] = Vq_subvec3(&lsf_r1[0], dico1_lsf3, &wf1[0], DICO31_SIZE, 0);
+      move16 ();
+      indice[1] = Vq_subvec3(&lsf_r1[3], dico2_lsf3, &wf1[3], DICO32_SIZE/2, 1);
+      move16 ();
+      indice[2] = Vq_subvec4(&lsf_r1[6], mr515_3_lsf, &wf1[6], MR515_3_SIZE);
+      move16 ();
+    }
+    else if (sub (mode, MR795) == 0)
+    {   /* MR795 */
+      test (); test (); test ();
+       
+      indice[0] = Vq_subvec3(&lsf_r1[0], mr795_1_lsf, &wf1[0], MR795_1_SIZE, 0);
+      move16 ();
+      indice[1] = Vq_subvec3(&lsf_r1[3], dico2_lsf3, &wf1[3], DICO32_SIZE, 0);
+      move16 ();
+      indice[2] = Vq_subvec4(&lsf_r1[6], dico3_lsf3, &wf1[6], DICO33_SIZE);
+      move16 ();
+    }
+    else 
+    {   /* MR59, MR67, MR74, MR102 , MRDTX */
+      test (); test (); test ();
+
+      indice[0] = Vq_subvec3(&lsf_r1[0], dico1_lsf3, &wf1[0], DICO31_SIZE, 0);
+      move16 ();
+      indice[1] = Vq_subvec3(&lsf_r1[3], dico2_lsf3, &wf1[3], DICO32_SIZE, 0);
+      move16 ();
+      indice[2] = Vq_subvec4(&lsf_r1[6], dico3_lsf3, &wf1[6], DICO33_SIZE);
+      move16 ();
+    }
+
+        
+    /* 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]);                        move16 ();
+        st->past_rq[i] = lsf_r1[i];                                  move16 ();
+    }
+
+    /* 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);
+}