Chris@1: /********************************************************************
Chris@1:  *                                                                  *
Chris@1:  * THIS FILE IS PART OF THE OggVorbis SOFTWARE CODEC SOURCE CODE.   *
Chris@1:  * USE, DISTRIBUTION AND REPRODUCTION OF THIS LIBRARY SOURCE IS     *
Chris@1:  * GOVERNED BY A BSD-STYLE SOURCE LICENSE INCLUDED WITH THIS SOURCE *
Chris@1:  * IN 'COPYING'. PLEASE READ THESE TERMS BEFORE DISTRIBUTING.       *
Chris@1:  *                                                                  *
Chris@1:  * THE OggVorbis SOURCE CODE IS (C) COPYRIGHT 1994-2010             *
Chris@1:  * by the Xiph.Org Foundation http://www.xiph.org/                  *
Chris@1:  *                                                                  *
Chris@1:  ********************************************************************
Chris@1: 
Chris@1:  function: psychoacoustics not including preecho
Chris@1:  last mod: $Id: psy.c 18077 2011-09-02 02:49:00Z giles $
Chris@1: 
Chris@1:  ********************************************************************/
Chris@1: 
Chris@1: #include <stdlib.h>
Chris@1: #include <math.h>
Chris@1: #include <string.h>
Chris@1: #include "vorbis/codec.h"
Chris@1: #include "codec_internal.h"
Chris@1: 
Chris@1: #include "masking.h"
Chris@1: #include "psy.h"
Chris@1: #include "os.h"
Chris@1: #include "lpc.h"
Chris@1: #include "smallft.h"
Chris@1: #include "scales.h"
Chris@1: #include "misc.h"
Chris@1: 
Chris@1: #define NEGINF -9999.f
Chris@1: static const double stereo_threshholds[]={0.0, .5, 1.0, 1.5, 2.5, 4.5, 8.5, 16.5, 9e10};
Chris@1: static const double stereo_threshholds_limited[]={0.0, .5, 1.0, 1.5, 2.0, 2.5, 4.5, 8.5, 9e10};
Chris@1: 
Chris@1: vorbis_look_psy_global *_vp_global_look(vorbis_info *vi){
Chris@1:   codec_setup_info *ci=vi->codec_setup;
Chris@1:   vorbis_info_psy_global *gi=&ci->psy_g_param;
Chris@1:   vorbis_look_psy_global *look=_ogg_calloc(1,sizeof(*look));
Chris@1: 
Chris@1:   look->channels=vi->channels;
Chris@1: 
Chris@1:   look->ampmax=-9999.;
Chris@1:   look->gi=gi;
Chris@1:   return(look);
Chris@1: }
Chris@1: 
Chris@1: void _vp_global_free(vorbis_look_psy_global *look){
Chris@1:   if(look){
Chris@1:     memset(look,0,sizeof(*look));
Chris@1:     _ogg_free(look);
Chris@1:   }
Chris@1: }
Chris@1: 
Chris@1: void _vi_gpsy_free(vorbis_info_psy_global *i){
Chris@1:   if(i){
Chris@1:     memset(i,0,sizeof(*i));
Chris@1:     _ogg_free(i);
Chris@1:   }
Chris@1: }
Chris@1: 
Chris@1: void _vi_psy_free(vorbis_info_psy *i){
Chris@1:   if(i){
Chris@1:     memset(i,0,sizeof(*i));
Chris@1:     _ogg_free(i);
Chris@1:   }
Chris@1: }
Chris@1: 
Chris@1: static void min_curve(float *c,
Chris@1:                        float *c2){
Chris@1:   int i;
Chris@1:   for(i=0;i<EHMER_MAX;i++)if(c2[i]<c[i])c[i]=c2[i];
Chris@1: }
Chris@1: static void max_curve(float *c,
Chris@1:                        float *c2){
Chris@1:   int i;
Chris@1:   for(i=0;i<EHMER_MAX;i++)if(c2[i]>c[i])c[i]=c2[i];
Chris@1: }
Chris@1: 
Chris@1: static void attenuate_curve(float *c,float att){
Chris@1:   int i;
Chris@1:   for(i=0;i<EHMER_MAX;i++)
Chris@1:     c[i]+=att;
Chris@1: }
Chris@1: 
Chris@1: static float ***setup_tone_curves(float curveatt_dB[P_BANDS],float binHz,int n,
Chris@1:                                   float center_boost, float center_decay_rate){
Chris@1:   int i,j,k,m;
Chris@1:   float ath[EHMER_MAX];
Chris@1:   float workc[P_BANDS][P_LEVELS][EHMER_MAX];
Chris@1:   float athc[P_LEVELS][EHMER_MAX];
Chris@1:   float *brute_buffer=alloca(n*sizeof(*brute_buffer));
Chris@1: 
Chris@1:   float ***ret=_ogg_malloc(sizeof(*ret)*P_BANDS);
Chris@1: 
Chris@1:   memset(workc,0,sizeof(workc));
Chris@1: 
Chris@1:   for(i=0;i<P_BANDS;i++){
Chris@1:     /* we add back in the ATH to avoid low level curves falling off to
Chris@1:        -infinity and unnecessarily cutting off high level curves in the
Chris@1:        curve limiting (last step). */
Chris@1: 
Chris@1:     /* A half-band's settings must be valid over the whole band, and
Chris@1:        it's better to mask too little than too much */
Chris@1:     int ath_offset=i*4;
Chris@1:     for(j=0;j<EHMER_MAX;j++){
Chris@1:       float min=999.;
Chris@1:       for(k=0;k<4;k++)
Chris@1:         if(j+k+ath_offset<MAX_ATH){
Chris@1:           if(min>ATH[j+k+ath_offset])min=ATH[j+k+ath_offset];
Chris@1:         }else{
Chris@1:           if(min>ATH[MAX_ATH-1])min=ATH[MAX_ATH-1];
Chris@1:         }
Chris@1:       ath[j]=min;
Chris@1:     }
Chris@1: 
Chris@1:     /* copy curves into working space, replicate the 50dB curve to 30
Chris@1:        and 40, replicate the 100dB curve to 110 */
Chris@1:     for(j=0;j<6;j++)
Chris@1:       memcpy(workc[i][j+2],tonemasks[i][j],EHMER_MAX*sizeof(*tonemasks[i][j]));
Chris@1:     memcpy(workc[i][0],tonemasks[i][0],EHMER_MAX*sizeof(*tonemasks[i][0]));
Chris@1:     memcpy(workc[i][1],tonemasks[i][0],EHMER_MAX*sizeof(*tonemasks[i][0]));
Chris@1: 
Chris@1:     /* apply centered curve boost/decay */
Chris@1:     for(j=0;j<P_LEVELS;j++){
Chris@1:       for(k=0;k<EHMER_MAX;k++){
Chris@1:         float adj=center_boost+abs(EHMER_OFFSET-k)*center_decay_rate;
Chris@1:         if(adj<0. && center_boost>0)adj=0.;
Chris@1:         if(adj>0. && center_boost<0)adj=0.;
Chris@1:         workc[i][j][k]+=adj;
Chris@1:       }
Chris@1:     }
Chris@1: 
Chris@1:     /* normalize curves so the driving amplitude is 0dB */
Chris@1:     /* make temp curves with the ATH overlayed */
Chris@1:     for(j=0;j<P_LEVELS;j++){
Chris@1:       attenuate_curve(workc[i][j],curveatt_dB[i]+100.-(j<2?2:j)*10.-P_LEVEL_0);
Chris@1:       memcpy(athc[j],ath,EHMER_MAX*sizeof(**athc));
Chris@1:       attenuate_curve(athc[j],+100.-j*10.f-P_LEVEL_0);
Chris@1:       max_curve(athc[j],workc[i][j]);
Chris@1:     }
Chris@1: 
Chris@1:     /* Now limit the louder curves.
Chris@1: 
Chris@1:        the idea is this: We don't know what the playback attenuation
Chris@1:        will be; 0dB SL moves every time the user twiddles the volume
Chris@1:        knob. So that means we have to use a single 'most pessimal' curve
Chris@1:        for all masking amplitudes, right?  Wrong.  The *loudest* sound
Chris@1:        can be in (we assume) a range of ...+100dB] SL.  However, sounds
Chris@1:        20dB down will be in a range ...+80], 40dB down is from ...+60],
Chris@1:        etc... */
Chris@1: 
Chris@1:     for(j=1;j<P_LEVELS;j++){
Chris@1:       min_curve(athc[j],athc[j-1]);
Chris@1:       min_curve(workc[i][j],athc[j]);
Chris@1:     }
Chris@1:   }
Chris@1: 
Chris@1:   for(i=0;i<P_BANDS;i++){
Chris@1:     int hi_curve,lo_curve,bin;
Chris@1:     ret[i]=_ogg_malloc(sizeof(**ret)*P_LEVELS);
Chris@1: 
Chris@1:     /* low frequency curves are measured with greater resolution than
Chris@1:        the MDCT/FFT will actually give us; we want the curve applied
Chris@1:        to the tone data to be pessimistic and thus apply the minimum
Chris@1:        masking possible for a given bin.  That means that a single bin
Chris@1:        could span more than one octave and that the curve will be a
Chris@1:        composite of multiple octaves.  It also may mean that a single
Chris@1:        bin may span > an eighth of an octave and that the eighth
Chris@1:        octave values may also be composited. */
Chris@1: 
Chris@1:     /* which octave curves will we be compositing? */
Chris@1:     bin=floor(fromOC(i*.5)/binHz);
Chris@1:     lo_curve=  ceil(toOC(bin*binHz+1)*2);
Chris@1:     hi_curve=  floor(toOC((bin+1)*binHz)*2);
Chris@1:     if(lo_curve>i)lo_curve=i;
Chris@1:     if(lo_curve<0)lo_curve=0;
Chris@1:     if(hi_curve>=P_BANDS)hi_curve=P_BANDS-1;
Chris@1: 
Chris@1:     for(m=0;m<P_LEVELS;m++){
Chris@1:       ret[i][m]=_ogg_malloc(sizeof(***ret)*(EHMER_MAX+2));
Chris@1: 
Chris@1:       for(j=0;j<n;j++)brute_buffer[j]=999.;
Chris@1: 
Chris@1:       /* render the curve into bins, then pull values back into curve.
Chris@1:          The point is that any inherent subsampling aliasing results in
Chris@1:          a safe minimum */
Chris@1:       for(k=lo_curve;k<=hi_curve;k++){
Chris@1:         int l=0;
Chris@1: 
Chris@1:         for(j=0;j<EHMER_MAX;j++){
Chris@1:           int lo_bin= fromOC(j*.125+k*.5-2.0625)/binHz;
Chris@1:           int hi_bin= fromOC(j*.125+k*.5-1.9375)/binHz+1;
Chris@1: 
Chris@1:           if(lo_bin<0)lo_bin=0;
Chris@1:           if(lo_bin>n)lo_bin=n;
Chris@1:           if(lo_bin<l)l=lo_bin;
Chris@1:           if(hi_bin<0)hi_bin=0;
Chris@1:           if(hi_bin>n)hi_bin=n;
Chris@1: 
Chris@1:           for(;l<hi_bin && l<n;l++)
Chris@1:             if(brute_buffer[l]>workc[k][m][j])
Chris@1:               brute_buffer[l]=workc[k][m][j];
Chris@1:         }
Chris@1: 
Chris@1:         for(;l<n;l++)
Chris@1:           if(brute_buffer[l]>workc[k][m][EHMER_MAX-1])
Chris@1:             brute_buffer[l]=workc[k][m][EHMER_MAX-1];
Chris@1: 
Chris@1:       }
Chris@1: 
Chris@1:       /* be equally paranoid about being valid up to next half ocatve */
Chris@1:       if(i+1<P_BANDS){
Chris@1:         int l=0;
Chris@1:         k=i+1;
Chris@1:         for(j=0;j<EHMER_MAX;j++){
Chris@1:           int lo_bin= fromOC(j*.125+i*.5-2.0625)/binHz;
Chris@1:           int hi_bin= fromOC(j*.125+i*.5-1.9375)/binHz+1;
Chris@1: 
Chris@1:           if(lo_bin<0)lo_bin=0;
Chris@1:           if(lo_bin>n)lo_bin=n;
Chris@1:           if(lo_bin<l)l=lo_bin;
Chris@1:           if(hi_bin<0)hi_bin=0;
Chris@1:           if(hi_bin>n)hi_bin=n;
Chris@1: 
Chris@1:           for(;l<hi_bin && l<n;l++)
Chris@1:             if(brute_buffer[l]>workc[k][m][j])
Chris@1:               brute_buffer[l]=workc[k][m][j];
Chris@1:         }
Chris@1: 
Chris@1:         for(;l<n;l++)
Chris@1:           if(brute_buffer[l]>workc[k][m][EHMER_MAX-1])
Chris@1:             brute_buffer[l]=workc[k][m][EHMER_MAX-1];
Chris@1: 
Chris@1:       }
Chris@1: 
Chris@1: 
Chris@1:       for(j=0;j<EHMER_MAX;j++){
Chris@1:         int bin=fromOC(j*.125+i*.5-2.)/binHz;
Chris@1:         if(bin<0){
Chris@1:           ret[i][m][j+2]=-999.;
Chris@1:         }else{
Chris@1:           if(bin>=n){
Chris@1:             ret[i][m][j+2]=-999.;
Chris@1:           }else{
Chris@1:             ret[i][m][j+2]=brute_buffer[bin];
Chris@1:           }
Chris@1:         }
Chris@1:       }
Chris@1: 
Chris@1:       /* add fenceposts */
Chris@1:       for(j=0;j<EHMER_OFFSET;j++)
Chris@1:         if(ret[i][m][j+2]>-200.f)break;
Chris@1:       ret[i][m][0]=j;
Chris@1: 
Chris@1:       for(j=EHMER_MAX-1;j>EHMER_OFFSET+1;j--)
Chris@1:         if(ret[i][m][j+2]>-200.f)
Chris@1:           break;
Chris@1:       ret[i][m][1]=j;
Chris@1: 
Chris@1:     }
Chris@1:   }
Chris@1: 
Chris@1:   return(ret);
Chris@1: }
Chris@1: 
Chris@1: void _vp_psy_init(vorbis_look_psy *p,vorbis_info_psy *vi,
Chris@1:                   vorbis_info_psy_global *gi,int n,long rate){
Chris@1:   long i,j,lo=-99,hi=1;
Chris@1:   long maxoc;
Chris@1:   memset(p,0,sizeof(*p));
Chris@1: 
Chris@1:   p->eighth_octave_lines=gi->eighth_octave_lines;
Chris@1:   p->shiftoc=rint(log(gi->eighth_octave_lines*8.f)/log(2.f))-1;
Chris@1: 
Chris@1:   p->firstoc=toOC(.25f*rate*.5/n)*(1<<(p->shiftoc+1))-gi->eighth_octave_lines;
Chris@1:   maxoc=toOC((n+.25f)*rate*.5/n)*(1<<(p->shiftoc+1))+.5f;
Chris@1:   p->total_octave_lines=maxoc-p->firstoc+1;
Chris@1:   p->ath=_ogg_malloc(n*sizeof(*p->ath));
Chris@1: 
Chris@1:   p->octave=_ogg_malloc(n*sizeof(*p->octave));
Chris@1:   p->bark=_ogg_malloc(n*sizeof(*p->bark));
Chris@1:   p->vi=vi;
Chris@1:   p->n=n;
Chris@1:   p->rate=rate;
Chris@1: 
Chris@1:   /* AoTuV HF weighting */
Chris@1:   p->m_val = 1.;
Chris@1:   if(rate < 26000) p->m_val = 0;
Chris@1:   else if(rate < 38000) p->m_val = .94;   /* 32kHz */
Chris@1:   else if(rate > 46000) p->m_val = 1.275; /* 48kHz */
Chris@1: 
Chris@1:   /* set up the lookups for a given blocksize and sample rate */
Chris@1: 
Chris@1:   for(i=0,j=0;i<MAX_ATH-1;i++){
Chris@1:     int endpos=rint(fromOC((i+1)*.125-2.)*2*n/rate);
Chris@1:     float base=ATH[i];
Chris@1:     if(j<endpos){
Chris@1:       float delta=(ATH[i+1]-base)/(endpos-j);
Chris@1:       for(;j<endpos && j<n;j++){
Chris@1:         p->ath[j]=base+100.;
Chris@1:         base+=delta;
Chris@1:       }
Chris@1:     }
Chris@1:   }
Chris@1: 
Chris@1:   for(;j<n;j++){
Chris@1:     p->ath[j]=p->ath[j-1];
Chris@1:   }
Chris@1: 
Chris@1:   for(i=0;i<n;i++){
Chris@1:     float bark=toBARK(rate/(2*n)*i);
Chris@1: 
Chris@1:     for(;lo+vi->noisewindowlomin<i &&
Chris@1:           toBARK(rate/(2*n)*lo)<(bark-vi->noisewindowlo);lo++);
Chris@1: 
Chris@1:     for(;hi<=n && (hi<i+vi->noisewindowhimin ||
Chris@1:           toBARK(rate/(2*n)*hi)<(bark+vi->noisewindowhi));hi++);
Chris@1: 
Chris@1:     p->bark[i]=((lo-1)<<16)+(hi-1);
Chris@1: 
Chris@1:   }
Chris@1: 
Chris@1:   for(i=0;i<n;i++)
Chris@1:     p->octave[i]=toOC((i+.25f)*.5*rate/n)*(1<<(p->shiftoc+1))+.5f;
Chris@1: 
Chris@1:   p->tonecurves=setup_tone_curves(vi->toneatt,rate*.5/n,n,
Chris@1:                                   vi->tone_centerboost,vi->tone_decay);
Chris@1: 
Chris@1:   /* set up rolling noise median */
Chris@1:   p->noiseoffset=_ogg_malloc(P_NOISECURVES*sizeof(*p->noiseoffset));
Chris@1:   for(i=0;i<P_NOISECURVES;i++)
Chris@1:     p->noiseoffset[i]=_ogg_malloc(n*sizeof(**p->noiseoffset));
Chris@1: 
Chris@1:   for(i=0;i<n;i++){
Chris@1:     float halfoc=toOC((i+.5)*rate/(2.*n))*2.;
Chris@1:     int inthalfoc;
Chris@1:     float del;
Chris@1: 
Chris@1:     if(halfoc<0)halfoc=0;
Chris@1:     if(halfoc>=P_BANDS-1)halfoc=P_BANDS-1;
Chris@1:     inthalfoc=(int)halfoc;
Chris@1:     del=halfoc-inthalfoc;
Chris@1: 
Chris@1:     for(j=0;j<P_NOISECURVES;j++)
Chris@1:       p->noiseoffset[j][i]=
Chris@1:         p->vi->noiseoff[j][inthalfoc]*(1.-del) +
Chris@1:         p->vi->noiseoff[j][inthalfoc+1]*del;
Chris@1: 
Chris@1:   }
Chris@1: #if 0
Chris@1:   {
Chris@1:     static int ls=0;
Chris@1:     _analysis_output_always("noiseoff0",ls,p->noiseoffset[0],n,1,0,0);
Chris@1:     _analysis_output_always("noiseoff1",ls,p->noiseoffset[1],n,1,0,0);
Chris@1:     _analysis_output_always("noiseoff2",ls++,p->noiseoffset[2],n,1,0,0);
Chris@1:   }
Chris@1: #endif
Chris@1: }
Chris@1: 
Chris@1: void _vp_psy_clear(vorbis_look_psy *p){
Chris@1:   int i,j;
Chris@1:   if(p){
Chris@1:     if(p->ath)_ogg_free(p->ath);
Chris@1:     if(p->octave)_ogg_free(p->octave);
Chris@1:     if(p->bark)_ogg_free(p->bark);
Chris@1:     if(p->tonecurves){
Chris@1:       for(i=0;i<P_BANDS;i++){
Chris@1:         for(j=0;j<P_LEVELS;j++){
Chris@1:           _ogg_free(p->tonecurves[i][j]);
Chris@1:         }
Chris@1:         _ogg_free(p->tonecurves[i]);
Chris@1:       }
Chris@1:       _ogg_free(p->tonecurves);
Chris@1:     }
Chris@1:     if(p->noiseoffset){
Chris@1:       for(i=0;i<P_NOISECURVES;i++){
Chris@1:         _ogg_free(p->noiseoffset[i]);
Chris@1:       }
Chris@1:       _ogg_free(p->noiseoffset);
Chris@1:     }
Chris@1:     memset(p,0,sizeof(*p));
Chris@1:   }
Chris@1: }
Chris@1: 
Chris@1: /* octave/(8*eighth_octave_lines) x scale and dB y scale */
Chris@1: static void seed_curve(float *seed,
Chris@1:                        const float **curves,
Chris@1:                        float amp,
Chris@1:                        int oc, int n,
Chris@1:                        int linesper,float dBoffset){
Chris@1:   int i,post1;
Chris@1:   int seedptr;
Chris@1:   const float *posts,*curve;
Chris@1: 
Chris@1:   int choice=(int)((amp+dBoffset-P_LEVEL_0)*.1f);
Chris@1:   choice=max(choice,0);
Chris@1:   choice=min(choice,P_LEVELS-1);
Chris@1:   posts=curves[choice];
Chris@1:   curve=posts+2;
Chris@1:   post1=(int)posts[1];
Chris@1:   seedptr=oc+(posts[0]-EHMER_OFFSET)*linesper-(linesper>>1);
Chris@1: 
Chris@1:   for(i=posts[0];i<post1;i++){
Chris@1:     if(seedptr>0){
Chris@1:       float lin=amp+curve[i];
Chris@1:       if(seed[seedptr]<lin)seed[seedptr]=lin;
Chris@1:     }
Chris@1:     seedptr+=linesper;
Chris@1:     if(seedptr>=n)break;
Chris@1:   }
Chris@1: }
Chris@1: 
Chris@1: static void seed_loop(vorbis_look_psy *p,
Chris@1:                       const float ***curves,
Chris@1:                       const float *f,
Chris@1:                       const float *flr,
Chris@1:                       float *seed,
Chris@1:                       float specmax){
Chris@1:   vorbis_info_psy *vi=p->vi;
Chris@1:   long n=p->n,i;
Chris@1:   float dBoffset=vi->max_curve_dB-specmax;
Chris@1: 
Chris@1:   /* prime the working vector with peak values */
Chris@1: 
Chris@1:   for(i=0;i<n;i++){
Chris@1:     float max=f[i];
Chris@1:     long oc=p->octave[i];
Chris@1:     while(i+1<n && p->octave[i+1]==oc){
Chris@1:       i++;
Chris@1:       if(f[i]>max)max=f[i];
Chris@1:     }
Chris@1: 
Chris@1:     if(max+6.f>flr[i]){
Chris@1:       oc=oc>>p->shiftoc;
Chris@1: 
Chris@1:       if(oc>=P_BANDS)oc=P_BANDS-1;
Chris@1:       if(oc<0)oc=0;
Chris@1: 
Chris@1:       seed_curve(seed,
Chris@1:                  curves[oc],
Chris@1:                  max,
Chris@1:                  p->octave[i]-p->firstoc,
Chris@1:                  p->total_octave_lines,
Chris@1:                  p->eighth_octave_lines,
Chris@1:                  dBoffset);
Chris@1:     }
Chris@1:   }
Chris@1: }
Chris@1: 
Chris@1: static void seed_chase(float *seeds, int linesper, long n){
Chris@1:   long  *posstack=alloca(n*sizeof(*posstack));
Chris@1:   float *ampstack=alloca(n*sizeof(*ampstack));
Chris@1:   long   stack=0;
Chris@1:   long   pos=0;
Chris@1:   long   i;
Chris@1: 
Chris@1:   for(i=0;i<n;i++){
Chris@1:     if(stack<2){
Chris@1:       posstack[stack]=i;
Chris@1:       ampstack[stack++]=seeds[i];
Chris@1:     }else{
Chris@1:       while(1){
Chris@1:         if(seeds[i]<ampstack[stack-1]){
Chris@1:           posstack[stack]=i;
Chris@1:           ampstack[stack++]=seeds[i];
Chris@1:           break;
Chris@1:         }else{
Chris@1:           if(i<posstack[stack-1]+linesper){
Chris@1:             if(stack>1 && ampstack[stack-1]<=ampstack[stack-2] &&
Chris@1:                i<posstack[stack-2]+linesper){
Chris@1:               /* we completely overlap, making stack-1 irrelevant.  pop it */
Chris@1:               stack--;
Chris@1:               continue;
Chris@1:             }
Chris@1:           }
Chris@1:           posstack[stack]=i;
Chris@1:           ampstack[stack++]=seeds[i];
Chris@1:           break;
Chris@1: 
Chris@1:         }
Chris@1:       }
Chris@1:     }
Chris@1:   }
Chris@1: 
Chris@1:   /* the stack now contains only the positions that are relevant. Scan
Chris@1:      'em straight through */
Chris@1: 
Chris@1:   for(i=0;i<stack;i++){
Chris@1:     long endpos;
Chris@1:     if(i<stack-1 && ampstack[i+1]>ampstack[i]){
Chris@1:       endpos=posstack[i+1];
Chris@1:     }else{
Chris@1:       endpos=posstack[i]+linesper+1; /* +1 is important, else bin 0 is
Chris@1:                                         discarded in short frames */
Chris@1:     }
Chris@1:     if(endpos>n)endpos=n;
Chris@1:     for(;pos<endpos;pos++)
Chris@1:       seeds[pos]=ampstack[i];
Chris@1:   }
Chris@1: 
Chris@1:   /* there.  Linear time.  I now remember this was on a problem set I
Chris@1:      had in Grad Skool... I didn't solve it at the time ;-) */
Chris@1: 
Chris@1: }
Chris@1: 
Chris@1: /* bleaugh, this is more complicated than it needs to be */
Chris@1: #include<stdio.h>
Chris@1: static void max_seeds(vorbis_look_psy *p,
Chris@1:                       float *seed,
Chris@1:                       float *flr){
Chris@1:   long   n=p->total_octave_lines;
Chris@1:   int    linesper=p->eighth_octave_lines;
Chris@1:   long   linpos=0;
Chris@1:   long   pos;
Chris@1: 
Chris@1:   seed_chase(seed,linesper,n); /* for masking */
Chris@1: 
Chris@1:   pos=p->octave[0]-p->firstoc-(linesper>>1);
Chris@1: 
Chris@1:   while(linpos+1<p->n){
Chris@1:     float minV=seed[pos];
Chris@1:     long end=((p->octave[linpos]+p->octave[linpos+1])>>1)-p->firstoc;
Chris@1:     if(minV>p->vi->tone_abs_limit)minV=p->vi->tone_abs_limit;
Chris@1:     while(pos+1<=end){
Chris@1:       pos++;
Chris@1:       if((seed[pos]>NEGINF && seed[pos]<minV) || minV==NEGINF)
Chris@1:         minV=seed[pos];
Chris@1:     }
Chris@1: 
Chris@1:     end=pos+p->firstoc;
Chris@1:     for(;linpos<p->n && p->octave[linpos]<=end;linpos++)
Chris@1:       if(flr[linpos]<minV)flr[linpos]=minV;
Chris@1:   }
Chris@1: 
Chris@1:   {
Chris@1:     float minV=seed[p->total_octave_lines-1];
Chris@1:     for(;linpos<p->n;linpos++)
Chris@1:       if(flr[linpos]<minV)flr[linpos]=minV;
Chris@1:   }
Chris@1: 
Chris@1: }
Chris@1: 
Chris@1: static void bark_noise_hybridmp(int n,const long *b,
Chris@1:                                 const float *f,
Chris@1:                                 float *noise,
Chris@1:                                 const float offset,
Chris@1:                                 const int fixed){
Chris@1: 
Chris@1:   float *N=alloca(n*sizeof(*N));
Chris@1:   float *X=alloca(n*sizeof(*N));
Chris@1:   float *XX=alloca(n*sizeof(*N));
Chris@1:   float *Y=alloca(n*sizeof(*N));
Chris@1:   float *XY=alloca(n*sizeof(*N));
Chris@1: 
Chris@1:   float tN, tX, tXX, tY, tXY;
Chris@1:   int i;
Chris@1: 
Chris@1:   int lo, hi;
Chris@1:   float R=0.f;
Chris@1:   float A=0.f;
Chris@1:   float B=0.f;
Chris@1:   float D=1.f;
Chris@1:   float w, x, y;
Chris@1: 
Chris@1:   tN = tX = tXX = tY = tXY = 0.f;
Chris@1: 
Chris@1:   y = f[0] + offset;
Chris@1:   if (y < 1.f) y = 1.f;
Chris@1: 
Chris@1:   w = y * y * .5;
Chris@1: 
Chris@1:   tN += w;
Chris@1:   tX += w;
Chris@1:   tY += w * y;
Chris@1: 
Chris@1:   N[0] = tN;
Chris@1:   X[0] = tX;
Chris@1:   XX[0] = tXX;
Chris@1:   Y[0] = tY;
Chris@1:   XY[0] = tXY;
Chris@1: 
Chris@1:   for (i = 1, x = 1.f; i < n; i++, x += 1.f) {
Chris@1: 
Chris@1:     y = f[i] + offset;
Chris@1:     if (y < 1.f) y = 1.f;
Chris@1: 
Chris@1:     w = y * y;
Chris@1: 
Chris@1:     tN += w;
Chris@1:     tX += w * x;
Chris@1:     tXX += w * x * x;
Chris@1:     tY += w * y;
Chris@1:     tXY += w * x * y;
Chris@1: 
Chris@1:     N[i] = tN;
Chris@1:     X[i] = tX;
Chris@1:     XX[i] = tXX;
Chris@1:     Y[i] = tY;
Chris@1:     XY[i] = tXY;
Chris@1:   }
Chris@1: 
Chris@1:   for (i = 0, x = 0.f;; i++, x += 1.f) {
Chris@1: 
Chris@1:     lo = b[i] >> 16;
Chris@1:     if( lo>=0 ) break;
Chris@1:     hi = b[i] & 0xffff;
Chris@1: 
Chris@1:     tN = N[hi] + N[-lo];
Chris@1:     tX = X[hi] - X[-lo];
Chris@1:     tXX = XX[hi] + XX[-lo];
Chris@1:     tY = Y[hi] + Y[-lo];
Chris@1:     tXY = XY[hi] - XY[-lo];
Chris@1: 
Chris@1:     A = tY * tXX - tX * tXY;
Chris@1:     B = tN * tXY - tX * tY;
Chris@1:     D = tN * tXX - tX * tX;
Chris@1:     R = (A + x * B) / D;
Chris@1:     if (R < 0.f)
Chris@1:       R = 0.f;
Chris@1: 
Chris@1:     noise[i] = R - offset;
Chris@1:   }
Chris@1: 
Chris@1:   for ( ;; i++, x += 1.f) {
Chris@1: 
Chris@1:     lo = b[i] >> 16;
Chris@1:     hi = b[i] & 0xffff;
Chris@1:     if(hi>=n)break;
Chris@1: 
Chris@1:     tN = N[hi] - N[lo];
Chris@1:     tX = X[hi] - X[lo];
Chris@1:     tXX = XX[hi] - XX[lo];
Chris@1:     tY = Y[hi] - Y[lo];
Chris@1:     tXY = XY[hi] - XY[lo];
Chris@1: 
Chris@1:     A = tY * tXX - tX * tXY;
Chris@1:     B = tN * tXY - tX * tY;
Chris@1:     D = tN * tXX - tX * tX;
Chris@1:     R = (A + x * B) / D;
Chris@1:     if (R < 0.f) R = 0.f;
Chris@1: 
Chris@1:     noise[i] = R - offset;
Chris@1:   }
Chris@1:   for ( ; i < n; i++, x += 1.f) {
Chris@1: 
Chris@1:     R = (A + x * B) / D;
Chris@1:     if (R < 0.f) R = 0.f;
Chris@1: 
Chris@1:     noise[i] = R - offset;
Chris@1:   }
Chris@1: 
Chris@1:   if (fixed <= 0) return;
Chris@1: 
Chris@1:   for (i = 0, x = 0.f;; i++, x += 1.f) {
Chris@1:     hi = i + fixed / 2;
Chris@1:     lo = hi - fixed;
Chris@1:     if(lo>=0)break;
Chris@1: 
Chris@1:     tN = N[hi] + N[-lo];
Chris@1:     tX = X[hi] - X[-lo];
Chris@1:     tXX = XX[hi] + XX[-lo];
Chris@1:     tY = Y[hi] + Y[-lo];
Chris@1:     tXY = XY[hi] - XY[-lo];
Chris@1: 
Chris@1: 
Chris@1:     A = tY * tXX - tX * tXY;
Chris@1:     B = tN * tXY - tX * tY;
Chris@1:     D = tN * tXX - tX * tX;
Chris@1:     R = (A + x * B) / D;
Chris@1: 
Chris@1:     if (R - offset < noise[i]) noise[i] = R - offset;
Chris@1:   }
Chris@1:   for ( ;; i++, x += 1.f) {
Chris@1: 
Chris@1:     hi = i + fixed / 2;
Chris@1:     lo = hi - fixed;
Chris@1:     if(hi>=n)break;
Chris@1: 
Chris@1:     tN = N[hi] - N[lo];
Chris@1:     tX = X[hi] - X[lo];
Chris@1:     tXX = XX[hi] - XX[lo];
Chris@1:     tY = Y[hi] - Y[lo];
Chris@1:     tXY = XY[hi] - XY[lo];
Chris@1: 
Chris@1:     A = tY * tXX - tX * tXY;
Chris@1:     B = tN * tXY - tX * tY;
Chris@1:     D = tN * tXX - tX * tX;
Chris@1:     R = (A + x * B) / D;
Chris@1: 
Chris@1:     if (R - offset < noise[i]) noise[i] = R - offset;
Chris@1:   }
Chris@1:   for ( ; i < n; i++, x += 1.f) {
Chris@1:     R = (A + x * B) / D;
Chris@1:     if (R - offset < noise[i]) noise[i] = R - offset;
Chris@1:   }
Chris@1: }
Chris@1: 
Chris@1: void _vp_noisemask(vorbis_look_psy *p,
Chris@1:                    float *logmdct,
Chris@1:                    float *logmask){
Chris@1: 
Chris@1:   int i,n=p->n;
Chris@1:   float *work=alloca(n*sizeof(*work));
Chris@1: 
Chris@1:   bark_noise_hybridmp(n,p->bark,logmdct,logmask,
Chris@1:                       140.,-1);
Chris@1: 
Chris@1:   for(i=0;i<n;i++)work[i]=logmdct[i]-logmask[i];
Chris@1: 
Chris@1:   bark_noise_hybridmp(n,p->bark,work,logmask,0.,
Chris@1:                       p->vi->noisewindowfixed);
Chris@1: 
Chris@1:   for(i=0;i<n;i++)work[i]=logmdct[i]-work[i];
Chris@1: 
Chris@1: #if 0
Chris@1:   {
Chris@1:     static int seq=0;
Chris@1: 
Chris@1:     float work2[n];
Chris@1:     for(i=0;i<n;i++){
Chris@1:       work2[i]=logmask[i]+work[i];
Chris@1:     }
Chris@1: 
Chris@1:     if(seq&1)
Chris@1:       _analysis_output("median2R",seq/2,work,n,1,0,0);
Chris@1:     else
Chris@1:       _analysis_output("median2L",seq/2,work,n,1,0,0);
Chris@1: 
Chris@1:     if(seq&1)
Chris@1:       _analysis_output("envelope2R",seq/2,work2,n,1,0,0);
Chris@1:     else
Chris@1:       _analysis_output("envelope2L",seq/2,work2,n,1,0,0);
Chris@1:     seq++;
Chris@1:   }
Chris@1: #endif
Chris@1: 
Chris@1:   for(i=0;i<n;i++){
Chris@1:     int dB=logmask[i]+.5;
Chris@1:     if(dB>=NOISE_COMPAND_LEVELS)dB=NOISE_COMPAND_LEVELS-1;
Chris@1:     if(dB<0)dB=0;
Chris@1:     logmask[i]= work[i]+p->vi->noisecompand[dB];
Chris@1:   }
Chris@1: 
Chris@1: }
Chris@1: 
Chris@1: void _vp_tonemask(vorbis_look_psy *p,
Chris@1:                   float *logfft,
Chris@1:                   float *logmask,
Chris@1:                   float global_specmax,
Chris@1:                   float local_specmax){
Chris@1: 
Chris@1:   int i,n=p->n;
Chris@1: 
Chris@1:   float *seed=alloca(sizeof(*seed)*p->total_octave_lines);
Chris@1:   float att=local_specmax+p->vi->ath_adjatt;
Chris@1:   for(i=0;i<p->total_octave_lines;i++)seed[i]=NEGINF;
Chris@1: 
Chris@1:   /* set the ATH (floating below localmax, not global max by a
Chris@1:      specified att) */
Chris@1:   if(att<p->vi->ath_maxatt)att=p->vi->ath_maxatt;
Chris@1: 
Chris@1:   for(i=0;i<n;i++)
Chris@1:     logmask[i]=p->ath[i]+att;
Chris@1: 
Chris@1:   /* tone masking */
Chris@1:   seed_loop(p,(const float ***)p->tonecurves,logfft,logmask,seed,global_specmax);
Chris@1:   max_seeds(p,seed,logmask);
Chris@1: 
Chris@1: }
Chris@1: 
Chris@1: void _vp_offset_and_mix(vorbis_look_psy *p,
Chris@1:                         float *noise,
Chris@1:                         float *tone,
Chris@1:                         int offset_select,
Chris@1:                         float *logmask,
Chris@1:                         float *mdct,
Chris@1:                         float *logmdct){
Chris@1:   int i,n=p->n;
Chris@1:   float de, coeffi, cx;/* AoTuV */
Chris@1:   float toneatt=p->vi->tone_masteratt[offset_select];
Chris@1: 
Chris@1:   cx = p->m_val;
Chris@1: 
Chris@1:   for(i=0;i<n;i++){
Chris@1:     float val= noise[i]+p->noiseoffset[offset_select][i];
Chris@1:     if(val>p->vi->noisemaxsupp)val=p->vi->noisemaxsupp;
Chris@1:     logmask[i]=max(val,tone[i]+toneatt);
Chris@1: 
Chris@1: 
Chris@1:     /* AoTuV */
Chris@1:     /** @ M1 **
Chris@1:         The following codes improve a noise problem.
Chris@1:         A fundamental idea uses the value of masking and carries out
Chris@1:         the relative compensation of the MDCT.
Chris@1:         However, this code is not perfect and all noise problems cannot be solved.
Chris@1:         by Aoyumi @ 2004/04/18
Chris@1:     */
Chris@1: 
Chris@1:     if(offset_select == 1) {
Chris@1:       coeffi = -17.2;       /* coeffi is a -17.2dB threshold */
Chris@1:       val = val - logmdct[i];  /* val == mdct line value relative to floor in dB */
Chris@1: 
Chris@1:       if(val > coeffi){
Chris@1:         /* mdct value is > -17.2 dB below floor */
Chris@1: 
Chris@1:         de = 1.0-((val-coeffi)*0.005*cx);
Chris@1:         /* pro-rated attenuation:
Chris@1:            -0.00 dB boost if mdct value is -17.2dB (relative to floor)
Chris@1:            -0.77 dB boost if mdct value is 0dB (relative to floor)
Chris@1:            -1.64 dB boost if mdct value is +17.2dB (relative to floor)
Chris@1:            etc... */
Chris@1: 
Chris@1:         if(de < 0) de = 0.0001;
Chris@1:       }else
Chris@1:         /* mdct value is <= -17.2 dB below floor */
Chris@1: 
Chris@1:         de = 1.0-((val-coeffi)*0.0003*cx);
Chris@1:       /* pro-rated attenuation:
Chris@1:          +0.00 dB atten if mdct value is -17.2dB (relative to floor)
Chris@1:          +0.45 dB atten if mdct value is -34.4dB (relative to floor)
Chris@1:          etc... */
Chris@1: 
Chris@1:       mdct[i] *= de;
Chris@1: 
Chris@1:     }
Chris@1:   }
Chris@1: }
Chris@1: 
Chris@1: float _vp_ampmax_decay(float amp,vorbis_dsp_state *vd){
Chris@1:   vorbis_info *vi=vd->vi;
Chris@1:   codec_setup_info *ci=vi->codec_setup;
Chris@1:   vorbis_info_psy_global *gi=&ci->psy_g_param;
Chris@1: 
Chris@1:   int n=ci->blocksizes[vd->W]/2;
Chris@1:   float secs=(float)n/vi->rate;
Chris@1: 
Chris@1:   amp+=secs*gi->ampmax_att_per_sec;
Chris@1:   if(amp<-9999)amp=-9999;
Chris@1:   return(amp);
Chris@1: }
Chris@1: 
Chris@1: static float FLOOR1_fromdB_LOOKUP[256]={
Chris@1:   1.0649863e-07F, 1.1341951e-07F, 1.2079015e-07F, 1.2863978e-07F,
Chris@1:   1.3699951e-07F, 1.4590251e-07F, 1.5538408e-07F, 1.6548181e-07F,
Chris@1:   1.7623575e-07F, 1.8768855e-07F, 1.9988561e-07F, 2.128753e-07F,
Chris@1:   2.2670913e-07F, 2.4144197e-07F, 2.5713223e-07F, 2.7384213e-07F,
Chris@1:   2.9163793e-07F, 3.1059021e-07F, 3.3077411e-07F, 3.5226968e-07F,
Chris@1:   3.7516214e-07F, 3.9954229e-07F, 4.2550680e-07F, 4.5315863e-07F,
Chris@1:   4.8260743e-07F, 5.1396998e-07F, 5.4737065e-07F, 5.8294187e-07F,
Chris@1:   6.2082472e-07F, 6.6116941e-07F, 7.0413592e-07F, 7.4989464e-07F,
Chris@1:   7.9862701e-07F, 8.5052630e-07F, 9.0579828e-07F, 9.6466216e-07F,
Chris@1:   1.0273513e-06F, 1.0941144e-06F, 1.1652161e-06F, 1.2409384e-06F,
Chris@1:   1.3215816e-06F, 1.4074654e-06F, 1.4989305e-06F, 1.5963394e-06F,
Chris@1:   1.7000785e-06F, 1.8105592e-06F, 1.9282195e-06F, 2.0535261e-06F,
Chris@1:   2.1869758e-06F, 2.3290978e-06F, 2.4804557e-06F, 2.6416497e-06F,
Chris@1:   2.8133190e-06F, 2.9961443e-06F, 3.1908506e-06F, 3.3982101e-06F,
Chris@1:   3.6190449e-06F, 3.8542308e-06F, 4.1047004e-06F, 4.3714470e-06F,
Chris@1:   4.6555282e-06F, 4.9580707e-06F, 5.2802740e-06F, 5.6234160e-06F,
Chris@1:   5.9888572e-06F, 6.3780469e-06F, 6.7925283e-06F, 7.2339451e-06F,
Chris@1:   7.7040476e-06F, 8.2047000e-06F, 8.7378876e-06F, 9.3057248e-06F,
Chris@1:   9.9104632e-06F, 1.0554501e-05F, 1.1240392e-05F, 1.1970856e-05F,
Chris@1:   1.2748789e-05F, 1.3577278e-05F, 1.4459606e-05F, 1.5399272e-05F,
Chris@1:   1.6400004e-05F, 1.7465768e-05F, 1.8600792e-05F, 1.9809576e-05F,
Chris@1:   2.1096914e-05F, 2.2467911e-05F, 2.3928002e-05F, 2.5482978e-05F,
Chris@1:   2.7139006e-05F, 2.8902651e-05F, 3.0780908e-05F, 3.2781225e-05F,
Chris@1:   3.4911534e-05F, 3.7180282e-05F, 3.9596466e-05F, 4.2169667e-05F,
Chris@1:   4.4910090e-05F, 4.7828601e-05F, 5.0936773e-05F, 5.4246931e-05F,
Chris@1:   5.7772202e-05F, 6.1526565e-05F, 6.5524908e-05F, 6.9783085e-05F,
Chris@1:   7.4317983e-05F, 7.9147585e-05F, 8.4291040e-05F, 8.9768747e-05F,
Chris@1:   9.5602426e-05F, 0.00010181521F, 0.00010843174F, 0.00011547824F,
Chris@1:   0.00012298267F, 0.00013097477F, 0.00013948625F, 0.00014855085F,
Chris@1:   0.00015820453F, 0.00016848555F, 0.00017943469F, 0.00019109536F,
Chris@1:   0.00020351382F, 0.00021673929F, 0.00023082423F, 0.00024582449F,
Chris@1:   0.00026179955F, 0.00027881276F, 0.00029693158F, 0.00031622787F,
Chris@1:   0.00033677814F, 0.00035866388F, 0.00038197188F, 0.00040679456F,
Chris@1:   0.00043323036F, 0.00046138411F, 0.00049136745F, 0.00052329927F,
Chris@1:   0.00055730621F, 0.00059352311F, 0.00063209358F, 0.00067317058F,
Chris@1:   0.00071691700F, 0.00076350630F, 0.00081312324F, 0.00086596457F,
Chris@1:   0.00092223983F, 0.00098217216F, 0.0010459992F, 0.0011139742F,
Chris@1:   0.0011863665F, 0.0012634633F, 0.0013455702F, 0.0014330129F,
Chris@1:   0.0015261382F, 0.0016253153F, 0.0017309374F, 0.0018434235F,
Chris@1:   0.0019632195F, 0.0020908006F, 0.0022266726F, 0.0023713743F,
Chris@1:   0.0025254795F, 0.0026895994F, 0.0028643847F, 0.0030505286F,
Chris@1:   0.0032487691F, 0.0034598925F, 0.0036847358F, 0.0039241906F,
Chris@1:   0.0041792066F, 0.0044507950F, 0.0047400328F, 0.0050480668F,
Chris@1:   0.0053761186F, 0.0057254891F, 0.0060975636F, 0.0064938176F,
Chris@1:   0.0069158225F, 0.0073652516F, 0.0078438871F, 0.0083536271F,
Chris@1:   0.0088964928F, 0.009474637F, 0.010090352F, 0.010746080F,
Chris@1:   0.011444421F, 0.012188144F, 0.012980198F, 0.013823725F,
Chris@1:   0.014722068F, 0.015678791F, 0.016697687F, 0.017782797F,
Chris@1:   0.018938423F, 0.020169149F, 0.021479854F, 0.022875735F,
Chris@1:   0.024362330F, 0.025945531F, 0.027631618F, 0.029427276F,
Chris@1:   0.031339626F, 0.033376252F, 0.035545228F, 0.037855157F,
Chris@1:   0.040315199F, 0.042935108F, 0.045725273F, 0.048696758F,
Chris@1:   0.051861348F, 0.055231591F, 0.058820850F, 0.062643361F,
Chris@1:   0.066714279F, 0.071049749F, 0.075666962F, 0.080584227F,
Chris@1:   0.085821044F, 0.091398179F, 0.097337747F, 0.10366330F,
Chris@1:   0.11039993F, 0.11757434F, 0.12521498F, 0.13335215F,
Chris@1:   0.14201813F, 0.15124727F, 0.16107617F, 0.17154380F,
Chris@1:   0.18269168F, 0.19456402F, 0.20720788F, 0.22067342F,
Chris@1:   0.23501402F, 0.25028656F, 0.26655159F, 0.28387361F,
Chris@1:   0.30232132F, 0.32196786F, 0.34289114F, 0.36517414F,
Chris@1:   0.38890521F, 0.41417847F, 0.44109412F, 0.46975890F,
Chris@1:   0.50028648F, 0.53279791F, 0.56742212F, 0.60429640F,
Chris@1:   0.64356699F, 0.68538959F, 0.72993007F, 0.77736504F,
Chris@1:   0.82788260F, 0.88168307F, 0.9389798F, 1.F,
Chris@1: };
Chris@1: 
Chris@1: /* this is for per-channel noise normalization */
Chris@1: static int apsort(const void *a, const void *b){
Chris@1:   float f1=**(float**)a;
Chris@1:   float f2=**(float**)b;
Chris@1:   return (f1<f2)-(f1>f2);
Chris@1: }
Chris@1: 
Chris@1: static void flag_lossless(int limit, float prepoint, float postpoint, float *mdct,
Chris@1:                          float *floor, int *flag, int i, int jn){
Chris@1:   int j;
Chris@1:   for(j=0;j<jn;j++){
Chris@1:     float point = j>=limit-i ? postpoint : prepoint;
Chris@1:     float r = fabs(mdct[j])/floor[j];
Chris@1:     if(r<point)
Chris@1:       flag[j]=0;
Chris@1:     else
Chris@1:       flag[j]=1;
Chris@1:   }
Chris@1: }
Chris@1: 
Chris@1: /* Overload/Side effect: On input, the *q vector holds either the
Chris@1:    quantized energy (for elements with the flag set) or the absolute
Chris@1:    values of the *r vector (for elements with flag unset).  On output,
Chris@1:    *q holds the quantized energy for all elements */
Chris@1: static float noise_normalize(vorbis_look_psy *p, int limit, float *r, float *q, float *f, int *flags, float acc, int i, int n, int *out){
Chris@1: 
Chris@1:   vorbis_info_psy *vi=p->vi;
Chris@1:   float **sort = alloca(n*sizeof(*sort));
Chris@1:   int j,count=0;
Chris@1:   int start = (vi->normal_p ? vi->normal_start-i : n);
Chris@1:   if(start>n)start=n;
Chris@1: 
Chris@1:   /* force classic behavior where only energy in the current band is considered */
Chris@1:   acc=0.f;
Chris@1: 
Chris@1:   /* still responsible for populating *out where noise norm not in
Chris@1:      effect.  There's no need to [re]populate *q in these areas */
Chris@1:   for(j=0;j<start;j++){
Chris@1:     if(!flags || !flags[j]){ /* lossless coupling already quantized.
Chris@1:                                 Don't touch; requantizing based on
Chris@1:                                 energy would be incorrect. */
Chris@1:       float ve = q[j]/f[j];
Chris@1:       if(r[j]<0)
Chris@1:         out[j] = -rint(sqrt(ve));
Chris@1:       else
Chris@1:         out[j] = rint(sqrt(ve));
Chris@1:     }
Chris@1:   }
Chris@1: 
Chris@1:   /* sort magnitudes for noise norm portion of partition */
Chris@1:   for(;j<n;j++){
Chris@1:     if(!flags || !flags[j]){ /* can't noise norm elements that have
Chris@1:                                 already been loslessly coupled; we can
Chris@1:                                 only account for their energy error */
Chris@1:       float ve = q[j]/f[j];
Chris@1:       /* Despite all the new, more capable coupling code, for now we
Chris@1:          implement noise norm as it has been up to this point. Only
Chris@1:          consider promotions to unit magnitude from 0.  In addition
Chris@1:          the only energy error counted is quantizations to zero. */
Chris@1:       /* also-- the original point code only applied noise norm at > pointlimit */
Chris@1:       if(ve<.25f && (!flags || j>=limit-i)){
Chris@1:         acc += ve;
Chris@1:         sort[count++]=q+j; /* q is fabs(r) for unflagged element */
Chris@1:       }else{
Chris@1:         /* For now: no acc adjustment for nonzero quantization.  populate *out and q as this value is final. */
Chris@1:         if(r[j]<0)
Chris@1:           out[j] = -rint(sqrt(ve));
Chris@1:         else
Chris@1:           out[j] = rint(sqrt(ve));
Chris@1:         q[j] = out[j]*out[j]*f[j];
Chris@1:       }
Chris@1:     }/* else{
Chris@1:         again, no energy adjustment for error in nonzero quant-- for now
Chris@1:         }*/
Chris@1:   }
Chris@1: 
Chris@1:   if(count){
Chris@1:     /* noise norm to do */
Chris@1:     qsort(sort,count,sizeof(*sort),apsort);
Chris@1:     for(j=0;j<count;j++){
Chris@1:       int k=sort[j]-q;
Chris@1:       if(acc>=vi->normal_thresh){
Chris@1:         out[k]=unitnorm(r[k]);
Chris@1:         acc-=1.f;
Chris@1:         q[k]=f[k];
Chris@1:       }else{
Chris@1:         out[k]=0;
Chris@1:         q[k]=0.f;
Chris@1:       }
Chris@1:     }
Chris@1:   }
Chris@1: 
Chris@1:   return acc;
Chris@1: }
Chris@1: 
Chris@1: /* Noise normalization, quantization and coupling are not wholly
Chris@1:    seperable processes in depth>1 coupling. */
Chris@1: void _vp_couple_quantize_normalize(int blobno,
Chris@1:                                    vorbis_info_psy_global *g,
Chris@1:                                    vorbis_look_psy *p,
Chris@1:                                    vorbis_info_mapping0 *vi,
Chris@1:                                    float **mdct,
Chris@1:                                    int   **iwork,
Chris@1:                                    int    *nonzero,
Chris@1:                                    int     sliding_lowpass,
Chris@1:                                    int     ch){
Chris@1: 
Chris@1:   int i;
Chris@1:   int n = p->n;
Chris@1:   int partition=(p->vi->normal_p ? p->vi->normal_partition : 16);
Chris@1:   int limit = g->coupling_pointlimit[p->vi->blockflag][blobno];
Chris@1:   float prepoint=stereo_threshholds[g->coupling_prepointamp[blobno]];
Chris@1:   float postpoint=stereo_threshholds[g->coupling_postpointamp[blobno]];
Chris@1: #if 0
Chris@1:   float de=0.1*p->m_val; /* a blend of the AoTuV M2 and M3 code here and below */
Chris@1: #endif
Chris@1: 
Chris@1:   /* mdct is our raw mdct output, floor not removed. */
Chris@1:   /* inout passes in the ifloor, passes back quantized result */
Chris@1: 
Chris@1:   /* unquantized energy (negative indicates amplitude has negative sign) */
Chris@1:   float **raw = alloca(ch*sizeof(*raw));
Chris@1: 
Chris@1:   /* dual pupose; quantized energy (if flag set), othersize fabs(raw) */
Chris@1:   float **quant = alloca(ch*sizeof(*quant));
Chris@1: 
Chris@1:   /* floor energy */
Chris@1:   float **floor = alloca(ch*sizeof(*floor));
Chris@1: 
Chris@1:   /* flags indicating raw/quantized status of elements in raw vector */
Chris@1:   int   **flag  = alloca(ch*sizeof(*flag));
Chris@1: 
Chris@1:   /* non-zero flag working vector */
Chris@1:   int    *nz    = alloca(ch*sizeof(*nz));
Chris@1: 
Chris@1:   /* energy surplus/defecit tracking */
Chris@1:   float  *acc   = alloca((ch+vi->coupling_steps)*sizeof(*acc));
Chris@1: 
Chris@1:   /* The threshold of a stereo is changed with the size of n */
Chris@1:   if(n > 1000)
Chris@1:     postpoint=stereo_threshholds_limited[g->coupling_postpointamp[blobno]];
Chris@1: 
Chris@1:   raw[0]   = alloca(ch*partition*sizeof(**raw));
Chris@1:   quant[0] = alloca(ch*partition*sizeof(**quant));
Chris@1:   floor[0] = alloca(ch*partition*sizeof(**floor));
Chris@1:   flag[0]  = alloca(ch*partition*sizeof(**flag));
Chris@1: 
Chris@1:   for(i=1;i<ch;i++){
Chris@1:     raw[i]   = &raw[0][partition*i];
Chris@1:     quant[i] = &quant[0][partition*i];
Chris@1:     floor[i] = &floor[0][partition*i];
Chris@1:     flag[i]  = &flag[0][partition*i];
Chris@1:   }
Chris@1:   for(i=0;i<ch+vi->coupling_steps;i++)
Chris@1:     acc[i]=0.f;
Chris@1: 
Chris@1:   for(i=0;i<n;i+=partition){
Chris@1:     int k,j,jn = partition > n-i ? n-i : partition;
Chris@1:     int step,track = 0;
Chris@1: 
Chris@1:     memcpy(nz,nonzero,sizeof(*nz)*ch);
Chris@1: 
Chris@1:     /* prefill */
Chris@1:     memset(flag[0],0,ch*partition*sizeof(**flag));
Chris@1:     for(k=0;k<ch;k++){
Chris@1:       int *iout = &iwork[k][i];
Chris@1:       if(nz[k]){
Chris@1: 
Chris@1:         for(j=0;j<jn;j++)
Chris@1:           floor[k][j] = FLOOR1_fromdB_LOOKUP[iout[j]];
Chris@1: 
Chris@1:         flag_lossless(limit,prepoint,postpoint,&mdct[k][i],floor[k],flag[k],i,jn);
Chris@1: 
Chris@1:         for(j=0;j<jn;j++){
Chris@1:           quant[k][j] = raw[k][j] = mdct[k][i+j]*mdct[k][i+j];
Chris@1:           if(mdct[k][i+j]<0.f) raw[k][j]*=-1.f;
Chris@1:           floor[k][j]*=floor[k][j];
Chris@1:         }
Chris@1: 
Chris@1:         acc[track]=noise_normalize(p,limit,raw[k],quant[k],floor[k],NULL,acc[track],i,jn,iout);
Chris@1: 
Chris@1:       }else{
Chris@1:         for(j=0;j<jn;j++){
Chris@1:           floor[k][j] = 1e-10f;
Chris@1:           raw[k][j] = 0.f;
Chris@1:           quant[k][j] = 0.f;
Chris@1:           flag[k][j] = 0;
Chris@1:           iout[j]=0;
Chris@1:         }
Chris@1:         acc[track]=0.f;
Chris@1:       }
Chris@1:       track++;
Chris@1:     }
Chris@1: 
Chris@1:     /* coupling */
Chris@1:     for(step=0;step<vi->coupling_steps;step++){
Chris@1:       int Mi = vi->coupling_mag[step];
Chris@1:       int Ai = vi->coupling_ang[step];
Chris@1:       int *iM = &iwork[Mi][i];
Chris@1:       int *iA = &iwork[Ai][i];
Chris@1:       float *reM = raw[Mi];
Chris@1:       float *reA = raw[Ai];
Chris@1:       float *qeM = quant[Mi];
Chris@1:       float *qeA = quant[Ai];
Chris@1:       float *floorM = floor[Mi];
Chris@1:       float *floorA = floor[Ai];
Chris@1:       int *fM = flag[Mi];
Chris@1:       int *fA = flag[Ai];
Chris@1: 
Chris@1:       if(nz[Mi] || nz[Ai]){
Chris@1:         nz[Mi] = nz[Ai] = 1;
Chris@1: 
Chris@1:         for(j=0;j<jn;j++){
Chris@1: 
Chris@1:           if(j<sliding_lowpass-i){
Chris@1:             if(fM[j] || fA[j]){
Chris@1:               /* lossless coupling */
Chris@1: 
Chris@1:               reM[j] = fabs(reM[j])+fabs(reA[j]);
Chris@1:               qeM[j] = qeM[j]+qeA[j];
Chris@1:               fM[j]=fA[j]=1;
Chris@1: 
Chris@1:               /* couple iM/iA */
Chris@1:               {
Chris@1:                 int A = iM[j];
Chris@1:                 int B = iA[j];
Chris@1: 
Chris@1:                 if(abs(A)>abs(B)){
Chris@1:                   iA[j]=(A>0?A-B:B-A);
Chris@1:                 }else{
Chris@1:                   iA[j]=(B>0?A-B:B-A);
Chris@1:                   iM[j]=B;
Chris@1:                 }
Chris@1: 
Chris@1:                 /* collapse two equivalent tuples to one */
Chris@1:                 if(iA[j]>=abs(iM[j])*2){
Chris@1:                   iA[j]= -iA[j];
Chris@1:                   iM[j]= -iM[j];
Chris@1:                 }
Chris@1: 
Chris@1:               }
Chris@1: 
Chris@1:             }else{
Chris@1:               /* lossy (point) coupling */
Chris@1:               if(j<limit-i){
Chris@1:                 /* dipole */
Chris@1:                 reM[j] += reA[j];
Chris@1:                 qeM[j] = fabs(reM[j]);
Chris@1:               }else{
Chris@1: #if 0
Chris@1:                 /* AoTuV */
Chris@1:                 /** @ M2 **
Chris@1:                     The boost problem by the combination of noise normalization and point stereo is eased.
Chris@1:                     However, this is a temporary patch.
Chris@1:                     by Aoyumi @ 2004/04/18
Chris@1:                 */
Chris@1:                 float derate = (1.0 - de*((float)(j-limit+i) / (float)(n-limit)));
Chris@1:                 /* elliptical */
Chris@1:                 if(reM[j]+reA[j]<0){
Chris@1:                   reM[j] = - (qeM[j] = (fabs(reM[j])+fabs(reA[j]))*derate*derate);
Chris@1:                 }else{
Chris@1:                   reM[j] =   (qeM[j] = (fabs(reM[j])+fabs(reA[j]))*derate*derate);
Chris@1:                 }
Chris@1: #else
Chris@1:                 /* elliptical */
Chris@1:                 if(reM[j]+reA[j]<0){
Chris@1:                   reM[j] = - (qeM[j] = fabs(reM[j])+fabs(reA[j]));
Chris@1:                 }else{
Chris@1:                   reM[j] =   (qeM[j] = fabs(reM[j])+fabs(reA[j]));
Chris@1:                 }
Chris@1: #endif
Chris@1: 
Chris@1:               }
Chris@1:               reA[j]=qeA[j]=0.f;
Chris@1:               fA[j]=1;
Chris@1:               iA[j]=0;
Chris@1:             }
Chris@1:           }
Chris@1:           floorM[j]=floorA[j]=floorM[j]+floorA[j];
Chris@1:         }
Chris@1:         /* normalize the resulting mag vector */
Chris@1:         acc[track]=noise_normalize(p,limit,raw[Mi],quant[Mi],floor[Mi],flag[Mi],acc[track],i,jn,iM);
Chris@1:         track++;
Chris@1:       }
Chris@1:     }
Chris@1:   }
Chris@1: 
Chris@1:   for(i=0;i<vi->coupling_steps;i++){
Chris@1:     /* make sure coupling a zero and a nonzero channel results in two
Chris@1:        nonzero channels. */
Chris@1:     if(nonzero[vi->coupling_mag[i]] ||
Chris@1:        nonzero[vi->coupling_ang[i]]){
Chris@1:       nonzero[vi->coupling_mag[i]]=1;
Chris@1:       nonzero[vi->coupling_ang[i]]=1;
Chris@1:     }
Chris@1:   }
Chris@1: }