All debug printf's are now within #ifdef DEBUG
[speexdsp.git] / libspeex / quant_lsp.c
1 /* Copyright (C) 2002 Jean-Marc Valin 
2    File: quant_lsp.c
3    LSP vector quantization
4
5    This library is free software; you can redistribute it and/or
6    modify it under the terms of the GNU Lesser General Public
7    License as published by the Free Software Foundation; either
8    version 2.1 of the License, or (at your option) any later version.
9    
10    This library is distributed in the hope that it will be useful,
11    but WITHOUT ANY WARRANTY; without even the implied warranty of
12    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
13    Lesser General Public License for more details.
14    
15    You should have received a copy of the GNU Lesser General Public
16    License along with this library; if not, write to the Free Software
17    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
18 */
19
20 #include "quant_lsp.h"
21 #include <math.h>
22 #include <stdio.h>
23
24 static float quant_weight[MAX_LSP_SIZE];
25
26 /* Note: x is modified*/
27 static int lsp_quant(float *x, float *cdbk, int nbVec, int nbDim)
28 {
29    int i,j;
30    float dist, tmp;
31    float best_dist=0;
32    int best_id=0;
33    float *ptr=cdbk;
34    for (i=0;i<nbVec;i++)
35    {
36       dist=0;
37       for (j=0;j<nbDim;j++)
38       {
39          tmp=(x[j]-*ptr++);
40          dist+=tmp*tmp;
41       }
42       if (dist<best_dist || i==0)
43       {
44          best_dist=dist;
45          best_id=i;
46       }
47    }
48
49    for (j=0;j<nbDim;j++)
50       x[j] -= cdbk[best_id*nbDim+j];
51     
52    return best_id;
53 }
54
55 /* Note: x is modified*/
56 static int lsp_weight_quant(float *x, float *weight, float *cdbk, int nbVec, int nbDim)
57 {
58    int i,j;
59    float dist, tmp;
60    float best_dist=0;
61    int best_id=0;
62    float *ptr=cdbk;
63    for (i=0;i<nbVec;i++)
64    {
65       dist=0;
66       for (j=0;j<nbDim;j++)
67       {
68          tmp=(x[j]-*ptr++);
69          dist+=weight[j]*tmp*tmp;
70       }
71       if (dist<best_dist || i==0)
72       {
73          best_dist=dist;
74          best_id=i;
75       }
76    }
77    
78    for (j=0;j<nbDim;j++)
79       x[j] -= cdbk[best_id*nbDim+j];
80    return best_id;
81 }
82
83
84 void lsp_quant_nb(float *lsp, float *qlsp, int order, FrameBits *bits)
85 {
86    int i;
87    float tmp1, tmp2;
88    int id;
89
90    for (i=0;i<order;i++)
91       qlsp[i]=lsp[i];
92
93    quant_weight[0] = 1/(qlsp[1]-qlsp[0]);
94    quant_weight[order-1] = 1/(qlsp[order-1]-qlsp[order-2]);
95    for (i=1;i<order-1;i++)
96    {
97       tmp1 = 1/(qlsp[i]-qlsp[i-1]);
98       tmp2 = 1/(qlsp[i+1]-qlsp[i]);
99       quant_weight[i] = tmp1 > tmp2 ? tmp1 : tmp2;
100    }
101    id = lsp_quant(qlsp, cdbk_nb, NB_CDBK_SIZE, order);
102    frame_bits_pack(bits, id, 6);
103
104    id = lsp_weight_quant(qlsp, quant_weight, cdbk_nb_low1, NB_CDBK_SIZE_LOW1, 5);
105    frame_bits_pack(bits, id, 6);
106
107    id = lsp_weight_quant(qlsp, quant_weight, cdbk_nb_low2, NB_CDBK_SIZE_LOW2, 5);
108    frame_bits_pack(bits, id, 6);
109
110    id = lsp_weight_quant(qlsp+5, quant_weight+5, cdbk_nb_high1, NB_CDBK_SIZE_HIGH1, 5);
111    frame_bits_pack(bits, id, 6);
112
113    id = lsp_weight_quant(qlsp+5, quant_weight+5, cdbk_nb_high2, NB_CDBK_SIZE_HIGH2, 5);
114    frame_bits_pack(bits, id, 6);
115
116    for (i=0;i<order;i++)
117       qlsp[i]=lsp[i]-qlsp[i];
118 }
119
120 void lsp_unquant_nb(float *lsp, int order, FrameBits *bits)
121 {
122    int i, id;
123    for (i=0;i<order;i++)
124       lsp[i]=0;
125
126
127    id=frame_bits_unpack_unsigned(bits, 6);
128    for (i=0;i<10;i++)
129       lsp[i] += cdbk_nb[id*10+i];
130
131    id=frame_bits_unpack_unsigned(bits, 6);
132    for (i=0;i<5;i++)
133       lsp[i] += cdbk_nb_low1[id*5+i];
134
135    id=frame_bits_unpack_unsigned(bits, 6);
136    for (i=0;i<5;i++)
137       lsp[i] += cdbk_nb_low2[id*5+i];
138
139    id=frame_bits_unpack_unsigned(bits, 6);
140    for (i=0;i<5;i++)
141       lsp[i+5] += cdbk_nb_high1[id*5+i];
142    
143    id=frame_bits_unpack_unsigned(bits, 6);
144    for (i=0;i<5;i++)
145       lsp[i+5] += cdbk_nb_high2[id*5+i];
146 }
147
148
149 extern float lsp_cdbk_wb[];
150 extern float lsp_cdbk_wb11[];
151 extern float lsp_cdbk_wb12[];
152 extern float lsp_cdbk_wb21[];
153 extern float lsp_cdbk_wb22[];
154 extern float lsp_cdbk_wb31[];
155 extern float lsp_cdbk_wb32[];
156 extern float lsp_cdbk_wb41[];
157 extern float lsp_cdbk_wb42[];
158
159 void lsp_quant_wb(float *lsp, float *qlsp, int order, FrameBits *bits)
160 {
161    int i;
162    float tmp1, tmp2;
163    int id;
164    for (i=0;i<order;i++)
165       qlsp[i]=lsp[i];
166
167    quant_weight[0] = 1/(qlsp[1]-qlsp[0]);
168    quant_weight[order-1] = 1/(qlsp[order-1]-qlsp[order-2]);
169    for (i=1;i<order-1;i++)
170    {
171       tmp1 = 1/(qlsp[i]-qlsp[i-1]);
172       tmp2 = 1/(qlsp[i+1]-qlsp[i]);
173       quant_weight[i] = tmp1 > tmp2 ? tmp1 : tmp2;
174    }
175    id = lsp_quant(qlsp, lsp_cdbk_wb, 64, order);
176    frame_bits_pack(bits, id, 6);
177
178    id = lsp_weight_quant(qlsp, quant_weight, lsp_cdbk_wb11, 64, 4);
179    frame_bits_pack(bits, id, 6);
180
181    id = lsp_weight_quant(qlsp, quant_weight, lsp_cdbk_wb12, 64, 4);
182    frame_bits_pack(bits, id, 6);
183
184    id = lsp_weight_quant(qlsp+4, quant_weight, lsp_cdbk_wb21, 64, 4);
185    frame_bits_pack(bits, id, 6);
186
187    id = lsp_weight_quant(qlsp+4, quant_weight, lsp_cdbk_wb22, 64, 4);
188    frame_bits_pack(bits, id, 6);
189
190    id = lsp_weight_quant(qlsp+8, quant_weight, lsp_cdbk_wb31, 64, 4);
191    frame_bits_pack(bits, id, 6);
192
193    id = lsp_weight_quant(qlsp+8, quant_weight, lsp_cdbk_wb32, 16, 4);
194    frame_bits_pack(bits, id, 4);
195
196    id = lsp_weight_quant(qlsp+12, quant_weight, lsp_cdbk_wb41, 64, 4);
197    frame_bits_pack(bits, id, 6);
198
199    id = lsp_weight_quant(qlsp+12, quant_weight, lsp_cdbk_wb42, 16, 4);
200    frame_bits_pack(bits, id, 4);
201
202    for (i=0;i<order;i++)
203       qlsp[i]=lsp[i]-qlsp[i];
204 }
205
206
207 void lsp_unquant_wb(float *lsp, int order, FrameBits *bits)
208 {
209
210    int i, id;
211    for (i=0;i<order;i++)
212       lsp[i]=0;
213
214
215    id=frame_bits_unpack_unsigned(bits, 6);
216    for (i=0;i<16;i++)
217       lsp[i] += lsp_cdbk_wb[id*16+i];
218
219
220    id=frame_bits_unpack_unsigned(bits, 6);
221    for (i=0;i<4;i++)
222       lsp[i] += lsp_cdbk_wb11[id*4+i];
223
224    id=frame_bits_unpack_unsigned(bits, 6);
225    for (i=0;i<4;i++)
226       lsp[i] += lsp_cdbk_wb12[id*4+i];
227
228
229    id=frame_bits_unpack_unsigned(bits, 6);
230    for (i=0;i<4;i++)
231       lsp[i+4] += lsp_cdbk_wb21[id*4+i];
232
233    id=frame_bits_unpack_unsigned(bits, 6);
234    for (i=0;i<4;i++)
235       lsp[i+4] += lsp_cdbk_wb22[id*4+i];
236
237
238    id=frame_bits_unpack_unsigned(bits, 6);
239    for (i=0;i<4;i++)
240       lsp[i+8] += lsp_cdbk_wb31[id*4+i];
241
242    id=frame_bits_unpack_unsigned(bits, 4);
243    for (i=0;i<4;i++)
244       lsp[i+8] += lsp_cdbk_wb32[id*4+i];
245
246
247    id=frame_bits_unpack_unsigned(bits, 6);
248    for (i=0;i<4;i++)
249       lsp[i+12] += lsp_cdbk_wb41[id*4+i];
250
251    id=frame_bits_unpack_unsigned(bits, 4);
252    for (i=0;i<4;i++)
253       lsp[i+12] += lsp_cdbk_wb42[id*4+i];
254
255 }