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