The code is getting horribly messy, but there's too much stuff that needs
[speexdsp.git] / libspeex / cb_search.c
1 /*-----------------------------------------------------------------------*\
2
3     FILE........: GAINSHAPE.C
4     TYPE........: C Module
5     AUTHOR......: David Rowe
6     COMPANY.....: Voicetronix
7     DATE CREATED: 19/2/02
8
9     General gain-shape codebook search.
10
11 \*-----------------------------------------------------------------------*/
12
13 /* Modified by Jean-Marc Valin 2002
14
15    This library is free software; you can redistribute it and/or
16    modify it under the terms of the GNU Lesser General Public
17    License as published by the Free Software Foundation; either
18    version 2.1 of the License, or (at your option) any later version.
19    
20    This library is distributed in the hope that it will be useful,
21    but WITHOUT ANY WARRANTY; without even the implied warranty of
22    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
23    Lesser General Public License for more details.
24    
25    You should have received a copy of the GNU Lesser General Public
26    License along with this library; if not, write to the Free Software
27    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
28 */
29
30
31
32 #include <stdlib.h>
33 #include <cb_search.h>
34 #include "filters.h"
35
36 #define min(a,b) ((a) < (b) ? (a) : (b))
37
38 /*---------------------------------------------------------------------------*\
39                                                                              
40  void overlap_cb_search()                                                             
41                                                                               
42  Searches a gain/shape codebook consisting of overlapping entries for the    
43  closest vector to the target.  Gives identical results to search() above   
44  buts uses fast end correction algorithm for the synthesis of response       
45  vectors.                                                                     
46                                                                              
47 \*---------------------------------------------------------------------------*/
48
49 float overlap_cb_search(
50 float target[],                 /* target vector */
51 float ak[],                     /* LPCs for this subframe */
52 float awk1[],                   /* Weighted LPCs for this subframe */
53 float awk2[],                   /* Weighted LPCs for this subframe */
54 float codebook[],               /* overlapping codebook */
55 int   entries,                  /* number of overlapping entries to search */
56 float *gain,                    /* gain of optimum entry */
57 int   *index,                   /* index of optimum entry */
58 int   p,                        /* number of LPC coeffs */
59 int   nsf                       /* number of samples in subframe */
60 )
61 {
62   float *resp;                  /* zero state response to current entry */
63   float *h;                     /* impulse response of synthesis filter */
64   float *impulse;               /* excitation vector containing one impulse */
65   float d,e,g,score;            /* codebook searching variables */
66   float bscore;                 /* score of "best" vector so far */
67   int i,k;                      /* loop variables */
68
69   /* Initialise */
70   
71   resp = (float*)malloc(sizeof(float)*nsf);
72   h = (float*)malloc(sizeof(float)*nsf);
73   impulse = (float*)malloc(sizeof(float)*nsf);
74
75   for(i=0; i<nsf; i++)
76     impulse[i] = 0.0;
77    
78   *gain = 0.0;
79   *index = 0;
80   bscore = 0.0;
81   impulse[0] = 1.0;
82
83   /* Calculate impulse response of  A(z/g2) / ( A(z)*(z/g1) ) */
84   residue_zero(impulse, awk1, h, nsf, p);
85   syn_filt_zero(h, ak, h, nsf, p);
86   syn_filt_zero(h, awk2, h, nsf,p);
87   
88   /* Calculate codebook zero-response */
89   residue_zero(&codebook[entries-1],awk1,resp,nsf,p);
90   syn_filt_zero(resp,ak,resp,nsf,p);
91   syn_filt_zero(resp,awk2,resp,nsf,p);
92     
93   /* Search codebook backwards using end correction for synthesis */
94   
95   for(k=entries-1; k>=0; k--) {
96
97     d = 0.0; e = 0.0;
98     for(i=0; i<nsf; i++) {
99       d += target[i]*resp[i];
100       e += resp[i]*resp[i];
101     }
102     g = d/(e+.1);
103     score = g*d;
104     /*printf ("score: %f %f %f %f\n", target[0],d,e,score);*/
105     if (score >= bscore) {
106       bscore = score;
107       *gain = g;
108       *index = k;
109     }
110     
111     /* Synthesise next entry */
112     
113     if (k) {
114       for(i=nsf-1; i>=1; i--)
115         resp[i] = resp[i-1] + codebook[k-1]*h[i];
116       resp[0] = codebook[k-1]*h[0];
117     }
118   }
119
120   free(resp);
121   free(h);
122   free(impulse);
123   return bscore;
124 }
125
126
127
128
129
130 float split_cb_search(
131 float target[],                 /* target vector */
132 float ak[],                     /* LPCs for this subframe */
133 float awk1[],                   /* Weighted LPCs for this subframe */
134 float awk2[],                   /* Weighted LPCs for this subframe */
135 float codebook[][8],            /* overlapping codebook */
136 int   entries,                  /* number of entries to search */
137 float *gain,                    /* gain of optimum entries */
138 int   *index,                   /* index of optimum entries */
139 int   p,                        /* number of LPC coeffs */
140 int   nsf,                      /* number of samples in subframe */
141 float *exc
142 )
143 {
144    int i,j;
145    float resp[64][8], E[64];
146    float t[40], r[40], e[40];
147    for (i=0;i<40;i++)
148       t[i]=target[i];
149    for (i=0;i<64;i++)
150    {
151       residue_zero(codebook[i], awk1, resp[i], 8, p);
152       syn_filt_zero(resp[i], ak, resp[i], 8, p);
153       syn_filt_zero(resp[i], awk2, resp[i], 8,p);
154       E[i]=0;
155       for(j=0;j<8;j++)
156          E[i]+=resp[i][j]*resp[i][j];
157    }
158    for (i=0;i<5;i++)
159    {
160       int best_index;
161       float corr, best_gain, score, best_score=-1;
162       for (j=0;j<64;j++)
163       {
164          corr=xcorr(resp[j],t+8*i,8);
165          score=corr*corr/(.001+E[j]);
166          if (score>best_score)
167          {
168             best_index=j;
169             best_score=score;
170             best_gain=corr/(.001+E[j]);
171          }
172       }
173       printf ("search: %d %f %f\n", best_index, best_gain, best_score);
174       for (j=0;j<40;j++)
175          e[j]=0;
176       for (j=0;j<8;j++)
177          e[8*i+j]=best_gain*codebook[best_index][j];
178       residue_zero(e, awk1, r, 40, p);
179       syn_filt_zero(r, ak, r, 40, p);
180       syn_filt_zero(r, awk2, r, 40,p);
181       for (j=0;j<40;j++)
182          t[j]-=r[j];
183
184       for (j=0;j<40;j++)
185          exc[j]+=e[j];
186    }
187
188
189    for (i=0;i<40;i++)
190       target[i]=t[i];
191 }
192