wideband coexists with narrowband now
[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    printf ("qid = %d\n", id);
103    frame_bits_pack(bits, id, 6);
104
105    id = lsp_weight_quant(qlsp, quant_weight, cdbk_nb_low1, NB_CDBK_SIZE_LOW1, 5);
106    printf ("qid = %d\n", id);
107    frame_bits_pack(bits, id, 6);
108
109    id = lsp_weight_quant(qlsp, quant_weight, cdbk_nb_low2, NB_CDBK_SIZE_LOW2, 5);
110    printf ("qid = %d\n", id);
111    frame_bits_pack(bits, id, 6);
112
113    id = lsp_weight_quant(qlsp+5, quant_weight+5, cdbk_nb_high1, NB_CDBK_SIZE_HIGH1, 5);
114    printf ("qid = %d\n", id);
115    frame_bits_pack(bits, id, 6);
116
117    id = lsp_weight_quant(qlsp+5, quant_weight+5, cdbk_nb_high2, NB_CDBK_SIZE_HIGH2, 5);
118    printf ("qid = %d\n", id);
119    frame_bits_pack(bits, id, 6);
120
121    for (i=0;i<order;i++)
122       qlsp[i]=lsp[i]-qlsp[i];
123 }
124
125 void lsp_unquant_nb(float *lsp, int order, FrameBits *bits)
126 {
127    int i, id;
128    for (i=0;i<order;i++)
129       lsp[i]=0;
130
131
132    id=frame_bits_unpack_unsigned(bits, 6);
133    printf ("did = %d\n", id);
134    for (i=0;i<10;i++)
135       lsp[i] += cdbk_nb[id*10+i];
136
137    id=frame_bits_unpack_unsigned(bits, 6);
138    printf ("did = %d\n", id);
139    for (i=0;i<5;i++)
140       lsp[i] += cdbk_nb_low1[id*5+i];
141
142    id=frame_bits_unpack_unsigned(bits, 6);
143    printf ("did = %d\n", id);
144    for (i=0;i<5;i++)
145       lsp[i] += cdbk_nb_low2[id*5+i];
146
147    id=frame_bits_unpack_unsigned(bits, 6);
148    printf ("did = %d\n", id);
149    for (i=0;i<5;i++)
150       lsp[i+5] += cdbk_nb_high1[id*5+i];
151    
152    id=frame_bits_unpack_unsigned(bits, 6);
153    printf ("did = %d\n", id);
154    for (i=0;i<5;i++)
155       lsp[i+5] += cdbk_nb_high2[id*5+i];
156 }
157
158
159 extern float lsp_cdbk_wb[];
160 extern float lsp_cdbk_wb11[];
161 extern float lsp_cdbk_wb12[];
162 extern float lsp_cdbk_wb21[];
163 extern float lsp_cdbk_wb22[];
164 extern float lsp_cdbk_wb31[];
165 extern float lsp_cdbk_wb32[];
166 extern float lsp_cdbk_wb41[];
167 extern float lsp_cdbk_wb42[];
168
169 void lsp_quant_wb(float *lsp, float *qlsp, int order, FrameBits *bits)
170 {
171    int i;
172    float tmp1, tmp2;
173    int id;
174    for (i=0;i<order;i++)
175       qlsp[i]=lsp[i];
176
177    quant_weight[0] = 1/(qlsp[1]-qlsp[0]);
178    quant_weight[order-1] = 1/(qlsp[order-1]-qlsp[order-2]);
179    for (i=1;i<order-1;i++)
180    {
181       tmp1 = 1/(qlsp[i]-qlsp[i-1]);
182       tmp2 = 1/(qlsp[i+1]-qlsp[i]);
183       quant_weight[i] = tmp1 > tmp2 ? tmp1 : tmp2;
184    }
185    id = lsp_quant(qlsp, lsp_cdbk_wb, 64, order);
186    frame_bits_pack(bits, id, 6);
187
188    id = lsp_weight_quant(qlsp, quant_weight, lsp_cdbk_wb11, 64, 4);
189    frame_bits_pack(bits, id, 6);
190
191    id = lsp_weight_quant(qlsp, quant_weight, lsp_cdbk_wb12, 64, 4);
192    frame_bits_pack(bits, id, 6);
193
194    id = lsp_weight_quant(qlsp+4, quant_weight, lsp_cdbk_wb21, 64, 4);
195    frame_bits_pack(bits, id, 6);
196
197    id = lsp_weight_quant(qlsp+4, quant_weight, lsp_cdbk_wb22, 64, 4);
198    frame_bits_pack(bits, id, 6);
199
200    id = lsp_weight_quant(qlsp+8, quant_weight, lsp_cdbk_wb31, 64, 4);
201    frame_bits_pack(bits, id, 6);
202
203    id = lsp_weight_quant(qlsp+8, quant_weight, lsp_cdbk_wb32, 16, 4);
204    frame_bits_pack(bits, id, 4);
205
206    id = lsp_weight_quant(qlsp+12, quant_weight, lsp_cdbk_wb41, 64, 4);
207    frame_bits_pack(bits, id, 6);
208
209    id = lsp_weight_quant(qlsp+12, quant_weight, lsp_cdbk_wb42, 16, 4);
210    frame_bits_pack(bits, id, 4);
211
212    for (i=0;i<order;i++)
213       qlsp[i]=lsp[i]-qlsp[i];
214 }
215
216
217 void lsp_unquant_wb(float *lsp, int order, FrameBits *bits)
218 {
219
220    int i, id;
221    for (i=0;i<order;i++)
222       lsp[i]=0;
223
224
225    id=frame_bits_unpack_unsigned(bits, 6);
226    for (i=0;i<16;i++)
227       lsp[i] += lsp_cdbk_wb[id*16+i];
228
229
230    id=frame_bits_unpack_unsigned(bits, 6);
231    for (i=0;i<4;i++)
232       lsp[i] += lsp_cdbk_wb11[id*4+i];
233
234    id=frame_bits_unpack_unsigned(bits, 6);
235    for (i=0;i<4;i++)
236       lsp[i] += lsp_cdbk_wb12[id*4+i];
237
238
239    id=frame_bits_unpack_unsigned(bits, 6);
240    for (i=0;i<4;i++)
241       lsp[i+4] += lsp_cdbk_wb21[id*4+i];
242
243    id=frame_bits_unpack_unsigned(bits, 6);
244    for (i=0;i<4;i++)
245       lsp[i+4] += lsp_cdbk_wb22[id*4+i];
246
247
248    id=frame_bits_unpack_unsigned(bits, 6);
249    for (i=0;i<4;i++)
250       lsp[i+8] += lsp_cdbk_wb31[id*4+i];
251
252    id=frame_bits_unpack_unsigned(bits, 4);
253    for (i=0;i<4;i++)
254       lsp[i+8] += lsp_cdbk_wb32[id*4+i];
255
256
257    id=frame_bits_unpack_unsigned(bits, 6);
258    for (i=0;i<4;i++)
259       lsp[i+12] += lsp_cdbk_wb41[id*4+i];
260
261    id=frame_bits_unpack_unsigned(bits, 4);
262    for (i=0;i<4;i++)
263       lsp[i+12] += lsp_cdbk_wb42[id*4+i];
264
265 }