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