git.mutantstargoat.com Git - laserbrain_demo/blob - libs/vorbis/floor1.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-2009             *
   9  * by the Xiph.Org Foundation http://www.xiph.org/                  *
  10  *                                                                  *
  11  ********************************************************************
  12
  13  function: floor backend 1 implementation
  14  last mod: $Id: floor1.c 18151 2012-01-20 07:35:26Z xiphmont $
  15
  16  ********************************************************************/
  17
  18 #include <stdlib.h>
  19 #include <string.h>
  20 #include <math.h>
  21 #include <ogg/ogg.h>
  22 #include "vorbis/codec.h"
  23 #include "codec_internal.h"
  24 #include "registry.h"
  25 #include "codebook.h"
  26 #include "misc.h"
  27 #include "scales.h"
  28
  29 #include <stdio.h>
  30
  31 #define floor1_rangedB 140 /* floor 1 fixed at -140dB to 0dB range */
  32
  33 typedef struct lsfit_acc{
  34   int x0;
  35   int x1;
  36
  37   int xa;
  38   int ya;
  39   int x2a;
  40   int y2a;
  41   int xya;
  42   int an;
  43
  44   int xb;
  45   int yb;
  46   int x2b;
  47   int y2b;
  48   int xyb;
  49   int bn;
  50 } lsfit_acc;
  51
  52 /***********************************************/
  53
  54 static void floor1_free_info(vorbis_info_floor *i){
  55   vorbis_info_floor1 *info=(vorbis_info_floor1 *)i;
  56   if(info){
  57     memset(info,0,sizeof(*info));
  58     _ogg_free(info);
  59   }
  60 }
  61
  62 static void floor1_free_look(vorbis_look_floor *i){
  63   vorbis_look_floor1 *look=(vorbis_look_floor1 *)i;
  64   if(look){
  65     /*fprintf(stderr,"floor 1 bit usage %f:%f (%f total)\n",
  66             (float)look->phrasebits/look->frames,
  67             (float)look->postbits/look->frames,
  68             (float)(look->postbits+look->phrasebits)/look->frames);*/
  69
  70     memset(look,0,sizeof(*look));
  71     _ogg_free(look);
  72   }
  73 }
  74
  75 static int ilog(unsigned int v){
  76   int ret=0;
  77   while(v){
  78     ret++;
  79     v>>=1;
  80   }
  81   return(ret);
  82 }
  83
  84 static int ilog2(unsigned int v){
  85   int ret=0;
  86   if(v)--v;
  87   while(v){
  88     ret++;
  89     v>>=1;
  90   }
  91   return(ret);
  92 }
  93
  94 static void floor1_pack (vorbis_info_floor *i,oggpack_buffer *opb){
  95   vorbis_info_floor1 *info=(vorbis_info_floor1 *)i;
  96   int j,k;
  97   int count=0;
  98   int rangebits;
  99   int maxposit=info->postlist[1];
 100   int maxclass=-1;
 101
 102   /* save out partitions */
 103   oggpack_write(opb,info->partitions,5); /* only 0 to 31 legal */
 104   for(j=0;j<info->partitions;j++){
 105     oggpack_write(opb,info->partitionclass[j],4); /* only 0 to 15 legal */
 106     if(maxclass<info->partitionclass[j])maxclass=info->partitionclass[j];
 107   }
 108
 109   /* save out partition classes */
 110   for(j=0;j<maxclass+1;j++){
 111     oggpack_write(opb,info->class_dim[j]-1,3); /* 1 to 8 */
 112     oggpack_write(opb,info->class_subs[j],2); /* 0 to 3 */
 113     if(info->class_subs[j])oggpack_write(opb,info->class_book[j],8);
 114     for(k=0;k<(1<<info->class_subs[j]);k++)
 115       oggpack_write(opb,info->class_subbook[j][k]+1,8);
 116   }
 117
 118   /* save out the post list */
 119   oggpack_write(opb,info->mult-1,2);     /* only 1,2,3,4 legal now */
 120   oggpack_write(opb,ilog2(maxposit),4);
 121   rangebits=ilog2(maxposit);
 122
 123   for(j=0,k=0;j<info->partitions;j++){
 124     count+=info->class_dim[info->partitionclass[j]];
 125     for(;k<count;k++)
 126       oggpack_write(opb,info->postlist[k+2],rangebits);
 127   }
 128 }
 129
 130 static int icomp(const void *a,const void *b){
 131   return(**(int **)a-**(int **)b);
 132 }
 133
 134 static vorbis_info_floor *floor1_unpack (vorbis_info *vi,oggpack_buffer *opb){
 135   codec_setup_info     *ci=vi->codec_setup;
 136   int j,k,count=0,maxclass=-1,rangebits;
 137
 138   vorbis_info_floor1 *info=_ogg_calloc(1,sizeof(*info));
 139   /* read partitions */
 140   info->partitions=oggpack_read(opb,5); /* only 0 to 31 legal */
 141   for(j=0;j<info->partitions;j++){
 142     info->partitionclass[j]=oggpack_read(opb,4); /* only 0 to 15 legal */
 143     if(info->partitionclass[j]<0)goto err_out;
 144     if(maxclass<info->partitionclass[j])maxclass=info->partitionclass[j];
 145   }
 146
 147   /* read partition classes */
 148   for(j=0;j<maxclass+1;j++){
 149     info->class_dim[j]=oggpack_read(opb,3)+1; /* 1 to 8 */
 150     info->class_subs[j]=oggpack_read(opb,2); /* 0,1,2,3 bits */
 151     if(info->class_subs[j]<0)
 152       goto err_out;
 153     if(info->class_subs[j])info->class_book[j]=oggpack_read(opb,8);
 154     if(info->class_book[j]<0 || info->class_book[j]>=ci->books)
 155       goto err_out;
 156     for(k=0;k<(1<<info->class_subs[j]);k++){
 157       info->class_subbook[j][k]=oggpack_read(opb,8)-1;
 158       if(info->class_subbook[j][k]<-1 || info->class_subbook[j][k]>=ci->books)
 159         goto err_out;
 160     }
 161   }
 162
 163   /* read the post list */
 164   info->mult=oggpack_read(opb,2)+1;     /* only 1,2,3,4 legal now */
 165   rangebits=oggpack_read(opb,4);
 166   if(rangebits<0)goto err_out;
 167
 168   for(j=0,k=0;j<info->partitions;j++){
 169     count+=info->class_dim[info->partitionclass[j]];
 170     if(count>VIF_POSIT) goto err_out;
 171     for(;k<count;k++){
 172       int t=info->postlist[k+2]=oggpack_read(opb,rangebits);
 173       if(t<0 || t>=(1<<rangebits))
 174         goto err_out;
 175     }
 176   }
 177   info->postlist[0]=0;
 178   info->postlist[1]=1<<rangebits;
 179
 180   /* don't allow repeated values in post list as they'd result in
 181      zero-length segments */
 182   {
 183     int *sortpointer[VIF_POSIT+2];
 184     for(j=0;j<count+2;j++)sortpointer[j]=info->postlist+j;
 185     qsort(sortpointer,count+2,sizeof(*sortpointer),icomp);
 186
 187     for(j=1;j<count+2;j++)
 188       if(*sortpointer[j-1]==*sortpointer[j])goto err_out;
 189   }
 190
 191   return(info);
 192
 193  err_out:
 194   floor1_free_info(info);
 195   return(NULL);
 196 }
 197
 198 static vorbis_look_floor *floor1_look(vorbis_dsp_state *vd,
 199                                       vorbis_info_floor *in){
 200
 201   int *sortpointer[VIF_POSIT+2];
 202   vorbis_info_floor1 *info=(vorbis_info_floor1 *)in;
 203   vorbis_look_floor1 *look=_ogg_calloc(1,sizeof(*look));
 204   int i,j,n=0;
 205
 206   look->vi=info;
 207   look->n=info->postlist[1];
 208
 209   /* we drop each position value in-between already decoded values,
 210      and use linear interpolation to predict each new value past the
 211      edges.  The positions are read in the order of the position
 212      list... we precompute the bounding positions in the lookup.  Of
 213      course, the neighbors can change (if a position is declined), but
 214      this is an initial mapping */
 215
 216   for(i=0;i<info->partitions;i++)n+=info->class_dim[info->partitionclass[i]];
 217   n+=2;
 218   look->posts=n;
 219
 220   /* also store a sorted position index */
 221   for(i=0;i<n;i++)sortpointer[i]=info->postlist+i;
 222   qsort(sortpointer,n,sizeof(*sortpointer),icomp);
 223
 224   /* points from sort order back to range number */
 225   for(i=0;i<n;i++)look->forward_index[i]=sortpointer[i]-info->postlist;
 226   /* points from range order to sorted position */
 227   for(i=0;i<n;i++)look->reverse_index[look->forward_index[i]]=i;
 228   /* we actually need the post values too */
 229   for(i=0;i<n;i++)look->sorted_index[i]=info->postlist[look->forward_index[i]];
 230
 231   /* quantize values to multiplier spec */
 232   switch(info->mult){
 233   case 1: /* 1024 -> 256 */
 234     look->quant_q=256;
 235     break;
 236   case 2: /* 1024 -> 128 */
 237     look->quant_q=128;
 238     break;
 239   case 3: /* 1024 -> 86 */
 240     look->quant_q=86;
 241     break;
 242   case 4: /* 1024 -> 64 */
 243     look->quant_q=64;
 244     break;
 245   }
 246
 247   /* discover our neighbors for decode where we don't use fit flags
 248      (that would push the neighbors outward) */
 249   for(i=0;i<n-2;i++){
 250     int lo=0;
 251     int hi=1;
 252     int lx=0;
 253     int hx=look->n;
 254     int currentx=info->postlist[i+2];
 255     for(j=0;j<i+2;j++){
 256       int x=info->postlist[j];
 257       if(x>lx && x<currentx){
 258         lo=j;
 259         lx=x;
 260       }
 261       if(x<hx && x>currentx){
 262         hi=j;
 263         hx=x;
 264       }
 265     }
 266     look->loneighbor[i]=lo;
 267     look->hineighbor[i]=hi;
 268   }
 269
 270   return(look);
 271 }
 272
 273 static int render_point(int x0,int x1,int y0,int y1,int x){
 274   y0&=0x7fff; /* mask off flag */
 275   y1&=0x7fff;
 276
 277   {
 278     int dy=y1-y0;
 279     int adx=x1-x0;
 280     int ady=abs(dy);
 281     int err=ady*(x-x0);
 282
 283     int off=err/adx;
 284     if(dy<0)return(y0-off);
 285     return(y0+off);
 286   }
 287 }
 288
 289 static int vorbis_dBquant(const float *x){
 290   int i= *x*7.3142857f+1023.5f;
 291   if(i>1023)return(1023);
 292   if(i<0)return(0);
 293   return i;
 294 }
 295
 296 static const float FLOOR1_fromdB_LOOKUP[256]={
 297   1.0649863e-07F, 1.1341951e-07F, 1.2079015e-07F, 1.2863978e-07F,
 298   1.3699951e-07F, 1.4590251e-07F, 1.5538408e-07F, 1.6548181e-07F,
 299   1.7623575e-07F, 1.8768855e-07F, 1.9988561e-07F, 2.128753e-07F,
 300   2.2670913e-07F, 2.4144197e-07F, 2.5713223e-07F, 2.7384213e-07F,
 301   2.9163793e-07F, 3.1059021e-07F, 3.3077411e-07F, 3.5226968e-07F,
 302   3.7516214e-07F, 3.9954229e-07F, 4.2550680e-07F, 4.5315863e-07F,
 303   4.8260743e-07F, 5.1396998e-07F, 5.4737065e-07F, 5.8294187e-07F,
 304   6.2082472e-07F, 6.6116941e-07F, 7.0413592e-07F, 7.4989464e-07F,
 305   7.9862701e-07F, 8.5052630e-07F, 9.0579828e-07F, 9.6466216e-07F,
 306   1.0273513e-06F, 1.0941144e-06F, 1.1652161e-06F, 1.2409384e-06F,
 307   1.3215816e-06F, 1.4074654e-06F, 1.4989305e-06F, 1.5963394e-06F,
 308   1.7000785e-06F, 1.8105592e-06F, 1.9282195e-06F, 2.0535261e-06F,
 309   2.1869758e-06F, 2.3290978e-06F, 2.4804557e-06F, 2.6416497e-06F,
 310   2.8133190e-06F, 2.9961443e-06F, 3.1908506e-06F, 3.3982101e-06F,
 311   3.6190449e-06F, 3.8542308e-06F, 4.1047004e-06F, 4.3714470e-06F,
 312   4.6555282e-06F, 4.9580707e-06F, 5.2802740e-06F, 5.6234160e-06F,
 313   5.9888572e-06F, 6.3780469e-06F, 6.7925283e-06F, 7.2339451e-06F,
 314   7.7040476e-06F, 8.2047000e-06F, 8.7378876e-06F, 9.3057248e-06F,
 315   9.9104632e-06F, 1.0554501e-05F, 1.1240392e-05F, 1.1970856e-05F,
 316   1.2748789e-05F, 1.3577278e-05F, 1.4459606e-05F, 1.5399272e-05F,
 317   1.6400004e-05F, 1.7465768e-05F, 1.8600792e-05F, 1.9809576e-05F,
 318   2.1096914e-05F, 2.2467911e-05F, 2.3928002e-05F, 2.5482978e-05F,
 319   2.7139006e-05F, 2.8902651e-05F, 3.0780908e-05F, 3.2781225e-05F,
 320   3.4911534e-05F, 3.7180282e-05F, 3.9596466e-05F, 4.2169667e-05F,
 321   4.4910090e-05F, 4.7828601e-05F, 5.0936773e-05F, 5.4246931e-05F,
 322   5.7772202e-05F, 6.1526565e-05F, 6.5524908e-05F, 6.9783085e-05F,
 323   7.4317983e-05F, 7.9147585e-05F, 8.4291040e-05F, 8.9768747e-05F,
 324   9.5602426e-05F, 0.00010181521F, 0.00010843174F, 0.00011547824F,
 325   0.00012298267F, 0.00013097477F, 0.00013948625F, 0.00014855085F,
 326   0.00015820453F, 0.00016848555F, 0.00017943469F, 0.00019109536F,
 327   0.00020351382F, 0.00021673929F, 0.00023082423F, 0.00024582449F,
 328   0.00026179955F, 0.00027881276F, 0.00029693158F, 0.00031622787F,
 329   0.00033677814F, 0.00035866388F, 0.00038197188F, 0.00040679456F,
 330   0.00043323036F, 0.00046138411F, 0.00049136745F, 0.00052329927F,
 331   0.00055730621F, 0.00059352311F, 0.00063209358F, 0.00067317058F,
 332   0.00071691700F, 0.00076350630F, 0.00081312324F, 0.00086596457F,
 333   0.00092223983F, 0.00098217216F, 0.0010459992F, 0.0011139742F,
 334   0.0011863665F, 0.0012634633F, 0.0013455702F, 0.0014330129F,
 335   0.0015261382F, 0.0016253153F, 0.0017309374F, 0.0018434235F,
 336   0.0019632195F, 0.0020908006F, 0.0022266726F, 0.0023713743F,
 337   0.0025254795F, 0.0026895994F, 0.0028643847F, 0.0030505286F,
 338   0.0032487691F, 0.0034598925F, 0.0036847358F, 0.0039241906F,
 339   0.0041792066F, 0.0044507950F, 0.0047400328F, 0.0050480668F,
 340   0.0053761186F, 0.0057254891F, 0.0060975636F, 0.0064938176F,
 341   0.0069158225F, 0.0073652516F, 0.0078438871F, 0.0083536271F,
 342   0.0088964928F, 0.009474637F, 0.010090352F, 0.010746080F,
 343   0.011444421F, 0.012188144F, 0.012980198F, 0.013823725F,
 344   0.014722068F, 0.015678791F, 0.016697687F, 0.017782797F,
 345   0.018938423F, 0.020169149F, 0.021479854F, 0.022875735F,
 346   0.024362330F, 0.025945531F, 0.027631618F, 0.029427276F,
 347   0.031339626F, 0.033376252F, 0.035545228F, 0.037855157F,
 348   0.040315199F, 0.042935108F, 0.045725273F, 0.048696758F,
 349   0.051861348F, 0.055231591F, 0.058820850F, 0.062643361F,
 350   0.066714279F, 0.071049749F, 0.075666962F, 0.080584227F,
 351   0.085821044F, 0.091398179F, 0.097337747F, 0.10366330F,
 352   0.11039993F, 0.11757434F, 0.12521498F, 0.13335215F,
 353   0.14201813F, 0.15124727F, 0.16107617F, 0.17154380F,
 354   0.18269168F, 0.19456402F, 0.20720788F, 0.22067342F,
 355   0.23501402F, 0.25028656F, 0.26655159F, 0.28387361F,
 356   0.30232132F, 0.32196786F, 0.34289114F, 0.36517414F,
 357   0.38890521F, 0.41417847F, 0.44109412F, 0.46975890F,
 358   0.50028648F, 0.53279791F, 0.56742212F, 0.60429640F,
 359   0.64356699F, 0.68538959F, 0.72993007F, 0.77736504F,
 360   0.82788260F, 0.88168307F, 0.9389798F, 1.F,
 361 };
 362
 363 static void render_line(int n, int x0,int x1,int y0,int y1,float *d){
 364   int dy=y1-y0;
 365   int adx=x1-x0;
 366   int ady=abs(dy);
 367   int base=dy/adx;
 368   int sy=(dy<0?base-1:base+1);
 369   int x=x0;
 370   int y=y0;
 371   int err=0;
 372
 373   ady-=abs(base*adx);
 374
 375   if(n>x1)n=x1;
 376
 377   if(x<n)
 378     d[x]*=FLOOR1_fromdB_LOOKUP[y];
 379
 380   while(++x<n){
 381     err=err+ady;
 382     if(err>=adx){
 383       err-=adx;
 384       y+=sy;
 385     }else{
 386       y+=base;
 387     }
 388     d[x]*=FLOOR1_fromdB_LOOKUP[y];
 389   }
 390 }
 391
 392 static void render_line0(int n, int x0,int x1,int y0,int y1,int *d){
 393   int dy=y1-y0;
 394   int adx=x1-x0;
 395   int ady=abs(dy);
 396   int base=dy/adx;
 397   int sy=(dy<0?base-1:base+1);
 398   int x=x0;
 399   int y=y0;
 400   int err=0;
 401
 402   ady-=abs(base*adx);
 403
 404   if(n>x1)n=x1;
 405
 406   if(x<n)
 407     d[x]=y;
 408
 409   while(++x<n){
 410     err=err+ady;
 411     if(err>=adx){
 412       err-=adx;
 413       y+=sy;
 414     }else{
 415       y+=base;
 416     }
 417     d[x]=y;
 418   }
 419 }
 420
 421 /* the floor has already been filtered to only include relevant sections */
 422 static int accumulate_fit(const float *flr,const float *mdct,
 423                           int x0, int x1,lsfit_acc *a,
 424                           int n,vorbis_info_floor1 *info){
 425   long i;
 426
 427   int xa=0,ya=0,x2a=0,y2a=0,xya=0,na=0, xb=0,yb=0,x2b=0,y2b=0,xyb=0,nb=0;
 428
 429   memset(a,0,sizeof(*a));
 430   a->x0=x0;
 431   a->x1=x1;
 432   if(x1>=n)x1=n-1;
 433
 434   for(i=x0;i<=x1;i++){
 435     int quantized=vorbis_dBquant(flr+i);
 436     if(quantized){
 437       if(mdct[i]+info->twofitatten>=flr[i]){
 438         xa  += i;
 439         ya  += quantized;
 440         x2a += i*i;
 441         y2a += quantized*quantized;
 442         xya += i*quantized;
 443         na++;
 444       }else{
 445         xb  += i;
 446         yb  += quantized;
 447         x2b += i*i;
 448         y2b += quantized*quantized;
 449         xyb += i*quantized;
 450         nb++;
 451       }
 452     }
 453   }
 454
 455   a->xa=xa;
 456   a->ya=ya;
 457   a->x2a=x2a;
 458   a->y2a=y2a;
 459   a->xya=xya;
 460   a->an=na;
 461
 462   a->xb=xb;
 463   a->yb=yb;
 464   a->x2b=x2b;
 465   a->y2b=y2b;
 466   a->xyb=xyb;
 467   a->bn=nb;
 468
 469   return(na);
 470 }
 471
 472 static int fit_line(lsfit_acc *a,int fits,int *y0,int *y1,
 473                     vorbis_info_floor1 *info){
 474   double xb=0,yb=0,x2b=0,y2b=0,xyb=0,bn=0;
 475   int i;
 476   int x0=a[0].x0;
 477   int x1=a[fits-1].x1;
 478
 479   for(i=0;i<fits;i++){
 480     double weight = (a[i].bn+a[i].an)*info->twofitweight/(a[i].an+1)+1.;
 481
 482     xb+=a[i].xb + a[i].xa * weight;
 483     yb+=a[i].yb + a[i].ya * weight;
 484     x2b+=a[i].x2b + a[i].x2a * weight;
 485     y2b+=a[i].y2b + a[i].y2a * weight;
 486     xyb+=a[i].xyb + a[i].xya * weight;
 487     bn+=a[i].bn + a[i].an * weight;
 488   }
 489
 490   if(*y0>=0){
 491     xb+=   x0;
 492     yb+=  *y0;
 493     x2b+=  x0 *  x0;
 494     y2b+= *y0 * *y0;
 495     xyb+= *y0 *  x0;
 496     bn++;
 497   }
 498
 499   if(*y1>=0){
 500     xb+=   x1;
 501     yb+=  *y1;
 502     x2b+=  x1 *  x1;
 503     y2b+= *y1 * *y1;
 504     xyb+= *y1 *  x1;
 505     bn++;
 506   }
 507
 508   {
 509     double denom=(bn*x2b-xb*xb);
 510
 511     if(denom>0.){
 512       double a=(yb*x2b-xyb*xb)/denom;
 513       double b=(bn*xyb-xb*yb)/denom;
 514       *y0=rint(a+b*x0);
 515       *y1=rint(a+b*x1);
 516
 517       /* limit to our range! */
 518       if(*y0>1023)*y0=1023;
 519       if(*y1>1023)*y1=1023;
 520       if(*y0<0)*y0=0;
 521       if(*y1<0)*y1=0;
 522
 523       return 0;
 524     }else{
 525       *y0=0;
 526       *y1=0;
 527       return 1;
 528     }
 529   }
 530 }
 531
 532 static int inspect_error(int x0,int x1,int y0,int y1,const float *mask,
 533                          const float *mdct,
 534                          vorbis_info_floor1 *info){
 535   int dy=y1-y0;
 536   int adx=x1-x0;
 537   int ady=abs(dy);
 538   int base=dy/adx;
 539   int sy=(dy<0?base-1:base+1);
 540   int x=x0;
 541   int y=y0;
 542   int err=0;
 543   int val=vorbis_dBquant(mask+x);
 544   int mse=0;
 545   int n=0;
 546
 547   ady-=abs(base*adx);
 548
 549   mse=(y-val);
 550   mse*=mse;
 551   n++;
 552   if(mdct[x]+info->twofitatten>=mask[x]){
 553     if(y+info->maxover<val)return(1);
 554     if(y-info->maxunder>val)return(1);
 555   }
 556
 557   while(++x<x1){
 558     err=err+ady;
 559     if(err>=adx){
 560       err-=adx;
 561       y+=sy;
 562     }else{
 563       y+=base;
 564     }
 565
 566     val=vorbis_dBquant(mask+x);
 567     mse+=((y-val)*(y-val));
 568     n++;
 569     if(mdct[x]+info->twofitatten>=mask[x]){
 570       if(val){
 571         if(y+info->maxover<val)return(1);
 572         if(y-info->maxunder>val)return(1);
 573       }
 574     }
 575   }
 576
 577   if(info->maxover*info->maxover/n>info->maxerr)return(0);
 578   if(info->maxunder*info->maxunder/n>info->maxerr)return(0);
 579   if(mse/n>info->maxerr)return(1);
 580   return(0);
 581 }
 582
 583 static int post_Y(int *A,int *B,int pos){
 584   if(A[pos]<0)
 585     return B[pos];
 586   if(B[pos]<0)
 587     return A[pos];
 588
 589   return (A[pos]+B[pos])>>1;
 590 }
 591
 592 int *floor1_fit(vorbis_block *vb,vorbis_look_floor1 *look,
 593                           const float *logmdct,   /* in */
 594                           const float *logmask){
 595   long i,j;
 596   vorbis_info_floor1 *info=look->vi;
 597   long n=look->n;
 598   long posts=look->posts;
 599   long nonzero=0;
 600   lsfit_acc fits[VIF_POSIT+1];
 601   int fit_valueA[VIF_POSIT+2]; /* index by range list position */
 602   int fit_valueB[VIF_POSIT+2]; /* index by range list position */
 603
 604   int loneighbor[VIF_POSIT+2]; /* sorted index of range list position (+2) */
 605   int hineighbor[VIF_POSIT+2];
 606   int *output=NULL;
 607   int memo[VIF_POSIT+2];
 608
 609   for(i=0;i<posts;i++)fit_valueA[i]=-200; /* mark all unused */
 610   for(i=0;i<posts;i++)fit_valueB[i]=-200; /* mark all unused */
 611   for(i=0;i<posts;i++)loneighbor[i]=0; /* 0 for the implicit 0 post */
 612   for(i=0;i<posts;i++)hineighbor[i]=1; /* 1 for the implicit post at n */
 613   for(i=0;i<posts;i++)memo[i]=-1;      /* no neighbor yet */
 614
 615   /* quantize the relevant floor points and collect them into line fit
 616      structures (one per minimal division) at the same time */
 617   if(posts==0){
 618     nonzero+=accumulate_fit(logmask,logmdct,0,n,fits,n,info);
 619   }else{
 620     for(i=0;i<posts-1;i++)
 621       nonzero+=accumulate_fit(logmask,logmdct,look->sorted_index[i],
 622                               look->sorted_index[i+1],fits+i,
 623                               n,info);
 624   }
 625
 626   if(nonzero){
 627     /* start by fitting the implicit base case.... */
 628     int y0=-200;
 629     int y1=-200;
 630     fit_line(fits,posts-1,&y0,&y1,info);
 631
 632     fit_valueA[0]=y0;
 633     fit_valueB[0]=y0;
 634     fit_valueB[1]=y1;
 635     fit_valueA[1]=y1;
 636
 637     /* Non degenerate case */
 638     /* start progressive splitting.  This is a greedy, non-optimal
 639        algorithm, but simple and close enough to the best
 640        answer. */
 641     for(i=2;i<posts;i++){
 642       int sortpos=look->reverse_index[i];
 643       int ln=loneighbor[sortpos];
 644       int hn=hineighbor[sortpos];
 645
 646       /* eliminate repeat searches of a particular range with a memo */
 647       if(memo[ln]!=hn){
 648         /* haven't performed this error search yet */
 649         int lsortpos=look->reverse_index[ln];
 650         int hsortpos=look->reverse_index[hn];
 651         memo[ln]=hn;
 652
 653         {
 654           /* A note: we want to bound/minimize *local*, not global, error */
 655           int lx=info->postlist[ln];
 656           int hx=info->postlist[hn];
 657           int ly=post_Y(fit_valueA,fit_valueB,ln);
 658           int hy=post_Y(fit_valueA,fit_valueB,hn);
 659
 660           if(ly==-1 || hy==-1){
 661             exit(1);
 662           }
 663
 664           if(inspect_error(lx,hx,ly,hy,logmask,logmdct,info)){
 665             /* outside error bounds/begin search area.  Split it. */
 666             int ly0=-200;
 667             int ly1=-200;
 668             int hy0=-200;
 669             int hy1=-200;
 670             int ret0=fit_line(fits+lsortpos,sortpos-lsortpos,&ly0,&ly1,info);
 671             int ret1=fit_line(fits+sortpos,hsortpos-sortpos,&hy0,&hy1,info);
 672
 673             if(ret0){
 674               ly0=ly;
 675               ly1=hy0;
 676             }
 677             if(ret1){
 678               hy0=ly1;
 679               hy1=hy;
 680             }
 681
 682             if(ret0 && ret1){
 683               fit_valueA[i]=-200;
 684               fit_valueB[i]=-200;
 685             }else{
 686               /* store new edge values */
 687               fit_valueB[ln]=ly0;
 688               if(ln==0)fit_valueA[ln]=ly0;
 689               fit_valueA[i]=ly1;
 690               fit_valueB[i]=hy0;
 691               fit_valueA[hn]=hy1;
 692               if(hn==1)fit_valueB[hn]=hy1;
 693
 694               if(ly1>=0 || hy0>=0){
 695                 /* store new neighbor values */
 696                 for(j=sortpos-1;j>=0;j--)
 697                   if(hineighbor[j]==hn)
 698                     hineighbor[j]=i;
 699                   else
 700                     break;
 701                 for(j=sortpos+1;j<posts;j++)
 702                   if(loneighbor[j]==ln)
 703                     loneighbor[j]=i;
 704                   else
 705                     break;
 706               }
 707             }
 708           }else{
 709             fit_valueA[i]=-200;
 710             fit_valueB[i]=-200;
 711           }
 712         }
 713       }
 714     }
 715
 716     output=_vorbis_block_alloc(vb,sizeof(*output)*posts);
 717
 718     output[0]=post_Y(fit_valueA,fit_valueB,0);
 719     output[1]=post_Y(fit_valueA,fit_valueB,1);
 720
 721     /* fill in posts marked as not using a fit; we will zero
 722        back out to 'unused' when encoding them so long as curve
 723        interpolation doesn't force them into use */
 724     for(i=2;i<posts;i++){
 725       int ln=look->loneighbor[i-2];
 726       int hn=look->hineighbor[i-2];
 727       int x0=info->postlist[ln];
 728       int x1=info->postlist[hn];
 729       int y0=output[ln];
 730       int y1=output[hn];
 731
 732       int predicted=render_point(x0,x1,y0,y1,info->postlist[i]);
 733       int vx=post_Y(fit_valueA,fit_valueB,i);
 734
 735       if(vx>=0 && predicted!=vx){
 736         output[i]=vx;
 737       }else{
 738         output[i]= predicted|0x8000;
 739       }
 740     }
 741   }
 742
 743   return(output);
 744
 745 }
 746
 747 int *floor1_interpolate_fit(vorbis_block *vb,vorbis_look_floor1 *look,
 748                           int *A,int *B,
 749                           int del){
 750
 751   long i;
 752   long posts=look->posts;
 753   int *output=NULL;
 754
 755   if(A && B){
 756     output=_vorbis_block_alloc(vb,sizeof(*output)*posts);
 757
 758     /* overly simpleminded--- look again post 1.2 */
 759     for(i=0;i<posts;i++){
 760       output[i]=((65536-del)*(A[i]&0x7fff)+del*(B[i]&0x7fff)+32768)>>16;
 761       if(A[i]&0x8000 && B[i]&0x8000)output[i]|=0x8000;
 762     }
 763   }
 764
 765   return(output);
 766 }
 767
 768
 769 int floor1_encode(oggpack_buffer *opb,vorbis_block *vb,
 770                   vorbis_look_floor1 *look,
 771                   int *post,int *ilogmask){
 772
 773   long i,j;
 774   vorbis_info_floor1 *info=look->vi;
 775   long posts=look->posts;
 776   codec_setup_info *ci=vb->vd->vi->codec_setup;
 777   int out[VIF_POSIT+2];
 778   static_codebook **sbooks=ci->book_param;
 779   codebook *books=ci->fullbooks;
 780
 781   /* quantize values to multiplier spec */
 782   if(post){
 783     for(i=0;i<posts;i++){
 784       int val=post[i]&0x7fff;
 785       switch(info->mult){
 786       case 1: /* 1024 -> 256 */
 787         val>>=2;
 788         break;
 789       case 2: /* 1024 -> 128 */
 790         val>>=3;
 791         break;
 792       case 3: /* 1024 -> 86 */
 793         val/=12;
 794         break;
 795       case 4: /* 1024 -> 64 */
 796         val>>=4;
 797         break;
 798       }
 799       post[i]=val | (post[i]&0x8000);
 800     }
 801
 802     out[0]=post[0];
 803     out[1]=post[1];
 804
 805     /* find prediction values for each post and subtract them */
 806     for(i=2;i<posts;i++){
 807       int ln=look->loneighbor[i-2];
 808       int hn=look->hineighbor[i-2];
 809       int x0=info->postlist[ln];
 810       int x1=info->postlist[hn];
 811       int y0=post[ln];
 812       int y1=post[hn];
 813
 814       int predicted=render_point(x0,x1,y0,y1,info->postlist[i]);
 815
 816       if((post[i]&0x8000) || (predicted==post[i])){
 817         post[i]=predicted|0x8000; /* in case there was roundoff jitter
 818                                      in interpolation */
 819         out[i]=0;
 820       }else{
 821         int headroom=(look->quant_q-predicted<predicted?
 822                       look->quant_q-predicted:predicted);
 823
 824         int val=post[i]-predicted;
 825
 826         /* at this point the 'deviation' value is in the range +/- max
 827            range, but the real, unique range can always be mapped to
 828            only [0-maxrange).  So we want to wrap the deviation into
 829            this limited range, but do it in the way that least screws
 830            an essentially gaussian probability distribution. */
 831
 832         if(val<0)
 833           if(val<-headroom)
 834             val=headroom-val-1;
 835           else
 836             val=-1-(val<<1);
 837         else
 838           if(val>=headroom)
 839             val= val+headroom;
 840           else
 841             val<<=1;
 842
 843         out[i]=val;
 844         post[ln]&=0x7fff;
 845         post[hn]&=0x7fff;
 846       }
 847     }
 848
 849     /* we have everything we need. pack it out */
 850     /* mark nontrivial floor */
 851     oggpack_write(opb,1,1);
 852
 853     /* beginning/end post */
 854     look->frames++;
 855     look->postbits+=ilog(look->quant_q-1)*2;
 856     oggpack_write(opb,out[0],ilog(look->quant_q-1));
 857     oggpack_write(opb,out[1],ilog(look->quant_q-1));
 858
 859
 860     /* partition by partition */
 861     for(i=0,j=2;i<info->partitions;i++){
 862       int class=info->partitionclass[i];
 863       int cdim=info->class_dim[class];
 864       int csubbits=info->class_subs[class];
 865       int csub=1<<csubbits;
 866       int bookas[8]={0,0,0,0,0,0,0,0};
 867       int cval=0;
 868       int cshift=0;
 869       int k,l;
 870
 871       /* generate the partition's first stage cascade value */
 872       if(csubbits){
 873         int maxval[8];
 874         for(k=0;k<csub;k++){
 875           int booknum=info->class_subbook[class][k];
 876           if(booknum<0){
 877             maxval[k]=1;
 878           }else{
 879             maxval[k]=sbooks[info->class_subbook[class][k]]->entries;
 880           }
 881         }
 882         for(k=0;k<cdim;k++){
 883           for(l=0;l<csub;l++){
 884             int val=out[j+k];
 885             if(val<maxval[l]){
 886               bookas[k]=l;
 887               break;
 888             }
 889           }
 890           cval|= bookas[k]<<cshift;
 891           cshift+=csubbits;
 892         }
 893         /* write it */
 894         look->phrasebits+=
 895           vorbis_book_encode(books+info->class_book[class],cval,opb);
 896
 897 #ifdef TRAIN_FLOOR1
 898         {
 899           FILE *of;
 900           char buffer[80];
 901           sprintf(buffer,"line_%dx%ld_class%d.vqd",
 902                   vb->pcmend/2,posts-2,class);
 903           of=fopen(buffer,"a");
 904           fprintf(of,"%d\n",cval);
 905           fclose(of);
 906         }
 907 #endif
 908       }
 909
 910       /* write post values */
 911       for(k=0;k<cdim;k++){
 912         int book=info->class_subbook[class][bookas[k]];
 913         if(book>=0){
 914           /* hack to allow training with 'bad' books */
 915           if(out[j+k]<(books+book)->entries)
 916             look->postbits+=vorbis_book_encode(books+book,
 917                                                out[j+k],opb);
 918           /*else
 919             fprintf(stderr,"+!");*/
 920
 921 #ifdef TRAIN_FLOOR1
 922           {
 923             FILE *of;
 924             char buffer[80];
 925             sprintf(buffer,"line_%dx%ld_%dsub%d.vqd",
 926                     vb->pcmend/2,posts-2,class,bookas[k]);
 927             of=fopen(buffer,"a");
 928             fprintf(of,"%d\n",out[j+k]);
 929             fclose(of);
 930           }
 931 #endif
 932         }
 933       }
 934       j+=cdim;
 935     }
 936
 937     {
 938       /* generate quantized floor equivalent to what we'd unpack in decode */
 939       /* render the lines */
 940       int hx=0;
 941       int lx=0;
 942       int ly=post[0]*info->mult;
 943       int n=ci->blocksizes[vb->W]/2;
 944
 945       for(j=1;j<look->posts;j++){
 946         int current=look->forward_index[j];
 947         int hy=post[current]&0x7fff;
 948         if(hy==post[current]){
 949
 950           hy*=info->mult;
 951           hx=info->postlist[current];
 952
 953           render_line0(n,lx,hx,ly,hy,ilogmask);
 954
 955           lx=hx;
 956           ly=hy;
 957         }
 958       }
 959       for(j=hx;j<vb->pcmend/2;j++)ilogmask[j]=ly; /* be certain */
 960       return(1);
 961     }
 962   }else{
 963     oggpack_write(opb,0,1);
 964     memset(ilogmask,0,vb->pcmend/2*sizeof(*ilogmask));
 965     return(0);
 966   }
 967 }
 968
 969 static void *floor1_inverse1(vorbis_block *vb,vorbis_look_floor *in){
 970   vorbis_look_floor1 *look=(vorbis_look_floor1 *)in;
 971   vorbis_info_floor1 *info=look->vi;
 972   codec_setup_info   *ci=vb->vd->vi->codec_setup;
 973
 974   int i,j,k;
 975   codebook *books=ci->fullbooks;
 976
 977   /* unpack wrapped/predicted values from stream */
 978   if(oggpack_read(&vb->opb,1)==1){
 979     int *fit_value=_vorbis_block_alloc(vb,(look->posts)*sizeof(*fit_value));
 980
 981     fit_value[0]=oggpack_read(&vb->opb,ilog(look->quant_q-1));
 982     fit_value[1]=oggpack_read(&vb->opb,ilog(look->quant_q-1));
 983
 984     /* partition by partition */
 985     for(i=0,j=2;i<info->partitions;i++){
 986       int class=info->partitionclass[i];
 987       int cdim=info->class_dim[class];
 988       int csubbits=info->class_subs[class];
 989       int csub=1<<csubbits;
 990       int cval=0;
 991
 992       /* decode the partition's first stage cascade value */
 993       if(csubbits){
 994         cval=vorbis_book_decode(books+info->class_book[class],&vb->opb);
 995
 996         if(cval==-1)goto eop;
 997       }
 998
 999       for(k=0;k<cdim;k++){
1000         int book=info->class_subbook[class][cval&(csub-1)];
1001         cval>>=csubbits;
1002         if(book>=0){
1003           if((fit_value[j+k]=vorbis_book_decode(books+book,&vb->opb))==-1)
1004             goto eop;
1005         }else{
1006           fit_value[j+k]=0;
1007         }
1008       }
1009       j+=cdim;
1010     }
1011
1012     /* unwrap positive values and reconsitute via linear interpolation */
1013     for(i=2;i<look->posts;i++){
1014       int predicted=render_point(info->postlist[look->loneighbor[i-2]],
1015                                  info->postlist[look->hineighbor[i-2]],
1016                                  fit_value[look->loneighbor[i-2]],
1017                                  fit_value[look->hineighbor[i-2]],
1018                                  info->postlist[i]);
1019       int hiroom=look->quant_q-predicted;
1020       int loroom=predicted;
1021       int room=(hiroom<loroom?hiroom:loroom)<<1;
1022       int val=fit_value[i];
1023
1024       if(val){
1025         if(val>=room){
1026           if(hiroom>loroom){
1027             val = val-loroom;
1028           }else{
1029             val = -1-(val-hiroom);
1030           }
1031         }else{
1032           if(val&1){
1033             val= -((val+1)>>1);
1034           }else{
1035             val>>=1;
1036           }
1037         }
1038
1039         fit_value[i]=(val+predicted)&0x7fff;
1040         fit_value[look->loneighbor[i-2]]&=0x7fff;
1041         fit_value[look->hineighbor[i-2]]&=0x7fff;
1042
1043       }else{
1044         fit_value[i]=predicted|0x8000;
1045       }
1046
1047     }
1048
1049     return(fit_value);
1050   }
1051  eop:
1052   return(NULL);
1053 }
1054
1055 static int floor1_inverse2(vorbis_block *vb,vorbis_look_floor *in,void *memo,
1056                           float *out){
1057   vorbis_look_floor1 *look=(vorbis_look_floor1 *)in;
1058   vorbis_info_floor1 *info=look->vi;
1059
1060   codec_setup_info   *ci=vb->vd->vi->codec_setup;
1061   int                  n=ci->blocksizes[vb->W]/2;
1062   int j;
1063
1064   if(memo){
1065     /* render the lines */
1066     int *fit_value=(int *)memo;
1067     int hx=0;
1068     int lx=0;
1069     int ly=fit_value[0]*info->mult;
1070     /* guard lookup against out-of-range values */
1071     ly=(ly<0?0:ly>255?255:ly);
1072
1073     for(j=1;j<look->posts;j++){
1074       int current=look->forward_index[j];
1075       int hy=fit_value[current]&0x7fff;
1076       if(hy==fit_value[current]){
1077
1078         hx=info->postlist[current];
1079         hy*=info->mult;
1080         /* guard lookup against out-of-range values */
1081         hy=(hy<0?0:hy>255?255:hy);
1082
1083         render_line(n,lx,hx,ly,hy,out);
1084
1085         lx=hx;
1086         ly=hy;
1087       }
1088     }
1089     for(j=hx;j<n;j++)out[j]*=FLOOR1_fromdB_LOOKUP[ly]; /* be certain */
1090     return(1);
1091   }
1092   memset(out,0,sizeof(*out)*n);
1093   return(0);
1094 }
1095
1096 /* export hooks */
1097 const vorbis_func_floor floor1_exportbundle={
1098   &floor1_pack,&floor1_unpack,&floor1_look,&floor1_free_info,
1099   &floor1_free_look,&floor1_inverse1,&floor1_inverse2
1100 };