New preecho detection/short block trigger code, replacing an IIR
[platform/upstream/libvorbis.git] / lib / psy.c
1 /********************************************************************
2  *                                                                  *
3  * THIS FILE IS PART OF THE OggVorbis SOFTWARE CODEC SOURCE CODE.   *
4  * USE, DISTRIBUTION AND REPRODUCTION OF THIS LIBRARY SOURCE IS     *
5  * GOVERNED BY A BSD-STYLE SOURCE LICENSE INCLUDED WITH THIS SOURCE *
6  * IN 'COPYING'. PLEASE READ THESE TERMS BEFORE DISTRIBUTING.       *
7  *                                                                  *
8  * THE OggVorbis SOURCE CODE IS (C) COPYRIGHT 1994-2001             *
9  * by the XIPHOPHORUS Company http://www.xiph.org/                  *
10  *                                                                  *
11  ********************************************************************
12
13  function: psychoacoustics not including preecho
14  last mod: $Id: psy.c,v 1.65 2002/03/17 19:50:47 xiphmont Exp $
15
16  ********************************************************************/
17
18 #include <stdlib.h>
19 #include <math.h>
20 #include <string.h>
21 #include "vorbis/codec.h"
22 #include "codec_internal.h"
23
24 #include "masking.h"
25 #include "psy.h"
26 #include "os.h"
27 #include "lpc.h"
28 #include "smallft.h"
29 #include "scales.h"
30 #include "misc.h"
31
32 #define NEGINF -9999.f
33
34 /* Why Bark scale for encoding but not masking computation? Because
35    masking has a strong harmonic dependency */
36
37 vorbis_look_psy_global *_vp_global_look(vorbis_info *vi){
38   codec_setup_info *ci=vi->codec_setup;
39   vorbis_info_psy_global *gi=&ci->psy_g_param;
40   vorbis_look_psy_global *look=_ogg_calloc(1,sizeof(*look));
41
42   look->channels=vi->channels;
43
44   look->ampmax=-9999.;
45   look->gi=gi;
46   return(look);
47 }
48
49 void _vp_global_free(vorbis_look_psy_global *look){
50   if(look){
51     memset(look,0,sizeof(*look));
52     _ogg_free(look);
53   }
54 }
55
56 void _vi_gpsy_free(vorbis_info_psy_global *i){
57   if(i){
58     memset(i,0,sizeof(*i));
59     _ogg_free(i);
60   }
61 }
62
63 void _vi_psy_free(vorbis_info_psy *i){
64   if(i){
65     memset(i,0,sizeof(*i));
66     _ogg_free(i);
67   }
68 }
69
70 vorbis_info_psy *_vi_psy_copy(vorbis_info_psy *i){
71   vorbis_info_psy *ret=_ogg_malloc(sizeof(*ret));
72   memcpy(ret,i,sizeof(*ret));
73   return(ret);
74 }
75
76 /* Set up decibel threshold slopes on a Bark frequency scale */
77 /* ATH is the only bit left on a Bark scale.  No reason to change it
78    right now */
79 static void set_curve(float *ref,float *c,int n, float crate){
80   int i,j=0;
81
82   for(i=0;i<MAX_BARK-1;i++){
83     int endpos=rint(fromBARK((float)(i+1))*2*n/crate);
84     float base=ref[i];
85     if(j<endpos){
86       float delta=(ref[i+1]-base)/(endpos-j);
87       for(;j<endpos && j<n;j++){
88         c[j]=base;
89         base+=delta;
90       }
91     }
92   }
93 }
94
95 static void min_curve(float *c,
96                        float *c2){
97   int i;  
98   for(i=0;i<EHMER_MAX;i++)if(c2[i]<c[i])c[i]=c2[i];
99 }
100 static void max_curve(float *c,
101                        float *c2){
102   int i;  
103   for(i=0;i<EHMER_MAX;i++)if(c2[i]>c[i])c[i]=c2[i];
104 }
105
106 static void attenuate_curve(float *c,float att){
107   int i;
108   for(i=0;i<EHMER_MAX;i++)
109     c[i]+=att;
110 }
111
112 static void interp_curve(float *c,float *c1,float *c2,float del){
113   int i;
114   for(i=0;i<EHMER_MAX;i++)
115     c[i]=c2[i]*del+c1[i]*(1.f-del);
116 }
117
118 extern int analysis_noisy;
119 static void setup_curve(float **c,
120                         int band,
121                         float *curveatt_dB){
122   int i,j;
123   float ath[EHMER_MAX];
124   float tempc[P_LEVELS][EHMER_MAX];
125   float *ATH=ATH_Bark_dB_lspconservative; /* just for limiting here */
126
127   memcpy(c[0]+2,c[4]+2,sizeof(*c[0])*EHMER_MAX);
128   memcpy(c[2]+2,c[4]+2,sizeof(*c[2])*EHMER_MAX);
129
130   /* we add back in the ATH to avoid low level curves falling off to
131      -infinity and unnecessarily cutting off high level curves in the
132      curve limiting (last step).  But again, remember... a half-band's
133      settings must be valid over the whole band, and it's better to
134      mask too little than too much, so be pessimistical. */
135
136   for(i=0;i<EHMER_MAX;i++){
137     float oc_min=band*.5+(i-EHMER_OFFSET)*.125;
138     float oc_max=band*.5+(i-EHMER_OFFSET+1)*.125;
139     float bark=toBARK(fromOC(oc_min));
140     int ibark=floor(bark);
141     float del=bark-ibark;
142     float ath_min,ath_max;
143
144     if(ibark<26)
145       ath_min=ATH[ibark]*(1.f-del)+ATH[ibark+1]*del;
146     else
147       ath_min=ATH[25];
148
149     bark=toBARK(fromOC(oc_max));
150     ibark=floor(bark);
151     del=bark-ibark;
152
153     if(ibark<26)
154       ath_max=ATH[ibark]*(1.f-del)+ATH[ibark+1]*del;
155     else
156       ath_max=ATH[25];
157
158     ath[i]=min(ath_min,ath_max);
159   }
160
161   /* The c array comes in as dB curves at 20 40 60 80 100 dB.
162      interpolate intermediate dB curves */
163   for(i=1;i<P_LEVELS;i+=2){
164     interp_curve(c[i]+2,c[i-1]+2,c[i+1]+2,.5);
165   }
166
167   /* normalize curves so the driving amplitude is 0dB */
168   /* make temp curves with the ATH overlayed */
169   for(i=0;i<P_LEVELS;i++){
170     attenuate_curve(c[i]+2,curveatt_dB[i]);
171     memcpy(tempc[i],ath,EHMER_MAX*sizeof(*tempc[i]));
172     attenuate_curve(tempc[i],-i*10.f);
173     max_curve(tempc[i],c[i]+2);
174   }
175
176   /* Now limit the louder curves.
177
178      the idea is this: We don't know what the playback attenuation
179      will be; 0dB SL moves every time the user twiddles the volume
180      knob. So that means we have to use a single 'most pessimal' curve
181      for all masking amplitudes, right?  Wrong.  The *loudest* sound
182      can be in (we assume) a range of ...+100dB] SL.  However, sounds
183      20dB down will be in a range ...+80], 40dB down is from ...+60],
184      etc... */
185
186   for(j=1;j<P_LEVELS;j++){
187     min_curve(tempc[j],tempc[j-1]);
188     min_curve(c[j]+2,tempc[j]);
189   }
190
191   /* add fenceposts */
192   for(j=0;j<P_LEVELS;j++){
193
194     for(i=0;i<EHMER_OFFSET;i++)
195       if(c[j][i+2]>-200.f)break;  
196     c[j][0]=i;
197
198     for(i=EHMER_MAX-1;i>EHMER_OFFSET+1;i--)
199       if(c[j][i+2]>-200.f)
200         break;
201     c[j][1]=i;
202
203   }
204 }
205
206 void _vp_psy_init(vorbis_look_psy *p,vorbis_info_psy *vi,
207                   vorbis_info_psy_global *gi,int n,long rate){
208   long i,j,k,lo=-99,hi=0;
209   long maxoc;
210   memset(p,0,sizeof(*p));
211
212
213   p->eighth_octave_lines=gi->eighth_octave_lines;
214   p->shiftoc=rint(log(gi->eighth_octave_lines*8.f)/log(2.f))-1;
215
216   p->firstoc=toOC(.25f*rate/n)*(1<<(p->shiftoc+1))-gi->eighth_octave_lines;
217   maxoc=toOC((n*.5f-.25f)*rate/n)*(1<<(p->shiftoc+1))+.5f;
218   p->total_octave_lines=maxoc-p->firstoc+1;
219
220   if(vi->ath)
221     p->ath=_ogg_malloc(n*sizeof(*p->ath));
222   p->octave=_ogg_malloc(n*sizeof(*p->octave));
223   p->bark=_ogg_malloc(n*sizeof(*p->bark));
224   p->vi=vi;
225   p->n=n;
226   p->rate=rate;
227
228   /* set up the lookups for a given blocksize and sample rate */
229   if(vi->ath)
230     set_curve(vi->ath, p->ath,n,(float)rate);
231   for(i=0;i<n;i++){
232     float bark=toBARK(rate/(2*n)*i); 
233
234     for(;lo+vi->noisewindowlomin<i && 
235           toBARK(rate/(2*n)*lo)<(bark-vi->noisewindowlo);lo++);
236     
237     for(;hi<n && (hi<i+vi->noisewindowhimin ||
238           toBARK(rate/(2*n)*hi)<(bark+vi->noisewindowhi));hi++);
239     
240     p->bark[i]=(lo<<16)+hi;
241
242   }
243
244   for(i=0;i<n;i++)
245     p->octave[i]=toOC((i*.5f+.25f)*rate/n)*(1<<(p->shiftoc+1))+.5f;
246
247   p->tonecurves=_ogg_malloc(P_BANDS*sizeof(*p->tonecurves));
248   p->noisethresh=_ogg_malloc(n*sizeof(*p->noisethresh));
249   p->noiseoffset=_ogg_malloc(n*sizeof(*p->noiseoffset));
250   for(i=0;i<P_BANDS;i++)
251     p->tonecurves[i]=_ogg_malloc(P_LEVELS*sizeof(*p->tonecurves[i]));
252   
253   for(i=0;i<P_BANDS;i++)
254     for(j=0;j<P_LEVELS;j++)
255       p->tonecurves[i][j]=_ogg_malloc((EHMER_MAX+2)*sizeof(*p->tonecurves[i][j]));
256   
257
258   /* OK, yeah, this was a silly way to do it */
259   memcpy(p->tonecurves[0][4]+2,tone_125_40dB_SL,sizeof(*p->tonecurves[0][4])*EHMER_MAX);
260   memcpy(p->tonecurves[0][6]+2,tone_125_60dB_SL,sizeof(*p->tonecurves[0][6])*EHMER_MAX);
261   memcpy(p->tonecurves[0][8]+2,tone_125_80dB_SL,sizeof(*p->tonecurves[0][8])*EHMER_MAX);
262   memcpy(p->tonecurves[0][10]+2,tone_125_100dB_SL,sizeof(*p->tonecurves[0][10])*EHMER_MAX);
263
264   memcpy(p->tonecurves[2][4]+2,tone_125_40dB_SL,sizeof(*p->tonecurves[2][4])*EHMER_MAX);
265   memcpy(p->tonecurves[2][6]+2,tone_125_60dB_SL,sizeof(*p->tonecurves[2][6])*EHMER_MAX);
266   memcpy(p->tonecurves[2][8]+2,tone_125_80dB_SL,sizeof(*p->tonecurves[2][8])*EHMER_MAX);
267   memcpy(p->tonecurves[2][10]+2,tone_125_100dB_SL,sizeof(*p->tonecurves[2][10])*EHMER_MAX);
268
269   memcpy(p->tonecurves[4][4]+2,tone_250_40dB_SL,sizeof(*p->tonecurves[4][4])*EHMER_MAX);
270   memcpy(p->tonecurves[4][6]+2,tone_250_60dB_SL,sizeof(*p->tonecurves[4][6])*EHMER_MAX);
271   memcpy(p->tonecurves[4][8]+2,tone_250_80dB_SL,sizeof(*p->tonecurves[4][8])*EHMER_MAX);
272   memcpy(p->tonecurves[4][10]+2,tone_250_100dB_SL,sizeof(*p->tonecurves[4][10])*EHMER_MAX);
273
274   memcpy(p->tonecurves[6][4]+2,tone_500_40dB_SL,sizeof(*p->tonecurves[6][4])*EHMER_MAX);
275   memcpy(p->tonecurves[6][6]+2,tone_500_60dB_SL,sizeof(*p->tonecurves[6][6])*EHMER_MAX);
276   memcpy(p->tonecurves[6][8]+2,tone_500_80dB_SL,sizeof(*p->tonecurves[6][8])*EHMER_MAX);
277   memcpy(p->tonecurves[6][10]+2,tone_500_100dB_SL,sizeof(*p->tonecurves[6][10])*EHMER_MAX);
278
279   memcpy(p->tonecurves[8][4]+2,tone_1000_40dB_SL,sizeof(*p->tonecurves[8][4])*EHMER_MAX);
280   memcpy(p->tonecurves[8][6]+2,tone_1000_60dB_SL,sizeof(*p->tonecurves[8][6])*EHMER_MAX);
281   memcpy(p->tonecurves[8][8]+2,tone_1000_80dB_SL,sizeof(*p->tonecurves[8][8])*EHMER_MAX);
282   memcpy(p->tonecurves[8][10]+2,tone_1000_100dB_SL,sizeof(*p->tonecurves[8][10])*EHMER_MAX);
283
284   memcpy(p->tonecurves[10][4]+2,tone_2000_40dB_SL,sizeof(*p->tonecurves[10][4])*EHMER_MAX);
285   memcpy(p->tonecurves[10][6]+2,tone_2000_60dB_SL,sizeof(*p->tonecurves[10][6])*EHMER_MAX);
286   memcpy(p->tonecurves[10][8]+2,tone_2000_80dB_SL,sizeof(*p->tonecurves[10][8])*EHMER_MAX);
287   memcpy(p->tonecurves[10][10]+2,tone_2000_100dB_SL,sizeof(*p->tonecurves[10][10])*EHMER_MAX);
288
289   memcpy(p->tonecurves[12][4]+2,tone_4000_40dB_SL,sizeof(*p->tonecurves[12][4])*EHMER_MAX);
290   memcpy(p->tonecurves[12][6]+2,tone_4000_60dB_SL,sizeof(*p->tonecurves[12][6])*EHMER_MAX);
291   memcpy(p->tonecurves[12][8]+2,tone_4000_80dB_SL,sizeof(*p->tonecurves[12][8])*EHMER_MAX);
292   memcpy(p->tonecurves[12][10]+2,tone_4000_100dB_SL,sizeof(*p->tonecurves[12][10])*EHMER_MAX);
293
294   memcpy(p->tonecurves[14][4]+2,tone_8000_40dB_SL,sizeof(*p->tonecurves[14][4])*EHMER_MAX);
295   memcpy(p->tonecurves[14][6]+2,tone_8000_60dB_SL,sizeof(*p->tonecurves[14][6])*EHMER_MAX);
296   memcpy(p->tonecurves[14][8]+2,tone_8000_80dB_SL,sizeof(*p->tonecurves[14][8])*EHMER_MAX);
297   memcpy(p->tonecurves[14][10]+2,tone_8000_100dB_SL,sizeof(*p->tonecurves[14][10])*EHMER_MAX);
298
299   memcpy(p->tonecurves[16][4]+2,tone_16000_40dB_SL,sizeof(*p->tonecurves[16][4])*EHMER_MAX);
300   memcpy(p->tonecurves[16][6]+2,tone_16000_60dB_SL,sizeof(*p->tonecurves[16][6])*EHMER_MAX);
301   memcpy(p->tonecurves[16][8]+2,tone_16000_80dB_SL,sizeof(*p->tonecurves[16][8])*EHMER_MAX);
302   memcpy(p->tonecurves[16][10]+2,tone_16000_100dB_SL,sizeof(*p->tonecurves[16][10])*EHMER_MAX);
303
304   for(i=0;i<P_BANDS;i+=2)
305     for(j=4;j<P_LEVELS;j+=2)
306       for(k=2;k<EHMER_MAX+2;k++)
307         p->tonecurves[i][j][k]+=vi->tone_masteratt;
308
309   /* interpolate curves between */
310   for(i=1;i<P_BANDS;i+=2)
311     for(j=4;j<P_LEVELS;j+=2){
312       memcpy(p->tonecurves[i][j]+2,p->tonecurves[i-1][j]+2,EHMER_MAX*sizeof(*p->tonecurves[i][j]));
313       /*interp_curve(p->tonecurves[i][j],
314                    p->tonecurves[i-1][j],
315                    p->tonecurves[i+1][j],.5);*/
316       min_curve(p->tonecurves[i][j]+2,p->tonecurves[i+1][j]+2);
317     }
318
319   /* set up the final curves */
320   for(i=0;i<P_BANDS;i++)
321     setup_curve(p->tonecurves[i],i,vi->toneatt.block[i]);
322
323   for(i=0;i<P_LEVELS;i++)
324     _analysis_output("curve_63Hz",i,p->tonecurves[0][i]+2,EHMER_MAX,0,0);
325   for(i=0;i<P_LEVELS;i++)
326     _analysis_output("curve_88Hz",i,p->tonecurves[1][i]+2,EHMER_MAX,0,0);
327   for(i=0;i<P_LEVELS;i++)
328     _analysis_output("curve_125Hz",i,p->tonecurves[2][i]+2,EHMER_MAX,0,0);
329   for(i=0;i<P_LEVELS;i++)
330     _analysis_output("curve_170Hz",i,p->tonecurves[3][i]+2,EHMER_MAX,0,0);
331   for(i=0;i<P_LEVELS;i++)
332     _analysis_output("curve_250Hz",i,p->tonecurves[4][i]+2,EHMER_MAX,0,0);
333   for(i=0;i<P_LEVELS;i++)
334     _analysis_output("curve_350Hz",i,p->tonecurves[5][i]+2,EHMER_MAX,0,0);
335   for(i=0;i<P_LEVELS;i++)
336     _analysis_output("curve_500Hz",i,p->tonecurves[6][i]+2,EHMER_MAX,0,0);
337   for(i=0;i<P_LEVELS;i++)
338     _analysis_output("curve_700Hz",i,p->tonecurves[7][i]+2,EHMER_MAX,0,0);
339   for(i=0;i<P_LEVELS;i++)
340     _analysis_output("curve_1kHz",i,p->tonecurves[8][i]+2,EHMER_MAX,0,0);
341   for(i=0;i<P_LEVELS;i++)
342     _analysis_output("curve_1.4Hz",i,p->tonecurves[9][i]+2,EHMER_MAX,0,0);
343   for(i=0;i<P_LEVELS;i++)
344     _analysis_output("curve_2kHz",i,p->tonecurves[10][i]+2,EHMER_MAX,0,0);
345   for(i=0;i<P_LEVELS;i++)
346     _analysis_output("curve_2.4kHz",i,p->tonecurves[11][i]+2,EHMER_MAX,0,0);
347   for(i=0;i<P_LEVELS;i++)
348      _analysis_output("curve_4kHz",i,p->tonecurves[12][i]+2,EHMER_MAX,0,0);
349   for(i=0;i<P_LEVELS;i++)
350     _analysis_output("curve_5.6kHz",i,p->tonecurves[13][i]+2,EHMER_MAX,0,0);
351   for(i=0;i<P_LEVELS;i++)
352     _analysis_output("curve_8kHz",i,p->tonecurves[14][i]+2,EHMER_MAX,0,0);
353   for(i=0;i<P_LEVELS;i++)
354     _analysis_output("curve_11.5kHz",i,p->tonecurves[15][i]+2,EHMER_MAX,0,0);
355   for(i=0;i<P_LEVELS;i++)
356     _analysis_output("curve_16kHz",i,p->tonecurves[16][i]+2,EHMER_MAX,0,0);
357
358   if(vi->curvelimitp){
359     /* value limit the tonal masking curves; the peakatt not only
360        optionally specifies maximum dynamic depth, but also
361        limits the masking curves to a minimum depth  */
362     for(i=0;i<P_BANDS;i++)
363       for(j=0;j<P_LEVELS;j++){
364         for(k=2;k<EHMER_OFFSET+2+vi->curvelimitp;k++)
365           if(p->tonecurves[i][j][k]> vi->peakatt.block[i][j])
366             p->tonecurves[i][j][k]=  vi->peakatt.block[i][j];
367           else
368             break;
369       }
370   }
371
372   for(i=0;i<P_LEVELS;i++)
373     _analysis_output("licurve_63Hz",i,p->tonecurves[0][i]+2,EHMER_MAX,0,0);
374   for(i=0;i<P_LEVELS;i++)
375     _analysis_output("licurve_88Hz",i,p->tonecurves[1][i]+2,EHMER_MAX,0,0);
376   for(i=0;i<P_LEVELS;i++)
377     _analysis_output("licurve_125Hz",i,p->tonecurves[2][i]+2,EHMER_MAX,0,0);
378   for(i=0;i<P_LEVELS;i++)
379     _analysis_output("licurve_170Hz",i,p->tonecurves[3][i]+2,EHMER_MAX,0,0);
380   for(i=0;i<P_LEVELS;i++)
381     _analysis_output("licurve_250Hz",i,p->tonecurves[4][i]+2,EHMER_MAX,0,0);
382   for(i=0;i<P_LEVELS;i++)
383     _analysis_output("licurve_350Hz",i,p->tonecurves[5][i]+2,EHMER_MAX,0,0);
384   for(i=0;i<P_LEVELS;i++)
385     _analysis_output("licurve_500Hz",i,p->tonecurves[6][i]+2,EHMER_MAX,0,0);
386   for(i=0;i<P_LEVELS;i++)
387     _analysis_output("licurve_700Hz",i,p->tonecurves[7][i]+2,EHMER_MAX,0,0);
388   for(i=0;i<P_LEVELS;i++)
389     _analysis_output("licurve_1kHz",i,p->tonecurves[8][i]+2,EHMER_MAX,0,0);
390   for(i=0;i<P_LEVELS;i++)
391     _analysis_output("licurve_1.4Hz",i,p->tonecurves[9][i]+2,EHMER_MAX,0,0);
392   for(i=0;i<P_LEVELS;i++)
393     _analysis_output("licurve_2kHz",i,p->tonecurves[10][i]+2,EHMER_MAX,0,0);
394   for(i=0;i<P_LEVELS;i++)
395     _analysis_output("licurve_2.4kHz",i,p->tonecurves[11][i]+2,EHMER_MAX,0,0);
396   for(i=0;i<P_LEVELS;i++)
397     _analysis_output("licurve_4kHz",i,p->tonecurves[12][i]+2,EHMER_MAX,0,0);
398   for(i=0;i<P_LEVELS;i++)
399     _analysis_output("licurve_5.6kHz",i,p->tonecurves[13][i]+2,EHMER_MAX,0,0);
400   for(i=0;i<P_LEVELS;i++)
401     _analysis_output("licurve_8kHz",i,p->tonecurves[14][i]+2,EHMER_MAX,0,0);
402   for(i=0;i<P_LEVELS;i++)
403     _analysis_output("licurve_11.5kHz",i,p->tonecurves[15][i]+2,EHMER_MAX,0,0);
404   for(i=0;i<P_LEVELS;i++)
405     _analysis_output("licurve_16kHz",i,p->tonecurves[16][i]+2,EHMER_MAX,0,0);
406
407   if(vi->peakattp) /* we limit maximum depth only optionally */
408     for(i=0;i<P_BANDS;i++)
409       for(j=0;j<P_LEVELS;j++)
410         if(p->tonecurves[i][j][EHMER_OFFSET+2]< vi->peakatt.block[i][j])
411           p->tonecurves[i][j][EHMER_OFFSET+2]=  vi->peakatt.block[i][j];
412
413   for(i=0;i<P_LEVELS;i++)
414     _analysis_output("pcurve_63Hz",i,p->tonecurves[0][i]+2,EHMER_MAX,0,0);
415   for(i=0;i<P_LEVELS;i++)
416     _analysis_output("pcurve_88Hz",i,p->tonecurves[1][i]+2,EHMER_MAX,0,0);
417   for(i=0;i<P_LEVELS;i++)
418     _analysis_output("pcurve_125Hz",i,p->tonecurves[2][i]+2,EHMER_MAX,0,0);
419   for(i=0;i<P_LEVELS;i++)
420     _analysis_output("pcurve_170Hz",i,p->tonecurves[3][i]+2,EHMER_MAX,0,0);
421   for(i=0;i<P_LEVELS;i++)
422     _analysis_output("pcurve_250Hz",i,p->tonecurves[4][i]+2,EHMER_MAX,0,0);
423   for(i=0;i<P_LEVELS;i++)
424     _analysis_output("pcurve_350Hz",i,p->tonecurves[5][i]+2,EHMER_MAX,0,0);
425   for(i=0;i<P_LEVELS;i++)
426     _analysis_output("pcurve_500Hz",i,p->tonecurves[6][i]+2,EHMER_MAX,0,0);
427   for(i=0;i<P_LEVELS;i++)
428     _analysis_output("pcurve_700Hz",i,p->tonecurves[7][i]+2,EHMER_MAX,0,0);
429   for(i=0;i<P_LEVELS;i++)
430     _analysis_output("pcurve_1kHz",i,p->tonecurves[8][i]+2,EHMER_MAX,0,0);
431   for(i=0;i<P_LEVELS;i++)
432     _analysis_output("pcurve_1.4Hz",i,p->tonecurves[9][i]+2,EHMER_MAX,0,0);
433   for(i=0;i<P_LEVELS;i++)
434     _analysis_output("pcurve_2kHz",i,p->tonecurves[10][i]+2,EHMER_MAX,0,0);
435   for(i=0;i<P_LEVELS;i++)
436     _analysis_output("pcurve_2.4kHz",i,p->tonecurves[11][i]+2,EHMER_MAX,0,0);
437   for(i=0;i<P_LEVELS;i++)
438     _analysis_output("pcurve_4kHz",i,p->tonecurves[12][i]+2,EHMER_MAX,0,0);
439   for(i=0;i<P_LEVELS;i++)
440     _analysis_output("pcurve_5.6kHz",i,p->tonecurves[13][i]+2,EHMER_MAX,0,0);
441   for(i=0;i<P_LEVELS;i++)
442     _analysis_output("pcurve_8kHz",i,p->tonecurves[14][i]+2,EHMER_MAX,0,0);
443   for(i=0;i<P_LEVELS;i++)
444     _analysis_output("pcurve_11.5kHz",i,p->tonecurves[15][i]+2,EHMER_MAX,0,0);
445   for(i=0;i<P_LEVELS;i++)
446     _analysis_output("pcurve_16kHz",i,p->tonecurves[16][i]+2,EHMER_MAX,0,0);
447
448   /* but guarding is mandatory */
449   for(i=0;i<P_BANDS;i++)
450     for(j=0;j<P_LEVELS;j++)
451       if(p->tonecurves[i][j][EHMER_OFFSET+2]< vi->tone_guard)
452           p->tonecurves[i][j][EHMER_OFFSET+2]=  vi->tone_guard;
453
454   for(i=0;i<P_LEVELS;i++)
455     _analysis_output("fcurve_63Hz",i,p->tonecurves[0][i]+2,EHMER_MAX,0,0);
456   for(i=0;i<P_LEVELS;i++)
457     _analysis_output("fcurve_88Hz",i,p->tonecurves[1][i]+2,EHMER_MAX,0,0);
458   for(i=0;i<P_LEVELS;i++)
459     _analysis_output("fcurve_125Hz",i,p->tonecurves[2][i]+2,EHMER_MAX,0,0);
460   for(i=0;i<P_LEVELS;i++)
461     _analysis_output("fcurve_170Hz",i,p->tonecurves[3][i]+2,EHMER_MAX,0,0);
462   for(i=0;i<P_LEVELS;i++)
463     _analysis_output("fcurve_250Hz",i,p->tonecurves[4][i]+2,EHMER_MAX,0,0);
464   for(i=0;i<P_LEVELS;i++)
465     _analysis_output("fcurve_350Hz",i,p->tonecurves[5][i]+2,EHMER_MAX,0,0);
466   for(i=0;i<P_LEVELS;i++)
467     _analysis_output("fcurve_500Hz",i,p->tonecurves[6][i]+2,EHMER_MAX,0,0);
468   for(i=0;i<P_LEVELS;i++)
469     _analysis_output("fcurve_700Hz",i,p->tonecurves[7][i]+2,EHMER_MAX,0,0);
470   for(i=0;i<P_LEVELS;i++)
471     _analysis_output("fcurve_1kHz",i,p->tonecurves[8][i]+2,EHMER_MAX,0,0);
472   for(i=0;i<P_LEVELS;i++)
473     _analysis_output("fcurve_1.4Hz",i,p->tonecurves[9][i]+2,EHMER_MAX,0,0);
474   for(i=0;i<P_LEVELS;i++)
475     _analysis_output("fcurve_2kHz",i,p->tonecurves[10][i]+2,EHMER_MAX,0,0);
476   for(i=0;i<P_LEVELS;i++)
477     _analysis_output("fcurve_2.4kHz",i,p->tonecurves[11][i]+2,EHMER_MAX,0,0);
478   for(i=0;i<P_LEVELS;i++)
479     _analysis_output("fcurve_4kHz",i,p->tonecurves[12][i]+2,EHMER_MAX,0,0);
480   for(i=0;i<P_LEVELS;i++)
481     _analysis_output("fcurve_5.6kHz",i,p->tonecurves[13][i]+2,EHMER_MAX,0,0);
482   for(i=0;i<P_LEVELS;i++)
483     _analysis_output("fcurve_8kHz",i,p->tonecurves[14][i]+2,EHMER_MAX,0,0);
484   for(i=0;i<P_LEVELS;i++)
485     _analysis_output("fcurve_11.5kHz",i,p->tonecurves[15][i]+2,EHMER_MAX,0,0);
486   for(i=0;i<P_LEVELS;i++)
487     _analysis_output("fcurve_16kHz",i,p->tonecurves[16][i]+2,EHMER_MAX,0,0);
488
489   /* set up rolling noise median */
490   for(i=0;i<n;i++){
491     float halfoc=toOC((i+.5)*rate/(2.*n))*2.;
492     int inthalfoc;
493     float del;
494     
495     if(halfoc<0)halfoc=0;
496     if(halfoc>=P_BANDS-1)halfoc=P_BANDS-1;
497     inthalfoc=(int)halfoc;
498     del=halfoc-inthalfoc;
499     p->noiseoffset[i]=
500       p->vi->noiseoff[inthalfoc]*(1.-del) + 
501       p->vi->noiseoff[inthalfoc+1]*del;
502   }
503
504   analysis_noisy=1;
505   _analysis_output("noiseoff",0,p->noiseoffset,n,1,0);
506   _analysis_output("noisethresh",0,p->noisethresh,n,1,0);
507   analysis_noisy=1;
508
509 }
510
511 void _vp_psy_clear(vorbis_look_psy *p){
512   int i,j;
513   if(p){
514     if(p->ath)_ogg_free(p->ath);
515     if(p->octave)_ogg_free(p->octave);
516     if(p->bark)_ogg_free(p->bark);
517     if(p->tonecurves){
518       for(i=0;i<P_BANDS;i++){
519         for(j=0;j<P_LEVELS;j++){
520           _ogg_free(p->tonecurves[i][j]);
521         }
522         _ogg_free(p->tonecurves[i]);
523       }
524       _ogg_free(p->tonecurves);
525     }
526     _ogg_free(p->noiseoffset);
527     _ogg_free(p->noisethresh);
528     memset(p,0,sizeof(*p));
529   }
530 }
531
532 /* octave/(8*eighth_octave_lines) x scale and dB y scale */
533 static void seed_curve(float *seed,
534                        const float **curves,
535                        float amp,
536                        int oc, int n,
537                        int linesper,float dBoffset){
538   int i,post1;
539   int seedptr;
540   const float *posts,*curve;
541
542   int choice=(int)((amp+dBoffset)*.1f);
543   choice=max(choice,0);
544   choice=min(choice,P_LEVELS-1);
545   posts=curves[choice];
546   curve=posts+2;
547   post1=(int)posts[1];
548   seedptr=oc+(posts[0]-16)*linesper-(linesper>>1);
549
550   for(i=posts[0];i<post1;i++){
551     if(seedptr>0){
552       float lin=amp+curve[i];
553       if(seed[seedptr]<lin)seed[seedptr]=lin;
554     }
555     seedptr+=linesper;
556     if(seedptr>=n)break;
557   }
558 }
559
560 static void seed_loop(vorbis_look_psy *p,
561                       const float ***curves,
562                       const float *f, 
563                       const float *flr,
564                       float *seed,
565                       float specmax){
566   vorbis_info_psy *vi=p->vi;
567   long n=p->n,i;
568   float dBoffset=vi->max_curve_dB-specmax;
569
570   /* prime the working vector with peak values */
571
572   for(i=0;i<n;i++){
573     float max=f[i];
574     long oc=p->octave[i];
575     while(i+1<n && p->octave[i+1]==oc){
576       i++;
577       if(f[i]>max)max=f[i];
578     }
579     
580     if(max+6.f>flr[i]){
581       oc=oc>>p->shiftoc;
582       if(oc>=P_BANDS)oc=P_BANDS-1;
583       if(oc<0)oc=0;
584       seed_curve(seed,
585                  curves[oc],
586                  max,
587                  p->octave[i]-p->firstoc,
588                  p->total_octave_lines,
589                  p->eighth_octave_lines,
590                  dBoffset);
591     }
592   }
593 }
594
595 static void seed_chase(float *seeds, int linesper, long n){
596   long  *posstack=alloca(n*sizeof(*posstack));
597   float *ampstack=alloca(n*sizeof(*ampstack));
598   long   stack=0;
599   long   pos=0;
600   long   i;
601
602   for(i=0;i<n;i++){
603     if(stack<2){
604       posstack[stack]=i;
605       ampstack[stack++]=seeds[i];
606     }else{
607       while(1){
608         if(seeds[i]<ampstack[stack-1]){
609           posstack[stack]=i;
610           ampstack[stack++]=seeds[i];
611           break;
612         }else{
613           if(i<posstack[stack-1]+linesper){
614             if(stack>1 && ampstack[stack-1]<=ampstack[stack-2] &&
615                i<posstack[stack-2]+linesper){
616               /* we completely overlap, making stack-1 irrelevant.  pop it */
617               stack--;
618               continue;
619             }
620           }
621           posstack[stack]=i;
622           ampstack[stack++]=seeds[i];
623           break;
624
625         }
626       }
627     }
628   }
629
630   /* the stack now contains only the positions that are relevant. Scan
631      'em straight through */
632
633   for(i=0;i<stack;i++){
634     long endpos;
635     if(i<stack-1 && ampstack[i+1]>ampstack[i]){
636       endpos=posstack[i+1];
637     }else{
638       endpos=posstack[i]+linesper+1; /* +1 is important, else bin 0 is
639                                         discarded in short frames */
640     }
641     if(endpos>n)endpos=n;
642     for(;pos<endpos;pos++)
643       seeds[pos]=ampstack[i];
644   }
645   
646   /* there.  Linear time.  I now remember this was on a problem set I
647      had in Grad Skool... I didn't solve it at the time ;-) */
648
649 }
650
651 /* bleaugh, this is more complicated than it needs to be */
652 static void max_seeds(vorbis_look_psy *p,
653                       float *seed,
654                       float *flr){
655   long   n=p->total_octave_lines;
656   int    linesper=p->eighth_octave_lines;
657   long   linpos=0;
658   long   pos;
659
660   seed_chase(seed,linesper,n); /* for masking */
661  
662   pos=p->octave[0]-p->firstoc-(linesper>>1);
663   while(linpos+1<p->n){
664     float minV=seed[pos];
665     long end=((p->octave[linpos]+p->octave[linpos+1])>>1)-p->firstoc;
666     if(minV>p->vi->tone_abs_limit)minV=p->vi->tone_abs_limit;
667     while(pos+1<=end){
668       pos++;
669       if((seed[pos]>NEGINF && seed[pos]<minV) || minV==NEGINF)
670         minV=seed[pos];
671     }
672     
673     /* seed scale is log.  Floor is linear.  Map back to it */
674     end=pos+p->firstoc;
675     for(;linpos<p->n && p->octave[linpos]<=end;linpos++)
676       if(flr[linpos]<minV)flr[linpos]=minV;
677   }
678   
679   {
680     float minV=seed[p->total_octave_lines-1];
681     for(;linpos<p->n;linpos++)
682       if(flr[linpos]<minV)flr[linpos]=minV;
683   }
684   
685 }
686
687 static void bark_noise_hybridmp(int n,const long *b,
688                                 const float *f,
689                                 float *noise,
690                                 const float offset,
691                                 const int fixed){
692   long i,hi=b[0]>>16,lo=b[0]>>16,hif=0,lof=0;
693   double xa=0,xb=0;
694   double ya=0,yb=0;
695   double x2a=0,x2b=0;
696   double xya=0,xyb=0; 
697   double na=0,nb=0;
698
699   for(i=0;i<n;i++){
700     if(hi<n){
701       /* find new lo/hi */
702       int bi=b[i]&0xffffL;
703       for(;hi<bi;hi++){
704         int ii=(hi<0?-hi:hi);
705         double bin=(f[ii]<-offset?1.:f[ii]+offset);
706         double nn= bin*bin;
707         na  += nn;
708         xa  += hi*nn;
709         ya  += bin*nn;
710         x2a += hi*hi*nn;
711         xya += hi*bin*nn;
712       }
713       bi=b[i]>>16;
714       for(;lo<bi;lo++){
715         int ii=(lo<0?-lo:lo);
716         double bin=(f[ii]<-offset?1.:f[ii]+offset);
717         double nn= bin*bin;
718         na  -= nn;
719         xa  -= lo*nn;
720         ya  -= bin*nn;
721         x2a -= lo*lo*nn;
722         xya -= lo*bin*nn;
723       }
724     }
725
726     if(hif<n && fixed>0){
727       int bi=i+fixed/2;
728       if(bi>n)bi=n;
729
730       for(;hif<bi;hif++){
731         int ii=(hif<0?-hif:hif);
732         double bin=(f[ii]<-offset?1.:f[ii]+offset);
733         double nn= bin*bin;
734         nb  += nn;
735         xb  += hif*nn;
736         yb  += bin*nn;
737         x2b += hif*hif*nn;
738         xyb += hif*bin*nn;
739       }
740       bi=i-(fixed+1)/2;
741       for(;lof<bi;lof++){
742         int ii=(lof<0?-lof:lof);
743         double bin=(f[ii]<-offset?1.:f[ii]+offset);
744         double nn= bin*bin;
745         nb  -= nn;
746         xb  -= lof*nn;
747         yb  -= bin*nn;
748         x2b -= lof*lof*nn;
749         xyb -= lof*bin*nn;
750       }
751     }
752
753     {    
754       double va=0.f;
755       
756       if(na>2){
757         double denom=1./(na*x2a-xa*xa);
758         double a=(ya*x2a-xya*xa)*denom;
759         double b=(na*xya-xa*ya)*denom;
760         va=a+b*i;
761       }
762       if(va<0.)va=0.;
763
764       if(fixed>0){
765         double vb=0.f;
766
767         if(nb>2){
768           double denomf=1./(nb*x2b-xb*xb);
769           double af=(yb*x2b-xyb*xb)*denomf;
770           double bf=(nb*xyb-xb*yb)*denomf;
771           vb=af+bf*i;
772         }
773         if(vb<0.)vb=0.;
774         if(va>vb && vb>0.)va=vb;
775
776       }
777
778       noise[i]=va-offset;
779     }
780   }
781 }
782
783    
784 void _vp_remove_floor(vorbis_look_psy *p,
785                       float *mdct,
786                       float *codedflr,
787                       float *residue){ 
788   int i,n=p->n;
789   
790   for(i=0;i<n;i++)
791     if(mdct[i]!=0.f)
792       residue[i]=mdct[i]/codedflr[i];
793     else
794       residue[i]=0.f;
795 }
796   
797
798 void _vp_compute_mask(vorbis_look_psy *p,
799                       float *logfft, 
800                       float *logmdct, 
801                       float *logmask, 
802                       float global_specmax,
803                       float local_specmax,
804                       float bitrate_noise_offset){
805   int i,n=p->n;
806   static int seq=0;
807
808   float *seed=alloca(sizeof(*seed)*p->total_octave_lines);
809   for(i=0;i<p->total_octave_lines;i++)seed[i]=NEGINF;
810
811   /* noise masking */
812   if(p->vi->noisemaskp){
813     float *work=alloca(n*sizeof(*work));
814
815     bark_noise_hybridmp(n,p->bark,logmdct,logmask,
816                         140.,-1);
817
818     for(i=0;i<n;i++)work[i]=logmdct[i]-logmask[i];
819
820     bark_noise_hybridmp(n,p->bark,work,logmask,0.,
821                         p->vi->noisewindowfixed);
822
823     for(i=0;i<n;i++)work[i]=logmdct[i]-work[i];
824
825     /* work[i] holds the median line (.5), logmask holds the upper
826        envelope line (1.) */
827     _analysis_output("noisemedian",seq,work,n,1,0);
828
829     for(i=0;i<n;i++)logmask[i]+=work[i];
830     _analysis_output("noiseenvelope",seq,logmask,n,1,0);
831     for(i=0;i<n;i++)logmask[i]-=work[i];
832
833     for(i=0;i<n;i++){
834       int dB=logmask[i]+.5;
835       if(dB>=NOISE_COMPAND_LEVELS)dB=NOISE_COMPAND_LEVELS-1;
836       logmask[i]= work[i]+p->vi->noisecompand[dB]+p->noiseoffset[i]+bitrate_noise_offset;
837       if(logmask[i]>p->vi->noisemaxsupp)logmask[i]=p->vi->noisemaxsupp;
838     }
839     _analysis_output("noise",seq,logmask,n,1,0);
840
841   }else{
842     for(i=0;i<n;i++)logmask[i]=NEGINF;
843   }
844
845   /* set the ATH (floating below localmax, not global max by a
846      specified att) */
847   if(p->vi->ath){
848     float att=local_specmax+p->vi->ath_adjatt;
849     if(att<p->vi->ath_maxatt)att=p->vi->ath_maxatt;
850
851     for(i=0;i<n;i++){
852       float av=p->ath[i]+att;
853       if(av>logmask[i])logmask[i]=av;
854     }
855   }
856
857   /* tone masking */
858   seed_loop(p,(const float ***)p->tonecurves,logfft,logmask,seed,global_specmax);
859   max_seeds(p,seed,logmask);
860
861   /* doing this here is clean, but we need to find a faster way to do
862      it than to just tack it on */
863
864   for(i=0;i<n;i++)if(logmdct[i]>=logmask[i])break;
865   if(i==n)
866     for(i=0;i<n;i++)logmask[i]=NEGINF;
867   else
868     for(i=0;i<n;i++)
869       logfft[i]=max(logmdct[i],logfft[i]);
870
871   seq++;
872
873 }
874
875 float _vp_ampmax_decay(float amp,vorbis_dsp_state *vd){
876   vorbis_info *vi=vd->vi;
877   codec_setup_info *ci=vi->codec_setup;
878   vorbis_info_psy_global *gi=&ci->psy_g_param;
879
880   int n=ci->blocksizes[vd->W]/2;
881   float secs=(float)n/vi->rate;
882
883   amp+=secs*gi->ampmax_att_per_sec;
884   if(amp<-9999)amp=-9999;
885   return(amp);
886 }
887
888 static void couple_lossless(float A, float B, 
889                             float granule,float igranule,
890                             float *mag, float *ang,
891                             int flip_p){
892
893   if(fabs(A)>fabs(B)){
894     A=rint(A*igranule)*granule; /* must be done *after* the comparison */
895     B=rint(B*igranule)*granule;
896   
897     *mag=A; *ang=(A>0.f?A-B:B-A);
898   }else{
899     A=rint(A*igranule)*granule;
900     B=rint(B*igranule)*granule;
901   
902     *mag=B; *ang=(B>0.f?A-B:B-A);
903   }
904
905   if(flip_p && *ang>fabs(*mag)*1.9999f){
906     *ang= -fabs(*mag)*2.f;
907     *mag= -*mag;
908   }
909 }
910
911 static void couple_point(float A, float B, float fA, float fB, 
912                          float granule,float igranule,
913                          float fmag, float *mag, float *ang){
914
915   float origmag=FAST_HYPOT(A*fA,B*fB),corr;
916
917   if(fmag!=0.f){
918     
919     if(fabs(A)>fabs(B)){
920       *mag=A;
921     }else{
922       *mag=B;
923     }
924     
925     corr=origmag/FAST_HYPOT(fA,fB);
926     *mag=unitnorm(*mag)*floorf(corr*igranule+.5f)*granule; 
927     *ang=0.f;
928
929   }else{
930     *mag=0.f;
931     *ang=0.f;
932   }    
933 }
934
935
936 void _vp_quantize_couple(vorbis_look_psy *p,
937                          vorbis_info_mapping0 *vi,
938                          float **pcm,
939                          float **sofar,
940                          float **quantized,
941                          int   *nonzero,
942                          int   passno){
943
944   int i,j,k,n=p->n;
945   vorbis_info_psy *info=p->vi;
946
947   /* perform any requested channel coupling */
948   for(i=0;i<vi->coupling_steps;i++){
949     float granulem=info->couple_pass[passno].granulem;
950     float igranulem=info->couple_pass[passno].igranulem;
951
952     /* make sure coupling a zero and a nonzero channel results in two
953        nonzero channels. */
954     if(nonzero[vi->coupling_mag[i]] ||
955        nonzero[vi->coupling_ang[i]]){
956       
957       float *pcmM=pcm[vi->coupling_mag[i]];
958       float *pcmA=pcm[vi->coupling_ang[i]];
959       float *floorM=pcm[vi->coupling_mag[i]]+n;
960       float *floorA=pcm[vi->coupling_ang[i]]+n;
961       float *sofarM=sofar[vi->coupling_mag[i]];
962       float *sofarA=sofar[vi->coupling_ang[i]];
963       float *qM=quantized[vi->coupling_mag[i]];
964       float *qA=quantized[vi->coupling_ang[i]];
965
966       nonzero[vi->coupling_mag[i]]=1; 
967       nonzero[vi->coupling_ang[i]]=1; 
968
969       for(j=0,k=0;j<n;k++){
970         vp_couple *part=info->couple_pass[passno].couple_pass+k;
971         float rqlimit=part->outofphase_requant_limit;
972         int flip_p=part->outofphase_redundant_flip_p;
973     
974         for(;j<part->limit && j<p->n;j++){
975           /* partition by partition; k is our by-location partition
976              class counter */
977           float ang,mag,fmag=max(fabs(pcmM[j]),fabs(pcmA[j]));
978
979           if(fmag<part->amppost_point){
980             couple_point(pcmM[j],pcmA[j],floorM[j],floorA[j],
981                          granulem,igranulem,fmag,&mag,&ang);
982
983           }else{
984             couple_lossless(pcmM[j],pcmA[j],
985                             granulem,igranulem,&mag,&ang,flip_p);
986           }
987
988           /* executive decision time: when requantizing and recoupling
989              residue in order to progressively encode at finer
990              resolution, an out of phase component that originally
991              quntized to 2*mag can flip flop magnitude/angle if it
992              requantizes to not-quite out of phase.  If that happens,
993              we opt not to fill in additional resolution (in order to
994              simplify the iterative codebook design and
995              efficiency). */
996
997           qM[j]=mag-sofarM[j];
998           qA[j]=ang-sofarA[j];
999          
1000           if(qA[j]<-rqlimit || qA[j]>rqlimit){
1001             qM[j]=0.f;
1002             qA[j]=0.f;
1003           }
1004         }
1005       }
1006     }
1007   }
1008 }