Include <math.h> for M_PI.
[daala.git] / src / pvq.c
1 /*Daala video codec
2 Copyright (c) 2012 Daala project contributors.  All rights reserved.
3
4 Redistribution and use in source and binary forms, with or without
5 modification, are permitted provided that the following conditions are met:
6
7 - Redistributions of source code must retain the above copyright notice, this
8   list of conditions and the following disclaimer.
9
10 - Redistributions in binary form must reproduce the above copyright notice,
11   this list of conditions and the following disclaimer in the documentation
12   and/or other materials provided with the distribution.
13
14 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
15 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
17 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
18 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
19 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
20 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
21 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
22 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
23 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.*/
24
25 #include "pvq.h"
26 #include <stdlib.h>
27 #include <stdio.h>
28 #include <math.h>
29
30 #define MAXN 256
31 #define EPSILON 1e-30
32
33 typedef struct {
34   int i;
35   float rd;
36 } RDOEntry;
37
38 #if 0
39 static int compare(const RDOEntry *a, const RDOEntry *b)
40 {
41   if (a->rd<b->rd)
42     return -1;
43   else if (a->rd>b->rd)
44     return 1;
45   else
46     return 0;
47 }
48 #endif
49
50 #define SWAP(x,a,b) do{RDOEntry tmp=x[b];x[b]=x[a];x[a]=tmp;}while(0);
51 static void find_nbest(RDOEntry *x, int n, int len)
52 {
53   int begin, end;
54   begin=0;
55   end=len;
56   if (n<=0)
57     return;
58   while(1) {
59     int i;
60     int index;
61     int pivot;
62     float pval;
63     pivot=(end+begin)/2;
64     pval = x[pivot].rd;
65     index = begin;
66     SWAP(x,pivot,end-1);
67     for (i=begin;i<end-1;i++)
68     {
69       if (x[i].rd<pval)
70       {
71         SWAP(x,i,index);
72         index++;
73       }
74     }
75     SWAP(x,index,end-1);
76     if (index<n-1)
77     {
78       begin=index+1;
79     } else if (index>n)
80     {
81       end=index;
82     } else {
83       break;
84     }
85   };
86 }
87
88 /* This is a "standard" pyramid vector quantizer search */
89 static void pvq_search_rdo(float *x,float *scale,float *scale_1,float g,int N,int K,int *y,int m,float lambda){
90   float L1;
91   float L2;
92   float L1_proj;
93   float dist_scale;
94   int   i;
95   int   j;
96   int   left;
97   int   s[MAXN];
98   float xy; /* sum(x*y) */
99   float yy; /* sum(y*y) */
100   float xx;
101   float p[MAXN];
102   RDOEntry rd[MAXN];
103   /* Some simple RDO constants that should eventually depend on the state of the pvq encoders */
104   const float rate_ym = .3; /* Cost advantage of y[m] compared to y[0] */
105   const float rate_lin = .1; /* Cost penalty of y[n+1] compared to y[n] */
106
107   /* Apply inverse scaling */
108   if (scale!=NULL){
109     for (i=0;i<N;i++){
110       x[i]*=scale_1[i];
111     }
112   }
113   /* Remove the sign and compute projection on the pyramid */
114   L1=0;
115   xx=0;
116   for (i=0;i<N;i++){
117     s[i]=x[i]>0?1:-1;
118     x[i]=fabs(x[i]);
119     xx += x[i]*x[i];
120     L1+=x[i];
121   }
122   {
123     float rescale = 1./sqrt(1e-15+xx);
124     for (i=0;i<N;i++){
125       x[i] *= rescale;
126     }
127     L1 *= rescale;
128     xx=1;
129   }
130   dist_scale = 1.f/(1e-15+L1*L1);
131   L1_proj=K/(EPSILON+L1);
132   left=K;
133   xy=0;
134   yy=0;
135   /* Find the first pulses based on the projection */
136   for (i=0;i<N;i++){
137     p[i]=L1_proj*x[i];
138     y[i]=floor(p[i]);
139     p[i] -= y[i];
140     rd[i].rd = dist_scale*(1-2*p[i]) + rate_lin*lambda*i ;
141     rd[i].i = i;
142     left-=y[i];
143     xy+=x[i]*y[i];
144     yy+=y[i]*y[i];
145   }
146   /* Revert the rate cost of m and replace by the special case cost */
147   rd[m].rd -= rate_ym*lambda + rate_lin*lambda*m;
148 #if 0
149   qsort(rd, N-1, sizeof(RDOEntry), compare);
150 #else
151   find_nbest(rd,left-1,N);
152 #endif
153 #if 1
154   i=0;
155   while(left>1)
156   {
157     int ii=rd[i].i;
158     y[ii]++;
159     left--;
160     xy+=x[ii];
161     yy+=2*y[ii]-1;
162     i++;
163   }
164 #endif
165   /* Find the remaining pulses "the long way" by minimizing the RDO cost function */
166   for(i=0;i<left;i++){
167     int   best_id;
168     float best_cost; /* cost has reversed sign */
169     best_cost=0;
170     yy+=1;
171     best_id = 0;
172     for(j=0;j<N;j++){
173       float tmp_xy;
174       float tmp_yy;
175       float cost; /* cost has reversed sign */
176       tmp_xy=xy+x[j];
177       tmp_yy=yy+2*y[j];
178       /* RDO cost function (reversed sign) */
179       cost = 2*tmp_xy/sqrt(tmp_yy)-rate_lin*lambda*j;
180       /* Revert the rate cost of m and replace by the special case cost */
181       if (j==m)
182         cost+=rate_ym*lambda+rate_lin*lambda*m;
183       if (cost > best_cost){
184         best_cost = cost;
185         best_id=j;
186       }
187     }
188     xy+=x[best_id];
189     yy+=2*y[best_id];
190     y[best_id]++;
191   }
192
193   if (scale!=NULL){
194     L2=0;
195     for(i=0;i<N;i++){
196       float tmp;
197       tmp=scale[i]*y[i];
198       L2+=tmp*tmp;
199     }
200     /* Scale x to unit norm (with scaling) */
201     g/=(EPSILON+sqrt(L2));
202     for(i=0;i<N;i++){
203       x[i]=s[i]*g*scale[i]*y[i];
204     }
205   } else {
206     /* Scale x to unit norm (without scaling) */
207     g/=(EPSILON+sqrt(yy));
208     for(i=0;i<N;i++){
209       x[i]=s[i]*g*y[i];
210     }
211   }
212
213   for(i=0;i<N;i++){
214     y[i]*=s[i];
215   }
216 }
217
218 /* This is a "standard" pyramid vector quantizer search */
219 static void pvq_search(float *x,float *scale,float *scale_1,float g,int N,int K,int *y){
220   float L1;
221   float L2;
222   float L1_proj;
223   int   i;
224   int   j;
225   int   left;
226   int   s[MAXN];
227   float xy; /* sum(x*y) */
228   float yy; /* sum(y*y) */
229   float xx;
230
231   /* Apply inverse scaling */
232   if (scale!=NULL){
233     for (i=0;i<N;i++){
234       x[i]*=scale_1[i];
235     }
236   }
237   /* Remove the sign and compute projection on the pyramid */
238   L1=0;
239   xx=0;
240   for (i=0;i<N;i++){
241     s[i]=x[i]>0?1:-1;
242     x[i]=fabs(x[i]);
243     xx += x[i]*x[i];
244     L1+=x[i];
245   }
246   L1_proj=K/(EPSILON+L1);
247   left=K;
248   xy=0;
249   yy=0;
250   /* Find the first pulses based on the projection */
251   for (i=0;i<N;i++){
252     y[i]=floor(L1_proj*x[i]);
253     left-=y[i];
254     xy+=x[i]*y[i];
255     yy+=y[i]*y[i];
256   }
257
258   /* Find the remaining pulses "the long way" by maximizing xy^2/yy */
259   for(i=0;i<left;i++){
260     float best_num;
261     float best_den;
262     int   best_id;
263     best_num=-1;
264     best_den=1e-15;
265     yy+=1;
266     best_id = 0;
267     for(j=0;j<N;j++){
268       float tmp_xy;
269       float tmp_yy;
270       tmp_xy=xy+x[j];
271       tmp_yy=yy+2*y[j];
272       tmp_xy*=tmp_xy;
273       /* Trick to avoid having to divide by the denominators */
274       if (tmp_xy*best_den > best_num*tmp_yy){
275       /*if (tmp_xy/sqrt(xx*tmp_yy)+best_cost > best_num/sqrt(xx*best_den)+cost){*/
276         best_num=tmp_xy;
277         best_den=tmp_yy;
278         best_id=j;
279       }
280     }
281     xy+=x[best_id];
282     yy+=2*y[best_id];
283     y[best_id]++;
284   }
285
286   if (scale!=NULL){
287     L2=0;
288     for(i=0;i<N;i++){
289       float tmp;
290       tmp=scale[i]*y[i];
291       L2+=tmp*tmp;
292     }
293     /* Scale x to unit norm (with scaling) */
294     g/=(EPSILON+sqrt(L2));
295     for(i=0;i<N;i++){
296       x[i]=s[i]*g*scale[i]*y[i];
297     }
298   } else {
299     /* Scale x to unit norm (without scaling) */
300     g/=(EPSILON+sqrt(yy));
301     for(i=0;i<N;i++){
302       x[i]=s[i]*g*y[i];
303     }
304   }
305
306   for(i=0;i<N;i++){
307     y[i]*=s[i];
308   }
309 }
310
311
312
313 #define GAIN_EXP (4./3.)
314 #define GAIN_EXP_1 (1./GAIN_EXP)
315
316 int quant_pvq_theta(ogg_int32_t *_x,const ogg_int32_t *_r,
317     ogg_int16_t *_scale,int *y,int N,int _Q, int *qg){
318   float L2x,L2r;
319   float g;
320   float gr;
321   float x[MAXN];
322   float r[MAXN];
323   float scale[MAXN];
324   float Q;
325   float scale_1[MAXN];
326   int   i;
327   int   m;
328   float s;
329   float maxr=-1;
330   float proj;
331   float xm;
332   float L2_m;
333   float theta;
334   float cg;              /* Companded gain of x*/
335   float cgq;
336   float cgr;             /* Companded gain of r*/
337   int qt;
338   int K;
339   float lambda;
340   OD_ASSERT(N>1);
341
342   /* Just some calibration -- should eventually go away */
343   Q=pow(_Q*1.3,GAIN_EXP_1); /* Converts Q to the "companded domain" */
344
345   /* High rate predicts that the constant should be log(2)/6 = 0.115, but in
346      practice, it should be lower. */
347   lambda = 0.10*Q*Q;
348
349   for(i=0;i<N;i++){
350     scale[i]=_scale[i];
351     scale_1[i]=1./scale[i];
352   }
353
354   L2x=0;
355   for(i=0;i<N;i++){
356     x[i]=_x[i];
357     r[i]=_r[i];
358     L2x+=x[i]*x[i];
359   }
360
361   g=sqrt(L2x);
362
363   L2r=0;
364   for(i=0;i<N;i++){
365     L2r+=r[i]*r[i];
366   }
367   gr=sqrt(L2r);
368
369   /* compand gains */
370   cg = pow(g,GAIN_EXP_1)/Q;
371   cgr = pow(gr,GAIN_EXP_1)/Q;
372
373   /* Doing some RDO on the gain, start by rounding down */
374   *qg = floor(cg-cgr);
375   cgq = cgr+*qg;
376   if (cgq<1e-15) cgq=1e-15;
377   /* Cost difference between rounding up or down */
378   if ( 2*(cgq-cg)+1 + (lambda/(Q*Q))*(2. + (N-1)*log2(1+1./(cgq)))  < 0)
379   {
380     (*qg)++;
381     cgq = cgr+*qg;
382   }
383
384   cg = cgr+*qg;
385   if (cg<0)cg=0;
386   /* This is the actual gain the decoder will apply */
387   g = pow(Q*cg, GAIN_EXP);
388
389   /*printf("%f ", xc0);*/
390   /* Pick component with largest magnitude. Not strictly
391    * necessary, but it helps numerical stability */
392   m=0;
393   for(i=0;i<N;i++){
394     if(fabs(r[i])>maxr){
395       maxr=fabs(r[i]);
396       m=i;
397     }
398   }
399
400   /*printf("max r: %f %f %d\n", maxr, r[m], m);*/
401   s=r[m]>0?1:-1;
402
403   /* This turns r into a Householder reflection vector that would reflect
404    * the original r[] to e_m */
405   r[m]+=gr*s;
406
407   L2r=0;
408   for(i=0;i<N;i++){
409     L2r+=r[i]*r[i];
410   }
411
412   /* Apply Householder reflection */
413   proj=0;
414   for(i=0;i<N;i++){
415     proj+=r[i]*x[i];
416   }
417   proj*=2.F/(EPSILON+L2r);
418   for(i=0;i<N;i++){
419     x[i]-=r[i]*proj;
420   }
421
422   xm=-x[m]*s;
423   x[m]=0;
424   L2_m=0;
425   for(i=0;i<N;i++){
426     L2_m+=x[i]*x[i];
427   }
428   theta=atan2(sqrt(L2_m),xm);
429
430   /* Quantize theta and compute K */
431   if (cg>0){
432     float beta;
433     float lambda;
434     float theta_mod;
435     int flip=0;
436
437     if (theta>M_PI/2){
438       flip=1;
439       theta=M_PI-theta;
440     }
441
442 #if 1
443     lambda = 1./(cg*cg);
444     beta = 2*sin(theta)*sin(theta)-lambda;
445     if (beta<=0 /*|| theta<.5*M_PI/qg*/){
446       theta=0;
447     }else{
448       theta = theta-lambda*cos(theta)*sin(theta)/beta;
449       if (theta<0){
450         theta=0;
451       }
452     }
453
454 #else
455     lambda=2;
456     beta = 1-lambda*cos(theta)/(qg*sin(theta));
457     if (beta>0)
458       beta = .5+.5*sqrt(beta);
459     else
460       beta = 0;
461 #endif
462     theta_mod = theta/1.2389 - (theta/1.2389)*(theta/1.2389)/6.;
463     qt = floor(cg*theta_mod);
464     theta_mod = qt/(float)cg;
465     theta = 1.2389 * 3*(1-sqrt(1-theta_mod/1.5));
466     if (flip)
467       theta=M_PI-theta;
468     if (qt==0){
469       K=0;
470     }else{
471       int K_large;
472       K = qt*qt;
473       K_large = sqrt(qt*N);
474       if (K>K_large){
475         K=K_large;
476       }
477     }
478   }else{
479     theta=0;
480     K=0;
481   }
482   /*printf("%d %d\n", K, N);*/
483
484   /*pvq_search(x,NULL,NULL,1,N,K,y,m,0);*/
485   pvq_search_rdo(x,NULL,NULL,1,N,K,y,m,.0*lambda/(cg*cg));
486
487   /*for(i=0;i<N;i++)printf("%d ", y[i]);*/
488   /*if (N==32)for(i=0;i<N;i++)printf("%d ", y[i]);printf("\n");*/
489
490   for(i=0;i<N;i++){
491     x[i]*=sin(theta);
492   }
493   x[m]=-s*cos(theta);
494
495   /* Apply Householder reflection again to get the quantized coefficients */
496   proj=0;
497   for(i=0;i<N;i++){
498     proj+=r[i]*x[i];
499   }
500   proj*=2.F/(EPSILON+L2r);
501   for(i=0;i<N;i++){
502     x[i]-=r[i]*proj;
503   }
504
505   L2x=0;
506   for(i=0;i<N;i++){
507     float tmp=x[i]/* *scale[i]*/;
508     L2x+=tmp*tmp;
509   }
510   g/=EPSILON+sqrt(L2x);
511   for(i=0;i<N;i++){
512     x[i]*=g/* *scale[i]*/;
513   }
514
515   for(i=0;i<N;i++){
516     _x[i]=floor(.5+x[i]);
517   }
518
519   /*printf("xc1=%f\n", xc1);*/
520   /*printf("y[m]=%d\n", y[m]);*/
521   return m;
522 }
523
524 /** PVQ quantizer based on a reference
525  *
526  * This function computes a Householder reflection that causes the reference
527  * to have a single non-zero value. The reflection is then applied to x, and
528  * the result is PVQ-quantized. If the prediction is good, then most of the
529  * PVQ "pulses" end up at the same position as the non-zero value in the
530  * reflected reference. This is probably a good way to quantize intra blocks
531  * with intra prediction.
532  *
533  * @param [in,out] _x     coefficients being quantized (output is the quantized values)
534  * @param [in]     _r     reference
535  * @param [in]     _scale quantization matrix (unused for now)
536  * @param [out]    _y     quantization output (to be encoded)
537  * @param [in]     N      length of vectors _x, _y, and _scale
538  * @param [in]     Q      quantization resolution (lower means higher quality)
539  * @param [out]    qg     quantized gain (to be encoded)
540  *
541  * @retval position that should have the most pulses in _y
542  */
543 int quant_pvq(ogg_int32_t *_x,const ogg_int32_t *_r,
544     ogg_int16_t *_scale,int *y,int N,int _Q,int *qg){
545   float L2x,L2r;
546   float g;               /* L2-norm of x */
547   float gr;              /* L2-norm of r */
548   float x[MAXN];
549   float r[MAXN];
550   float scale[MAXN];
551   float Q;
552   float scale_1[MAXN];
553   int   i;
554   int   m;
555   float s;
556   float maxr=-1;
557   float proj;
558   int   K,ym;
559   float cg;              /* Companded gain of x*/
560   float cgq;
561   float cgr;             /* Companded gain of r*/
562   float lambda;
563   OD_ASSERT(N>1);
564
565   /* Just some calibration -- should eventually go away */
566   Q=pow(_Q*1.3,GAIN_EXP_1); /* Converts Q to the "companded domain" */
567   /* High rate predicts that the constant should be log(2)/6 = 0.115, but in
568      practice, it should be lower. */
569   lambda = 0.10*Q*Q;
570
571   for(i=0;i<N;i++){
572     scale[i]=_scale[i];
573     scale_1[i]=1./scale[i];
574   }
575
576   L2x=0;
577   for(i=0;i<N;i++){
578     x[i]=_x[i];
579     r[i]=_r[i];
580     L2x+=x[i]*x[i];
581   }
582
583   g=sqrt(L2x);
584
585   L2r=0;
586   for(i=0;i<N;i++){
587     L2r+=r[i]*r[i];
588   }
589   gr=sqrt(L2r);
590
591   /*printf("%f\n", g);*/
592   /* compand gain of x and subtract a constant for "pseudo-RDO" purposes */
593   cg = pow(g,GAIN_EXP_1)/Q;
594   if (cg<0)
595     cg=0;
596   /* FIXME: Make that 0.2 adaptive */
597   cgr = pow(gr,GAIN_EXP_1)/Q+.2;
598
599   /* Gain quantization. Round to nearest because we've already reduced cg.
600      Maybe we should have a dead zone */
601 #if 0
602   *qg = floor(.5+cg-cgr);
603 #else
604   /* Doing some RDO on the gain, start by rounding down */
605   *qg = floor(cg-cgr);
606   cgq = cgr+*qg;
607   if (cgq<1e-15) cgq=1e-15;
608   /* Cost difference between rounding up or down */
609   if ( 2*(cgq-cg)+1 + (lambda/(Q*Q))*(2. + (N-1)*log2(1+1./(cgq)))  < 0)
610   {
611     (*qg)++;
612     cgq = cgr+*qg;
613   }
614 #endif
615   cg = cgr+*qg;
616   if (cg<0)cg=0;
617   /* This is the actual gain the decoder will apply */
618   g = pow(Q*cg, GAIN_EXP);
619
620   /* Compute the number of pulses K based on the quantized gain -- still work
621      to do here */
622 #if 0
623   K = floor(.5+ 1.*(M_PI/2)*(cg)/GAIN_EXP );
624 #else
625   if (cg==0){
626     K=0;
627   }else{
628     int K_large;
629     K = floor(.5+0.6*cg*cg);
630     K_large = floor(.5+1.5*cg*sqrt(N/2));
631     if (K>K_large){
632       K=K_large;
633     }
634   }
635 #endif
636   if (K==0)
637   {
638     g=0;
639     cg=0;
640   }
641   /*if(N==16)printf("%d ", qg);*/
642
643   /*if (g>100000 && g0>100000)
644     printf("%f %f\n", g, g0);*/
645   /*for(i=0;i<N;i++){
646     x[i]*=scale_1[i];
647     r[i]*=scale_1[i];
648   }*/
649
650   L2r=0;
651   for(i=0;i<N;i++){
652     L2r+=r[i]*r[i];
653   }
654   gr=sqrt(L2r);
655
656   /* This is where we can skip */
657   /*
658   if (K<=0)
659   {
660     for(i=0;i<N;i++){
661       _x[i]=_r[i];
662     }
663     return 0;
664   }
665 */
666
667   /*printf("%f ", xc0);*/
668   /* Pick component with largest magnitude. Not strictly
669    * necessary, but it helps numerical stability */
670   m=0;
671   for(i=0;i<N;i++){
672     if(fabs(r[i])>maxr){
673       maxr=fabs(r[i]);
674       m=i;
675     }
676   }
677
678   /*printf("max r: %f %f %d\n", maxr, r[m], m);*/
679   s=r[m]>0?1:-1;
680
681   /* This turns r into a Householder reflection vector that would reflect
682    * the original r[] to e_m */
683   r[m]+=gr*s;
684
685   L2r=0;
686   for(i=0;i<N;i++){
687     L2r+=r[i]*r[i];
688   }
689
690   /* Apply Householder reflection */
691   proj=0;
692   for(i=0;i<N;i++){
693     proj+=r[i]*x[i];
694   }
695   proj*=2.F/(EPSILON+L2r);
696   for(i=0;i<N;i++){
697     x[i]-=r[i]*proj;
698   }
699
700   /*printf("%d ", qt);*/
701   /*printf("%d %d\n", K, N);*/
702
703   /* Normalize lambda for quantizing on the unit circle */
704   /* FIXME: See if we can avoid setting lambda to zero! */
705   pvq_search_rdo(x,NULL,NULL,1,N,K,y,m,.0*lambda/(cg*cg));
706   /*printf("%d ", K-abs(y[m]));*/
707   /*for(i=0;i<N;i++)printf("%d ", (m==i)?0:y[i]);*/
708
709   /*printf("%d %d\n", K-y[m], N);*/
710
711   /* Apply Householder reflection again to get the quantized coefficients */
712   proj=0;
713   for(i=0;i<N;i++){
714     proj+=r[i]*x[i];
715   }
716   proj*=2.F/(EPSILON+L2r);
717   for(i=0;i<N;i++){
718     x[i]-=r[i]*proj;
719   }
720
721   L2x=0;
722   for(i=0;i<N;i++){
723     float tmp=x[i]/* *scale[i]*/;
724     L2x+=tmp*tmp;
725   }
726   g/=EPSILON+sqrt(L2x);
727   for(i=0;i<N;i++){
728     x[i]*=g/* *scale[i]*/;
729   }
730
731   for(i=0;i<N;i++){
732     _x[i]=floor(.5+x[i]);
733   }
734
735   /* Move y[m] to the front */
736   ym = y[m];
737   for (i=m;i>=1;i--)
738     y[i] = y[i-1];
739   /* Make y[0] positive when prediction is good  */
740   y[0] = -ym*s;
741
742   /*printf("%d ", *qg);*/
743   /*printf("xc1=%f\n", xc1);*/
744   /*printf("y[m]=%d\n", y[m]);*/
745   return m;
746 }
747
748 int pvq_unquant_k(const ogg_int32_t *_r,int _n,int _qg, int _scale){
749   int    i;
750   int    vk;
751   double Q;
752   double cgr;
753   Q=pow(_scale*1.3,1./(4./3.));
754   vk=0;
755   for(i=0;i<_n;i++)vk+=_r[i]*_r[i];
756   cgr=pow(sqrt(vk),1./(4./3.))/Q+.2;
757   cgr=cgr+_qg;
758   if(cgr<0)cgr=0;
759   if(cgr==0){
760     vk=0;
761   }else{
762     int K_large;
763     vk=floor(.5+0.6*cgr*cgr);
764     K_large=floor(.5+1.5*cgr*sqrt(_n/2));
765     if(vk>K_large){
766       vk=K_large;
767     }
768   }
769   return vk;
770 }
771
772 /** PVQ dequantizer based on a reference
773  *
774  * @param [in,out] _x     coefficients being dequantized (output is the dequantized values)
775  * @param [in]     _r     reference
776  * @param [in]     _scale quantization matrix (unused for now)
777  * @param [in]     N      length of vectors _x, _y, and _scale
778  * @param [in]     Q      quantization resolution (lower means higher quality)
779  * @param [in]    qg     quantized gain
780  *
781  */
782 void dequant_pvq(ogg_int32_t *_x,const ogg_int32_t *_r,
783     ogg_int16_t *_scale,int N,int _Q,int qg){
784   float L2x,L2r;
785   float g;               /* L2-norm of x */
786   float gr;              /* L2-norm of r */
787   float x[MAXN];
788   float r[MAXN];
789   float scale[MAXN];
790   float Q;
791   float scale_1[MAXN];
792   int   i;
793   int   m;
794   float s;
795   float maxr=-1;
796   float proj;
797   int   xm;
798   float cg;              /* Companded gain of x*/
799   float cgr;             /* Companded gain of r*/
800   OD_ASSERT(N>1);
801
802   /* Just some calibration -- should eventually go away */
803   Q=pow(_Q*1.3,GAIN_EXP_1); /* Converts Q to the "companded domain" */
804   /* High rate predicts that the constant should be log(2)/6 = 0.115, but in
805      practice, it should be lower. */
806
807   for(i=0;i<N;i++){
808     scale[i]=_scale[i];
809     scale_1[i]=1./scale[i];
810   }
811
812   L2r=0;
813   for(i=0;i<N;i++){
814     x[i]=_x[i];
815     r[i]=_r[i];
816     L2r+=r[i]*r[i];
817   }
818   gr=sqrt(L2r);
819   cgr = pow(gr,GAIN_EXP_1)/Q+.2;
820   cg = cgr+qg;
821   if (cg<0)cg=0;
822   g = pow(Q*cg, GAIN_EXP);
823
824   /* Pick component with largest magnitude. Not strictly
825    * necessary, but it helps numerical stability */
826   m=0;
827   for(i=0;i<N;i++){
828     if(fabs(r[i])>maxr){
829       maxr=fabs(r[i]);
830       m=i;
831     }
832   }
833   s=r[m]>0?1:-1;
834
835   /* This turns r into a Householder reflection vector that would reflect
836    * the original r[] to e_m */
837   r[m]+=gr*s;
838
839   L2r=0;
840   for(i=0;i<N;i++){
841     L2r+=r[i]*r[i];
842   }
843
844   /* Move x[m] back */
845   xm = x[0];
846   for (i=0;i<m;i++)
847     x[i] = x[i+1];
848   /* x[0] is positive when prediction is good  */
849   x[m] = -xm*s;
850
851   /* Apply Householder reflection to the quantized coefficients */
852   proj=0;
853   for(i=0;i<N;i++){
854     proj+=r[i]*x[i];
855   }
856   proj*=2.F/(EPSILON+L2r);
857   for(i=0;i<N;i++){
858     x[i]-=r[i]*proj;
859   }
860
861   L2x=0;
862   for(i=0;i<N;i++){
863     float tmp=x[i]/* *scale[i]*/;
864     L2x+=tmp*tmp;
865   }
866   g/=EPSILON+sqrt(L2x);
867   for(i=0;i<N;i++){
868     x[i]*=g/* *scale[i]*/;
869   }
870
871   for(i=0;i<N;i++){
872     _x[i]=floor(.5+x[i]);
873   }
874 }
875
876 /** PVQ quantizer with no reference
877  *
878  * This quantizer just applies companding on the gain and does the PVQ search
879  *
880  * @param [in,out] _x     coefficients being quantized (output is the quantized values)
881  * @param [in]     gr     reference gain (i.e. predicted valud of the gain)
882  * @param [in]     _scale quantization matrix (unused for now)
883  * @param [out]    _y     quantization output (to be encoded)
884  * @param [in]     N      length of vectors _x, _y, and _scale
885  * @param [in]     Q      quantization resolution (lower means higher quality)
886  *
887  * @retval quantized gain (to be encoded)
888  */
889 int quant_pvq_noref(ogg_int32_t *_x,float gr,
890     ogg_int16_t *_scale,int *y,int N,int Q){
891   float L2x;
892   float g;
893   float x[MAXN];
894   float scale[MAXN];
895   float scale_1[MAXN];
896   int   i;
897   int K;
898   float cg, cgr;
899   int qg;
900   OD_ASSERT(N>1);
901
902   Q *= 1.52;
903
904   for(i=0;i<N;i++){
905     scale[i]=_scale[i];
906     scale_1[i]=1./scale[i];
907   }
908
909   L2x=0;
910   for(i=0;i<N;i++){
911     x[i]=_x[i];
912     L2x+=x[i]*x[i];
913   }
914
915   g=sqrt(L2x);
916
917
918   cg = pow(g/Q,GAIN_EXP_1)-1.;
919   if (cg<0)
920     cg=0;
921   cgr = pow(gr/Q,GAIN_EXP_1);
922
923   qg = floor(.5+cg-cgr);
924   cg = cgr+qg;
925   if (cg<0)cg=0;
926   g = Q*pow(cg, GAIN_EXP);
927   qg = floor(.5+cg);
928
929   K = floor(.5+cg*cg);
930   pvq_search(x,NULL,NULL,1,N,K,y);
931
932   L2x=0;
933   for(i=0;i<N;i++){
934     float tmp=x[i]/* *scale[i]*/;
935     L2x+=tmp*tmp;
936   }
937   g/=EPSILON+sqrt(L2x);
938   for(i=0;i<N;i++){
939     x[i]*=g/* *scale[i]*/;
940   }
941
942   for(i=0;i<N;i++){
943     _x[i]=floor(.5+x[i]);
944   }
945
946   return qg;
947 }
948
949 int quant_scalar(ogg_int32_t *_x,const ogg_int32_t *_r,
950     ogg_int16_t *_scale,int *y,int N,int Q, od_adapt_ctx *_adapt){
951   int i;
952   float Q2, Q2_1;
953   int K;
954   float lambda;
955   int Kn;
956   OD_ASSERT(N>0);
957
958   K=0;
959   Q2=1.4*Q;
960   lambda = .115;
961   Q2_1=1./Q2;
962   for (i=0;i<N;i++)
963   {
964     float tmp;
965     _x[i] -= _r[i];
966     tmp=Q2_1*_x[i];
967     y[i]=floor(.5+tmp);
968     K+=abs(y[i]);
969   }
970
971
972 #if 0
973   /* Attempt at modelling the rate more accurately -- doesn't work for now */
974   mean_k_q8=_adapt->mean_k_q8;
975   mean_sum_ex_q8=_adapt->mean_sum_ex_q8;
976   if(mean_k_q8<1<<23)expQ8=256*mean_k_q8/(1+mean_sum_ex_q8);
977   else expQ8=mean_k_q8/(1+(mean_sum_ex_q8>>8));
978
979   Kn=0;
980   for (i=N-1;i>=0;i--)
981   {
982     float e;
983     int Ex;
984     int decay;
985     float rate0, rate_1;
986     float cost0,cost_1;
987     float ay;
988
989     ay=abs(y[i]);
990     if (y[i]==0)
991       continue;
992     e = Q2_1*_x[i]-y[i];
993
994     Kn += abs(y[i]);
995     Ex=(2*expQ8*Kn+(N-i))/(2*(N-i));
996     if(Ex>Kn*128)
997       Ex=Kn*128;
998
999     decay = OD_MINI(255,(int)((256*Ex/(Ex+256) + 8*Ex*Ex/(256*(Kn+2)*(Kn)*(Kn)))));
1000     /*printf("(%d %d %d %d ", expQ8, Kn, Ex, decay);*/
1001     rate0 = -log2(pow(decay/256.,ay)*(1-decay/256.)/(1-pow(decay/256.,Kn+1)));
1002     if(Kn==1){
1003       rate_1=0;
1004     } else {
1005       Ex=(2*expQ8*(Kn-1)+(N-i))/(2*(N-i));
1006       if(Ex>(Kn-1)*256)
1007         Ex=(Kn-1)*256;
1008
1009       decay = OD_MINI(255,(int)((256*Ex/(Ex+256) + 8*Ex*Ex/(256*(Kn+1)*(Kn-1)*(Kn-1)))));
1010       /*printf("%d ", decay);*/
1011       rate_1 = -log2(pow(decay/256.,ay-1)*(1-decay/256.)/(1-pow(decay/256.,Kn)));
1012     }
1013
1014     cost0 = e*e + lambda*rate0;
1015     cost_1 = (e+1)*(e+1) + lambda*rate_1;
1016     /*printf("%f %f %f %f) ", e*e, (e+1)*(e+1), rate0, rate_1);*/
1017     if (cost_1<cost0)
1018     {
1019       if (y[i]>0)
1020         y[i]--;
1021       else
1022         y[i]++;
1023       Kn--;
1024     }
1025   }
1026   /*printf("\n");*/
1027 #else
1028   if (K!=0) {
1029     float alpha;
1030     float r, r_1;
1031     float E0;
1032     float bias;
1033     alpha = (float)_adapt->mean[OD_ADAPT_K_Q8]/(float)_adapt->mean[OD_ADAPT_SUM_EX_Q8];
1034     if(alpha<1)
1035       alpha=1;
1036     r = 1-alpha/N;
1037     if (r>.1)
1038       r = .1;
1039     E0 = alpha*K/N;
1040     r_1 = 1./r;
1041     bias = -lambda/E0;
1042     if (bias<-.49)
1043       bias=-.49;
1044     K=0;
1045     for (i=0;i<N;i++)
1046     {
1047       float tmp;
1048       tmp=Q2_1*_x[i];
1049       if (tmp>0)
1050         tmp+=bias;
1051       else
1052         tmp-=bias;
1053       y[i]=floor(.5+tmp);
1054       K+=abs(y[i]);
1055       bias *= r_1;
1056       if (bias<-.49)
1057         bias=-.49;
1058     }
1059   }
1060   Kn=K;
1061 #endif
1062
1063   for (i=0;i<N;i++)
1064     _x[i]=_r[i]+Q2*y[i];
1065   return Kn;
1066 }