patch a nasty bounds bug introduced a week or two ago. It could nuke
[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.71 2002/07/01 05:29:41 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 static double stereo_threshholds[]={0.0, .5, 1.0, 1.5, 2.5, 4.5, 8.5, 16.5, 9e10};
34
35 vorbis_look_psy_global *_vp_global_look(vorbis_info *vi){
36   codec_setup_info *ci=vi->codec_setup;
37   vorbis_info_psy_global *gi=&ci->psy_g_param;
38   vorbis_look_psy_global *look=_ogg_calloc(1,sizeof(*look));
39
40   look->channels=vi->channels;
41
42   look->ampmax=-9999.;
43   look->gi=gi;
44   return(look);
45 }
46
47 void _vp_global_free(vorbis_look_psy_global *look){
48   if(look){
49     memset(look,0,sizeof(*look));
50     _ogg_free(look);
51   }
52 }
53
54 void _vi_gpsy_free(vorbis_info_psy_global *i){
55   if(i){
56     memset(i,0,sizeof(*i));
57     _ogg_free(i);
58   }
59 }
60
61 void _vi_psy_free(vorbis_info_psy *i){
62   if(i){
63     memset(i,0,sizeof(*i));
64     _ogg_free(i);
65   }
66 }
67
68 static void min_curve(float *c,
69                        float *c2){
70   int i;  
71   for(i=0;i<EHMER_MAX;i++)if(c2[i]<c[i])c[i]=c2[i];
72 }
73 static void max_curve(float *c,
74                        float *c2){
75   int i;  
76   for(i=0;i<EHMER_MAX;i++)if(c2[i]>c[i])c[i]=c2[i];
77 }
78
79 static void attenuate_curve(float *c,float att){
80   int i;
81   for(i=0;i<EHMER_MAX;i++)
82     c[i]+=att;
83 }
84
85 static float ***setup_tone_curves(float curveatt_dB[P_BANDS],float binHz,int n,
86                                   float center_boost, float center_decay_rate){
87   int i,j,k,m;
88   float ath[EHMER_MAX];
89   float workc[P_BANDS][P_LEVELS][EHMER_MAX];
90   float athc[P_LEVELS][EHMER_MAX];
91   float *brute_buffer=alloca(n*sizeof(*brute_buffer));
92
93   float ***ret=_ogg_malloc(sizeof(*ret)*P_BANDS);
94
95   memset(workc,0,sizeof(workc));
96
97   for(i=0;i<P_BANDS;i++){
98     /* we add back in the ATH to avoid low level curves falling off to
99        -infinity and unnecessarily cutting off high level curves in the
100        curve limiting (last step). */
101
102     /* A half-band's settings must be valid over the whole band, and
103        it's better to mask too little than too much */  
104     int ath_offset=i*4;
105     for(j=0;j<EHMER_MAX;j++){
106       float min=999.;
107       for(k=0;k<4;k++)
108         if(j+k+ath_offset<MAX_ATH){
109           if(min>ATH[j+k+ath_offset])min=ATH[j+k+ath_offset];
110         }else{
111           if(min>ATH[MAX_ATH-1])min=ATH[MAX_ATH-1];
112         }
113       ath[j]=min;
114     }
115
116     /* copy curves into working space, replicate the 50dB curve to 30
117        and 40, replicate the 100dB curve to 110 */
118     for(j=0;j<6;j++)
119       memcpy(workc[i][j+2],tonemasks[i][j],EHMER_MAX*sizeof(*tonemasks[i][j]));
120     memcpy(workc[i][0],tonemasks[i][0],EHMER_MAX*sizeof(*tonemasks[i][0]));
121     memcpy(workc[i][1],tonemasks[i][0],EHMER_MAX*sizeof(*tonemasks[i][0]));
122     
123     /* apply centered curve boost/decay */
124     for(j=0;j<P_LEVELS;j++){
125       for(k=0;k<EHMER_MAX;k++){
126         float adj=center_boost+abs(EHMER_OFFSET-k)*center_decay_rate;
127         if(adj<0. && center_boost>0)adj=0.;
128         if(adj>0. && center_boost<0)adj=0.;
129         workc[i][j][k]+=adj;
130       }
131     }
132
133     /* normalize curves so the driving amplitude is 0dB */
134     /* make temp curves with the ATH overlayed */
135     for(j=0;j<P_LEVELS;j++){
136       attenuate_curve(workc[i][j],curveatt_dB[i]+100.-(j<2?2:j)*10.-P_LEVEL_0);
137       memcpy(athc[j],ath,EHMER_MAX*sizeof(**athc));
138       attenuate_curve(athc[j],+100.-j*10.f-P_LEVEL_0);
139       max_curve(athc[j],workc[i][j]);
140     }
141
142     /* Now limit the louder curves.
143        
144        the idea is this: We don't know what the playback attenuation
145        will be; 0dB SL moves every time the user twiddles the volume
146        knob. So that means we have to use a single 'most pessimal' curve
147        for all masking amplitudes, right?  Wrong.  The *loudest* sound
148        can be in (we assume) a range of ...+100dB] SL.  However, sounds
149        20dB down will be in a range ...+80], 40dB down is from ...+60],
150        etc... */
151     
152     for(j=1;j<P_LEVELS;j++){
153       min_curve(athc[j],athc[j-1]);
154       min_curve(workc[i][j],athc[j]);
155     }
156   }
157
158   for(i=0;i<P_BANDS;i++){
159     int hi_curve,lo_curve,bin;
160     ret[i]=_ogg_malloc(sizeof(**ret)*P_LEVELS);
161
162     /* low frequency curves are measured with greater resolution than
163        the MDCT/FFT will actually give us; we want the curve applied
164        to the tone data to be pessimistic and thus apply the minimum
165        masking possible for a given bin.  That means that a single bin
166        could span more than one octave and that the curve will be a
167        composite of multiple octaves.  It also may mean that a single
168        bin may span > an eighth of an octave and that the eighth
169        octave values may also be composited. */
170     
171     /* which octave curves will we be compositing? */
172     bin=floor(fromOC(i*.5)/binHz);
173     lo_curve=  ceil(toOC(bin*binHz+1)*2);
174     hi_curve=  floor(toOC((bin+1)*binHz)*2);
175     if(lo_curve>i)lo_curve=i;
176     if(lo_curve<0)lo_curve=0;
177     if(hi_curve>=P_BANDS)hi_curve=P_BANDS-1;
178
179     for(m=0;m<P_LEVELS;m++){
180       ret[i][m]=_ogg_malloc(sizeof(***ret)*(EHMER_MAX+2));
181       
182       for(j=0;j<n;j++)brute_buffer[j]=999.;
183       
184       /* render the curve into bins, then pull values back into curve.
185          The point is that any inherent subsampling aliasing results in
186          a safe minimum */
187       for(k=lo_curve;k<=hi_curve;k++){
188         int l=0;
189
190         for(j=0;j<EHMER_MAX;j++){
191           int lo_bin= fromOC(j*.125+k*.5-2.0625)/binHz;
192           int hi_bin= fromOC(j*.125+k*.5-1.9375)/binHz+1;
193           
194           if(lo_bin<0)lo_bin=0;
195           if(lo_bin>n)lo_bin=n;
196           if(lo_bin<l)l=lo_bin;
197           if(hi_bin<0)hi_bin=0;
198           if(hi_bin>n)hi_bin=n;
199
200           for(;l<hi_bin && l<n;l++)
201             if(brute_buffer[l]>workc[k][m][j])
202               brute_buffer[l]=workc[k][m][j];
203         }
204
205         for(;l<n;l++)
206           if(brute_buffer[l]>workc[k][m][EHMER_MAX-1])
207             brute_buffer[l]=workc[k][m][EHMER_MAX-1];
208
209       }
210
211       /* be equally paranoid about being valid up to next half ocatve */
212       if(i+1<P_BANDS){
213         int l=0;
214         k=i+1;
215         for(j=0;j<EHMER_MAX;j++){
216           int lo_bin= fromOC(j*.125+i*.5-2.0625)/binHz;
217           int hi_bin= fromOC(j*.125+i*.5-1.9375)/binHz+1;
218           
219           if(lo_bin<0)lo_bin=0;
220           if(lo_bin>n)lo_bin=n;
221           if(lo_bin<l)l=lo_bin;
222           if(hi_bin<0)hi_bin=0;
223           if(hi_bin>n)hi_bin=n;
224
225           for(;l<hi_bin && l<n;l++)
226             if(brute_buffer[l]>workc[k][m][j])
227               brute_buffer[l]=workc[k][m][j];
228         }
229
230         for(;l<n;l++)
231           if(brute_buffer[l]>workc[k][m][EHMER_MAX-1])
232             brute_buffer[l]=workc[k][m][EHMER_MAX-1];
233
234       }
235
236
237       for(j=0;j<EHMER_MAX;j++){
238         int bin=fromOC(j*.125+i*.5-2.)/binHz;
239         if(bin<0){
240           ret[i][m][j+2]=-999.;
241         }else{
242           if(bin>=n){
243             ret[i][m][j+2]=-999.;
244           }else{
245             ret[i][m][j+2]=brute_buffer[bin];
246           }
247         }
248       }
249
250       /* add fenceposts */
251       for(j=0;j<EHMER_OFFSET;j++)
252         if(ret[i][m][j+2]>-200.f)break;  
253       ret[i][m][0]=j;
254       
255       for(j=EHMER_MAX-1;j>EHMER_OFFSET+1;j--)
256         if(ret[i][m][j+2]>-200.f)
257           break;
258       ret[i][m][1]=j;
259
260     }
261   }
262
263   return(ret);
264 }
265
266 void _vp_psy_init(vorbis_look_psy *p,vorbis_info_psy *vi,
267                   vorbis_info_psy_global *gi,int n,long rate){
268   long i,j,lo=-99,hi=0;
269   long maxoc;
270   memset(p,0,sizeof(*p));
271
272   p->eighth_octave_lines=gi->eighth_octave_lines;
273   p->shiftoc=rint(log(gi->eighth_octave_lines*8.f)/log(2.f))-1;
274
275   p->firstoc=toOC(.25f*rate*.5/n)*(1<<(p->shiftoc+1))-gi->eighth_octave_lines;
276   maxoc=toOC((n+.25f)*rate*.5/n)*(1<<(p->shiftoc+1))+.5f;
277   p->total_octave_lines=maxoc-p->firstoc+1;
278   p->ath=_ogg_malloc(n*sizeof(*p->ath));
279
280   p->octave=_ogg_malloc(n*sizeof(*p->octave));
281   p->bark=_ogg_malloc(n*sizeof(*p->bark));
282   p->vi=vi;
283   p->n=n;
284   p->rate=rate;
285
286   /* set up the lookups for a given blocksize and sample rate */
287
288   for(i=0,j=0;i<MAX_ATH-1;i++){
289     int endpos=rint(fromOC((i+1)*.125-2.)*2*n/rate);
290     float base=ATH[i];
291     if(j<endpos){
292       float delta=(ATH[i+1]-base)/(endpos-j);
293       for(;j<endpos && j<n;j++){
294         p->ath[j]=base+100.;
295         base+=delta;
296       }
297     }
298   }
299
300   for(i=0;i<n;i++){
301     float bark=toBARK(rate/(2*n)*i); 
302
303     for(;lo+vi->noisewindowlomin<i && 
304           toBARK(rate/(2*n)*lo)<(bark-vi->noisewindowlo);lo++);
305     
306     for(;hi<n && (hi<i+vi->noisewindowhimin ||
307           toBARK(rate/(2*n)*hi)<(bark+vi->noisewindowhi));hi++);
308     
309     p->bark[i]=(lo<<16)+hi;
310
311   }
312
313   for(i=0;i<n;i++)
314     p->octave[i]=toOC((i+.25f)*.5*rate/n)*(1<<(p->shiftoc+1))+.5f;
315
316   p->tonecurves=setup_tone_curves(vi->toneatt,rate*.5/n,n,
317                                   vi->tone_centerboost,vi->tone_decay);
318   
319   /* set up rolling noise median */
320   p->noiseoffset=_ogg_malloc(P_NOISECURVES*sizeof(*p->noiseoffset));
321   for(i=0;i<P_NOISECURVES;i++)
322     p->noiseoffset[i]=_ogg_malloc(n*sizeof(**p->noiseoffset));
323   
324   for(i=0;i<n;i++){
325     float halfoc=toOC((i+.5)*rate/(2.*n))*2.;
326     int inthalfoc;
327     float del;
328     
329     if(halfoc<0)halfoc=0;
330     if(halfoc>=P_BANDS-1)halfoc=P_BANDS-1;
331     inthalfoc=(int)halfoc;
332     del=halfoc-inthalfoc;
333     
334     for(j=0;j<P_NOISECURVES;j++)
335       p->noiseoffset[j][i]=
336         p->vi->noiseoff[j][inthalfoc]*(1.-del) + 
337         p->vi->noiseoff[j][inthalfoc+1]*del;
338     
339   }
340 #if 0
341   {
342     static int ls=0;
343     _analysis_output_always("noiseoff0",ls,p->noiseoffset[0],n,1,0,0);
344     _analysis_output_always("noiseoff1",ls,p->noiseoffset[1],n,1,0,0);
345     _analysis_output_always("noiseoff2",ls++,p->noiseoffset[2],n,1,0,0);
346   }
347 #endif
348 }
349
350 void _vp_psy_clear(vorbis_look_psy *p){
351   int i,j;
352   if(p){
353     if(p->ath)_ogg_free(p->ath);
354     if(p->octave)_ogg_free(p->octave);
355     if(p->bark)_ogg_free(p->bark);
356     if(p->tonecurves){
357       for(i=0;i<P_BANDS;i++){
358         for(j=0;j<P_LEVELS;j++){
359           _ogg_free(p->tonecurves[i][j]);
360         }
361         _ogg_free(p->tonecurves[i]);
362       }
363       _ogg_free(p->tonecurves);
364     }
365     if(p->noiseoffset){
366       for(i=0;i<P_NOISECURVES;i++){
367         _ogg_free(p->noiseoffset[i]);
368       }
369       _ogg_free(p->noiseoffset);
370     }
371     memset(p,0,sizeof(*p));
372   }
373 }
374
375 /* octave/(8*eighth_octave_lines) x scale and dB y scale */
376 static void seed_curve(float *seed,
377                        const float **curves,
378                        float amp,
379                        int oc, int n,
380                        int linesper,float dBoffset){
381   int i,post1;
382   int seedptr;
383   const float *posts,*curve;
384
385   int choice=(int)((amp+dBoffset-P_LEVEL_0)*.1f);
386   choice=max(choice,0);
387   choice=min(choice,P_LEVELS-1);
388   posts=curves[choice];
389   curve=posts+2;
390   post1=(int)posts[1];
391   seedptr=oc+(posts[0]-EHMER_OFFSET)*linesper-(linesper>>1);
392
393   for(i=posts[0];i<post1;i++){
394     if(seedptr>0){
395       float lin=amp+curve[i];
396       if(seed[seedptr]<lin)seed[seedptr]=lin;
397     }
398     seedptr+=linesper;
399     if(seedptr>=n)break;
400   }
401 }
402
403 static void seed_loop(vorbis_look_psy *p,
404                       const float ***curves,
405                       const float *f, 
406                       const float *flr,
407                       float *seed,
408                       float specmax){
409   vorbis_info_psy *vi=p->vi;
410   long n=p->n,i;
411   float dBoffset=vi->max_curve_dB-specmax;
412
413   /* prime the working vector with peak values */
414
415   for(i=0;i<n;i++){
416     float max=f[i];
417     long oc=p->octave[i];
418     while(i+1<n && p->octave[i+1]==oc){
419       i++;
420       if(f[i]>max)max=f[i];
421     }
422     
423     if(max+6.f>flr[i]){
424       oc=oc>>p->shiftoc;
425
426       if(oc>=P_BANDS)oc=P_BANDS-1;
427       if(oc<0)oc=0;
428
429       seed_curve(seed,
430                  curves[oc],
431                  max,
432                  p->octave[i]-p->firstoc,
433                  p->total_octave_lines,
434                  p->eighth_octave_lines,
435                  dBoffset);
436     }
437   }
438 }
439
440 static void seed_chase(float *seeds, int linesper, long n){
441   long  *posstack=alloca(n*sizeof(*posstack));
442   float *ampstack=alloca(n*sizeof(*ampstack));
443   long   stack=0;
444   long   pos=0;
445   long   i;
446
447   for(i=0;i<n;i++){
448     if(stack<2){
449       posstack[stack]=i;
450       ampstack[stack++]=seeds[i];
451     }else{
452       while(1){
453         if(seeds[i]<ampstack[stack-1]){
454           posstack[stack]=i;
455           ampstack[stack++]=seeds[i];
456           break;
457         }else{
458           if(i<posstack[stack-1]+linesper){
459             if(stack>1 && ampstack[stack-1]<=ampstack[stack-2] &&
460                i<posstack[stack-2]+linesper){
461               /* we completely overlap, making stack-1 irrelevant.  pop it */
462               stack--;
463               continue;
464             }
465           }
466           posstack[stack]=i;
467           ampstack[stack++]=seeds[i];
468           break;
469
470         }
471       }
472     }
473   }
474
475   /* the stack now contains only the positions that are relevant. Scan
476      'em straight through */
477
478   for(i=0;i<stack;i++){
479     long endpos;
480     if(i<stack-1 && ampstack[i+1]>ampstack[i]){
481       endpos=posstack[i+1];
482     }else{
483       endpos=posstack[i]+linesper+1; /* +1 is important, else bin 0 is
484                                         discarded in short frames */
485     }
486     if(endpos>n)endpos=n;
487     for(;pos<endpos;pos++)
488       seeds[pos]=ampstack[i];
489   }
490   
491   /* there.  Linear time.  I now remember this was on a problem set I
492      had in Grad Skool... I didn't solve it at the time ;-) */
493
494 }
495
496 /* bleaugh, this is more complicated than it needs to be */
497 #include<stdio.h>
498 static void max_seeds(vorbis_look_psy *p,
499                       float *seed,
500                       float *flr){
501   long   n=p->total_octave_lines;
502   int    linesper=p->eighth_octave_lines;
503   long   linpos=0;
504   long   pos;
505
506   seed_chase(seed,linesper,n); /* for masking */
507  
508   pos=p->octave[0]-p->firstoc-(linesper>>1);
509
510   while(linpos+1<p->n){
511     float minV=seed[pos];
512     long end=((p->octave[linpos]+p->octave[linpos+1])>>1)-p->firstoc;
513     if(minV>p->vi->tone_abs_limit)minV=p->vi->tone_abs_limit;
514     while(pos+1<=end){
515       pos++;
516       if((seed[pos]>NEGINF && seed[pos]<minV) || minV==NEGINF)
517         minV=seed[pos];
518     }
519     
520     end=pos+p->firstoc;
521     for(;linpos<p->n && p->octave[linpos]<=end;linpos++)
522       if(flr[linpos]<minV)flr[linpos]=minV;
523   }
524   
525   {
526     float minV=seed[p->total_octave_lines-1];
527     for(;linpos<p->n;linpos++)
528       if(flr[linpos]<minV)flr[linpos]=minV;
529   }
530   
531 }
532
533 static void bark_noise_hybridmp(int n,const long *b,
534                                 const float *f,
535                                 float *noise,
536                                 const float offset,
537                                 const int fixed){
538   
539   float *N=alloca((n+1)*sizeof(*N));
540   float *X=alloca((n+1)*sizeof(*N));
541   float *XX=alloca((n+1)*sizeof(*N));
542   float *Y=alloca((n+1)*sizeof(*N));
543   float *XY=alloca((n+1)*sizeof(*N));
544
545   float tN, tX, tXX, tY, tXY;
546   float fi;
547   int i;
548
549   int lo, hi;
550   float R, A, B, D;
551   
552   tN = tX = tXX = tY = tXY = 0.f;
553   for (i = 0, fi = 0.f; i < n; i++, fi += 1.f) {
554     float w, x, y;
555     
556     x = fi;
557     y = f[i] + offset;
558     if (y < 1.f) y = 1.f;
559     w = y * y;
560     N[i] = tN;
561     X[i] = tX;
562     XX[i] = tXX;
563     Y[i] = tY;
564     XY[i] = tXY;
565     tN += w;
566     tX += w * x;
567     tXX += w * x * x;
568     tY += w * y;
569     tXY += w * x * y;
570   }
571   N[i] = tN;
572   X[i] = tX;
573   XX[i] = tXX;
574   Y[i] = tY;
575   XY[i] = tXY;
576   
577   for (i = 0, fi = 0.f;; i++, fi += 1.f) {
578     
579     lo = b[i] >> 16;
580     if( lo>=0 ) break;
581     hi = b[i] & 0xffff;
582     
583     tN = N[hi] + N[-lo];
584     tX = X[hi] - X[-lo];
585     tXX = XX[hi] + XX[-lo];
586     tY = Y[hi] + Y[-lo];    
587     tXY = XY[hi] - XY[-lo];
588     
589     A = tY * tXX - tX * tXY;
590     B = tN * tXY - tX * tY;
591     D = tN * tXX - tX * tX;
592     R = (A + fi * B) / D;
593     if (R < 0.f)
594       R = 0.f;
595     
596     noise[i] = R - offset;
597   }
598   
599   for ( ; hi < n; i++, fi += 1.f) {
600     
601     lo = b[i] >> 16;
602     hi = b[i] & 0xffff;
603     
604     tN = N[hi] - N[lo];
605     tX = X[hi] - X[lo];
606     tXX = XX[hi] - XX[lo];
607     tY = Y[hi] - Y[lo];
608     tXY = XY[hi] - XY[lo];
609     
610     A = tY * tXX - tX * tXY;
611     B = tN * tXY - tX * tY;
612     D = tN * tXX - tX * tX;
613     R = (A + fi * B) / D;
614     if (R < 0.f) R = 0.f;
615     
616     noise[i] = R - offset;
617   }
618   for ( ; i < n; i++, fi += 1.f) {
619     
620     R = (A + fi * B) / D;
621     if (R < 0.f) R = 0.f;
622     
623     noise[i] = R - offset;
624   }
625   
626   if (fixed <= 0) return;
627   
628   for (i = 0, fi = 0.f; i < (fixed + 1) / 2; i++, fi += 1.f) {
629     hi = i + fixed / 2;
630     lo = hi - fixed;
631     
632     tN = N[hi] + N[-lo];
633     tX = X[hi] - X[-lo];
634     tXX = XX[hi] + XX[-lo];
635     tY = Y[hi] + Y[-lo];
636     tXY = XY[hi] - XY[-lo];
637     
638     
639     A = tY * tXX - tX * tXY;
640     B = tN * tXY - tX * tY;
641     D = tN * tXX - tX * tX;
642     R = (A + fi * B) / D;
643
644     if (R > 0.f && R - offset < noise[i]) noise[i] = R - offset;
645   }
646   for ( ; hi < n; i++, fi += 1.f) {
647     
648     hi = i + fixed / 2;
649     lo = hi - fixed;
650     
651     tN = N[hi] - N[lo];
652     tX = X[hi] - X[lo];
653     tXX = XX[hi] - XX[lo];
654     tY = Y[hi] - Y[lo];
655     tXY = XY[hi] - XY[lo];
656     
657     A = tY * tXX - tX * tXY;
658     B = tN * tXY - tX * tY;
659     D = tN * tXX - tX * tX;
660     R = (A + fi * B) / D;
661     
662     if (R > 0.f && R - offset < noise[i]) noise[i] = R - offset;
663   }
664   for ( ; i < n; i++, fi += 1.f) {
665     R = (A + fi * B) / D;
666     if (R > 0.f && R - offset < noise[i]) noise[i] = R - offset;
667   }
668 }
669
670 static float FLOOR1_fromdB_INV_LOOKUP[256]={
671   0.F, 8.81683e+06F, 8.27882e+06F, 7.77365e+06F, 
672   7.29930e+06F, 6.85389e+06F, 6.43567e+06F, 6.04296e+06F, 
673   5.67422e+06F, 5.32798e+06F, 5.00286e+06F, 4.69759e+06F, 
674   4.41094e+06F, 4.14178e+06F, 3.88905e+06F, 3.65174e+06F, 
675   3.42891e+06F, 3.21968e+06F, 3.02321e+06F, 2.83873e+06F, 
676   2.66551e+06F, 2.50286e+06F, 2.35014e+06F, 2.20673e+06F, 
677   2.07208e+06F, 1.94564e+06F, 1.82692e+06F, 1.71544e+06F, 
678   1.61076e+06F, 1.51247e+06F, 1.42018e+06F, 1.33352e+06F, 
679   1.25215e+06F, 1.17574e+06F, 1.10400e+06F, 1.03663e+06F, 
680   973377.F, 913981.F, 858210.F, 805842.F, 
681   756669.F, 710497.F, 667142.F, 626433.F, 
682   588208.F, 552316.F, 518613.F, 486967.F, 
683   457252.F, 429351.F, 403152.F, 378551.F, 
684   355452.F, 333762.F, 313396.F, 294273.F, 
685   276316.F, 259455.F, 243623.F, 228757.F, 
686   214798.F, 201691.F, 189384.F, 177828.F, 
687   166977.F, 156788.F, 147221.F, 138237.F, 
688   129802.F, 121881.F, 114444.F, 107461.F, 
689   100903.F, 94746.3F, 88964.9F, 83536.2F, 
690   78438.8F, 73652.5F, 69158.2F, 64938.1F, 
691   60975.6F, 57254.9F, 53761.2F, 50480.6F, 
692   47400.3F, 44507.9F, 41792.0F, 39241.9F, 
693   36847.3F, 34598.9F, 32487.7F, 30505.3F, 
694   28643.8F, 26896.0F, 25254.8F, 23713.7F, 
695   22266.7F, 20908.0F, 19632.2F, 18434.2F, 
696   17309.4F, 16253.1F, 15261.4F, 14330.1F, 
697   13455.7F, 12634.6F, 11863.7F, 11139.7F, 
698   10460.0F, 9821.72F, 9222.39F, 8659.64F, 
699   8131.23F, 7635.06F, 7169.17F, 6731.70F, 
700   6320.93F, 5935.23F, 5573.06F, 5232.99F, 
701   4913.67F, 4613.84F, 4332.30F, 4067.94F, 
702   3819.72F, 3586.64F, 3367.78F, 3162.28F, 
703   2969.31F, 2788.13F, 2617.99F, 2458.24F, 
704   2308.24F, 2167.39F, 2035.14F, 1910.95F, 
705   1794.35F, 1684.85F, 1582.04F, 1485.51F, 
706   1394.86F, 1309.75F, 1229.83F, 1154.78F, 
707   1084.32F, 1018.15F, 956.024F, 897.687F, 
708   842.910F, 791.475F, 743.179F, 697.830F, 
709   655.249F, 615.265F, 577.722F, 542.469F, 
710   509.367F, 478.286F, 449.101F, 421.696F, 
711   395.964F, 371.803F, 349.115F, 327.812F, 
712   307.809F, 289.026F, 271.390F, 254.830F, 
713   239.280F, 224.679F, 210.969F, 198.096F, 
714   186.008F, 174.658F, 164.000F, 153.993F, 
715   144.596F, 135.773F, 127.488F, 119.708F, 
716   112.404F, 105.545F, 99.1046F, 93.0572F, 
717   87.3788F, 82.0469F, 77.0404F, 72.3394F, 
718   67.9252F, 63.7804F, 59.8885F, 56.2341F, 
719   52.8027F, 49.5807F, 46.5553F, 43.7144F, 
720   41.0470F, 38.5423F, 36.1904F, 33.9821F, 
721   31.9085F, 29.9614F, 28.1332F, 26.4165F, 
722   24.8045F, 23.2910F, 21.8697F, 20.5352F, 
723   19.2822F, 18.1056F, 17.0008F, 15.9634F, 
724   14.9893F, 14.0746F, 13.2158F, 12.4094F, 
725   11.6522F, 10.9411F, 10.2735F, 9.64662F, 
726   9.05798F, 8.50526F, 7.98626F, 7.49894F, 
727   7.04135F, 6.61169F, 6.20824F, 5.82941F, 
728   5.47370F, 5.13970F, 4.82607F, 4.53158F, 
729   4.25507F, 3.99542F, 3.75162F, 3.52269F, 
730   3.30774F, 3.10590F, 2.91638F, 2.73842F, 
731   2.57132F, 2.41442F, 2.26709F, 2.12875F, 
732   1.99885F, 1.87688F, 1.76236F, 1.65482F, 
733   1.55384F, 1.45902F, 1.36999F, 1.28640F, 
734   1.20790F, 1.13419F, 1.06499F, 1.F
735 };
736
737 void _vp_remove_floor(vorbis_look_psy *p,
738                       float *mdct,
739                       int *codedflr,
740                       float *residue,
741                       int sliding_lowpass){ 
742
743   int i,n=p->n;
744  
745   if(sliding_lowpass>n)sliding_lowpass=n;
746   
747   for(i=0;i<sliding_lowpass;i++){
748     residue[i]=
749       mdct[i]*FLOOR1_fromdB_INV_LOOKUP[codedflr[i]];
750   }
751
752   for(;i<n;i++)
753     residue[i]=0.;
754 }
755
756 void _vp_noisemask(vorbis_look_psy *p,
757                    float *logmdct, 
758                    float *logmask){
759
760   int i,n=p->n;
761   float *work=alloca(n*sizeof(*work));
762   
763   bark_noise_hybridmp(n,p->bark,logmdct,logmask,
764                       140.,-1);
765
766   for(i=0;i<n;i++)work[i]=logmdct[i]-logmask[i];
767
768   bark_noise_hybridmp(n,p->bark,work,logmask,0.,
769                       p->vi->noisewindowfixed);
770
771   for(i=0;i<n;i++)work[i]=logmdct[i]-work[i];
772
773   /* work[i] holds the median line (.5), logmask holds the upper
774      envelope line (1.) */
775   
776   for(i=0;i<n;i++){
777     int dB=logmask[i]+.5;
778     if(dB>=NOISE_COMPAND_LEVELS)dB=NOISE_COMPAND_LEVELS-1;
779     logmask[i]= work[i]+p->vi->noisecompand[dB];
780   }
781 }
782
783 void _vp_tonemask(vorbis_look_psy *p,
784                   float *logfft,
785                   float *logmask,
786                   float global_specmax,
787                   float local_specmax){
788
789   int i,n=p->n;
790
791   float *seed=alloca(sizeof(*seed)*p->total_octave_lines);
792   float att=local_specmax+p->vi->ath_adjatt;
793   for(i=0;i<p->total_octave_lines;i++)seed[i]=NEGINF;
794   
795   /* set the ATH (floating below localmax, not global max by a
796      specified att) */
797   if(att<p->vi->ath_maxatt)att=p->vi->ath_maxatt;
798   
799   for(i=0;i<n;i++)
800     logmask[i]=p->ath[i]+att;
801
802   /* tone masking */
803   seed_loop(p,(const float ***)p->tonecurves,logfft,logmask,seed,global_specmax);
804   max_seeds(p,seed,logmask);
805
806 }
807
808 void _vp_offset_and_mix(vorbis_look_psy *p,
809                         float *noise,
810                         float *tone,
811                         int offset_select,
812                         float *logmask){
813   int i,n=p->n;
814   float toneatt=p->vi->tone_masteratt[offset_select];
815   
816   for(i=0;i<n;i++){
817     float val= noise[i]+p->noiseoffset[offset_select][i];
818     if(val>p->vi->noisemaxsupp)val=p->vi->noisemaxsupp;
819     logmask[i]=max(val,tone[i]+toneatt);
820   }
821 }
822
823 float _vp_ampmax_decay(float amp,vorbis_dsp_state *vd){
824   vorbis_info *vi=vd->vi;
825   codec_setup_info *ci=vi->codec_setup;
826   vorbis_info_psy_global *gi=&ci->psy_g_param;
827
828   int n=ci->blocksizes[vd->W]/2;
829   float secs=(float)n/vi->rate;
830
831   amp+=secs*gi->ampmax_att_per_sec;
832   if(amp<-9999)amp=-9999;
833   return(amp);
834 }
835
836 static void couple_lossless(float A, float B, 
837                             float *qA, float *qB){
838   int test1=fabs(*qA)>fabs(*qB);
839   test1-= fabs(*qA)<fabs(*qB);
840   
841   if(!test1)test1=((fabs(A)>fabs(B))<<1)-1;
842   if(test1==1){
843     *qB=(*qA>0.f?*qA-*qB:*qB-*qA);
844   }else{
845     float temp=*qB;  
846     *qB=(*qB>0.f?*qA-*qB:*qB-*qA);
847     *qA=temp;
848   }
849
850   if(*qB>fabs(*qA)*1.9999f){
851     *qB= -fabs(*qA)*2.f;
852     *qA= -*qA;
853   }
854 }
855
856 static float hypot_lookup[32]={
857   -0.009935, -0.011245, -0.012726, -0.014397, 
858   -0.016282, -0.018407, -0.020800, -0.023494, 
859   -0.026522, -0.029923, -0.033737, -0.038010, 
860   -0.042787, -0.048121, -0.054064, -0.060671, 
861   -0.068000, -0.076109, -0.085054, -0.094892, 
862   -0.105675, -0.117451, -0.130260, -0.144134, 
863   -0.159093, -0.175146, -0.192286, -0.210490, 
864   -0.229718, -0.249913, -0.271001, -0.292893};
865
866 static void precomputed_couple_point(float premag,
867                                      int floorA,int floorB,
868                                      float *mag, float *ang){
869   
870   int test=(floorA>floorB)-1;
871   int offset=31-abs(floorA-floorB);
872   float floormag=hypot_lookup[((offset<0)-1)&offset]+1.f;
873
874   floormag*=FLOOR1_fromdB_INV_LOOKUP[(floorB&test)|(floorA&(~test))];
875
876   *mag=premag*floormag;
877   *ang=0.f;
878 }
879
880 /* just like below, this is currently set up to only do
881    single-step-depth coupling.  Otherwise, we'd have to do more
882    copying (which will be inevitable later) */
883
884 /* doing the real circular magnitude calculation is audibly superior
885    to (A+B)/sqrt(2) */
886 static float cardoid_hypot(float a, float b){
887   if(a>0.){
888     if(b>0.)return sqrt(a*a+b*b);
889     if(a>-b)return sqrt(a*a-b*b);
890     return -sqrt(b*b-a*a);
891   }
892   if(b<0.)return -sqrt(a*a+b*b);
893   if(-a>b)return -sqrt(a*a-b*b);
894   return sqrt(b*b-a*a);
895 }
896
897 float **_vp_quantize_couple_memo(vorbis_block *vb,
898                                  vorbis_look_psy *p,
899                                  vorbis_info_mapping0 *vi,
900                                  float **mdct){
901   
902   int i,j,n=p->n;
903   float **ret=_vorbis_block_alloc(vb,vi->coupling_steps*sizeof(*ret));
904   
905   for(i=0;i<vi->coupling_steps;i++){
906     float *mdctM=mdct[vi->coupling_mag[i]];
907     float *mdctA=mdct[vi->coupling_ang[i]];
908     ret[i]=_vorbis_block_alloc(vb,n*sizeof(**ret));
909     for(j=0;j<n;j++)
910       ret[i][j]=cardoid_hypot(mdctM[j],mdctA[j]);
911   }
912
913   return(ret);
914 }
915
916 /* this is for per-channel noise normalization */
917 static int apsort(const void *a, const void *b){
918   if(fabs(**(float **)a)>fabs(**(float **)b))return -1;
919   return 1;
920 }
921
922 int **_vp_quantize_couple_sort(vorbis_block *vb,
923                                  vorbis_look_psy *p,
924                                  vorbis_info_mapping0 *vi,
925                                  float **mags){
926
927
928   if(p->vi->normal_point_p){
929     int i,j,k,n=p->n;
930     int **ret=_vorbis_block_alloc(vb,vi->coupling_steps*sizeof(*ret));
931     int partition=p->vi->normal_partition;
932     float **work=alloca(sizeof(*work)*partition);
933     
934     for(i=0;i<vi->coupling_steps;i++){
935       ret[i]=_vorbis_block_alloc(vb,n*sizeof(**ret));
936       
937       for(j=0;j<n;j+=partition){
938         for(k=0;k<partition;k++)work[k]=mags[i]+k+j;
939         qsort(work,partition,sizeof(*work),apsort);
940         for(k=0;k<partition;k++)ret[i][k+j]=work[k]-mags[i];
941       }
942     }
943     return(ret);
944   }
945   return(NULL);
946 }
947
948 void _vp_noise_normalize_sort(vorbis_look_psy *p,
949                               float *magnitudes,int *sortedindex){
950   int i,j,n=p->n;
951   vorbis_info_psy *vi=p->vi;
952   int partition=vi->normal_partition;
953   float **work=alloca(sizeof(*work)*partition);
954   int start=vi->normal_start;
955
956   for(j=start;j<n;j+=partition){
957     if(j+partition>n)partition=n-j;
958     for(i=0;i<partition;i++)work[i]=magnitudes+i+j;
959     qsort(work,partition,sizeof(*work),apsort);
960     for(i=0;i<partition;i++){
961       sortedindex[i+j-start]=work[i]-magnitudes;
962     }
963   }
964 }
965
966 void _vp_noise_normalize(vorbis_look_psy *p,
967                          float *in,float *out,int *sortedindex){
968   int flag=0,i,j=0,n=p->n;
969   vorbis_info_psy *vi=p->vi;
970   int partition=vi->normal_partition;
971   int start=vi->normal_start;
972
973   if(start>n)start=n;
974
975   if(vi->normal_channel_p){
976     for(;j<start;j++)
977       out[j]=rint(in[j]);
978     
979     for(;j+partition<=n;j+=partition){
980       float acc=0.;
981       int k;
982       
983       for(i=j;i<j+partition;i++)
984         acc+=in[i]*in[i];
985       
986       for(i=0;i<partition;i++){
987         k=sortedindex[i+j-start];
988         
989         if(in[k]*in[k]>=.25f){
990           out[k]=rint(in[k]);
991           acc-=in[k]*in[k];
992           flag=1;
993         }else{
994           if(acc<vi->normal_thresh)break;
995           out[k]=unitnorm(in[k]);
996           acc-=1.;
997         }
998       }
999       
1000       //if(!flag && i<3)i=0;
1001       for(;i<partition;i++){
1002         k=sortedindex[i+j-start];
1003         out[k]=0.;
1004       }
1005     }
1006   }
1007   
1008   for(;j<n;j++)
1009     out[j]=rint(in[j]);
1010   
1011 }
1012
1013 void _vp_couple(int blobno,
1014                 vorbis_info_psy_global *g,
1015                 vorbis_look_psy *p,
1016                 vorbis_info_mapping0 *vi,
1017                 float **res,
1018                 float **mag_memo,
1019                 int   **mag_sort,
1020                 int   **ifloor,
1021                 int   *nonzero,
1022                 int  sliding_lowpass){
1023
1024   int i,j,k,n=p->n;
1025
1026   /* perform any requested channel coupling */
1027   /* point stereo can only be used in a first stage (in this encoder)
1028      because of the dependency on floor lookups */
1029   for(i=0;i<vi->coupling_steps;i++){
1030
1031     /* once we're doing multistage coupling in which a channel goes
1032        through more than one coupling step, the floor vector
1033        magnitudes will also have to be recalculated an propogated
1034        along with PCM.  Right now, we're not (that will wait until 5.1
1035        most likely), so the code isn't here yet. The memory management
1036        here is all assuming single depth couplings anyway. */
1037
1038     /* make sure coupling a zero and a nonzero channel results in two
1039        nonzero channels. */
1040     if(nonzero[vi->coupling_mag[i]] ||
1041        nonzero[vi->coupling_ang[i]]){
1042      
1043
1044       float *rM=res[vi->coupling_mag[i]];
1045       float *rA=res[vi->coupling_ang[i]];
1046       float *qM=rM+n;
1047       float *qA=rA+n;
1048       int *floorM=ifloor[vi->coupling_mag[i]];
1049       int *floorA=ifloor[vi->coupling_ang[i]];
1050       float prepoint=stereo_threshholds[g->coupling_prepointamp[blobno]];
1051       float postpoint=stereo_threshholds[g->coupling_postpointamp[blobno]];
1052       int partition=(p->vi->normal_point_p?p->vi->normal_partition:p->n);
1053       int limit=g->coupling_pointlimit[p->vi->blockflag][blobno];
1054       int pointlimit=limit;
1055
1056       nonzero[vi->coupling_mag[i]]=1; 
1057       nonzero[vi->coupling_ang[i]]=1; 
1058
1059       for(j=0;j<p->n;j+=partition){
1060         float acc=0.f;
1061
1062         for(k=0;k<partition;k++){
1063           int l=k+j;
1064
1065           if(l<sliding_lowpass){
1066             if((l>=limit && fabs(rM[l])<postpoint && fabs(rA[l])<postpoint) ||
1067                (fabs(rM[l])<prepoint && fabs(rA[l])<prepoint)){
1068               precomputed_couple_point(mag_memo[i][l],
1069                                        floorM[l],floorA[l],
1070                                        qM+l,qA+l);
1071               if(rint(qM[l])==0.f)acc+=qM[l]*qM[l];
1072             }else{
1073               couple_lossless(rM[l],rA[l],qM+l,qA+l);
1074             }
1075           }else{
1076             qM[l]=0.;
1077             qA[l]=0.;
1078           }
1079         }
1080         
1081         if(p->vi->normal_point_p){
1082           for(k=0;k<partition && acc>=p->vi->normal_thresh;k++){
1083             int l=mag_sort[i][j+k];
1084             if(l<sliding_lowpass && l>=pointlimit && rint(qM[l])==0.f){
1085               qM[l]=unitnorm(qM[l]);
1086               acc-=1.f;
1087             }
1088           } 
1089         }
1090       }
1091     }
1092   }
1093 }
1094