4c43cc51fbe0381bf28a726ec024670e0d6c0f82
[speexdsp.git] / libspeex / smallft.c
1 /********************************************************************
2  *                                                                  *
3  * THIS FILE IS PART OF THE OggVorbis SOFTWARE CODEC SOURCE CODE.   *
4  * USE, DISTRIBUTION AND REPRODUCTION OF THIS LIBRARY SOURCE IS     *
5  * GOVERNED BY A BSD-STYLE SOURCE LICENSE INCLUDED WITH THIS SOURCE *
6  * IN 'COPYING'. PLEASE READ THESE TERMS BEFORE DISTRIBUTING.       *
7  *                                                                  *
8  * THE OggVorbis SOURCE CODE IS (C) COPYRIGHT 1994-2001             *
9  * by the XIPHOPHORUS Company http://www.xiph.org/                  *
10  *                                                                  *
11  ********************************************************************
12
13  function: *unnormalized* fft transform
14  last mod: $Id: smallft.c,v 1.11 2003/10/08 04:53:18 jm Exp $
15
16  ********************************************************************/
17
18 /* FFT implementation from OggSquish, minus cosine transforms,
19  * minus all but radix 2/4 case.  In Vorbis we only need this
20  * cut-down version.
21  *
22  * To do more than just power-of-two sized vectors, see the full
23  * version I wrote for NetLib.
24  *
25  * Note that the packing is a little strange; rather than the FFT r/i
26  * packing following R_0, I_n, R_1, I_1, R_2, I_2 ... R_n-1, I_n-1,
27  * it follows R_0, R_1, I_1, R_2, I_2 ... R_n-1, I_n-1, I_n like the
28  * FORTRAN version
29  */
30
31 #include <math.h>
32 #include "smallft.h"
33 #include "misc.h"
34
35 static void drfti1(int n, float *wa, int *ifac){
36   static int ntryh[4] = { 4,2,3,5 };
37   static float tpi = 6.28318530717958648f;
38   float arg,argh,argld,fi;
39   int ntry=0,i,j=-1;
40   int k1, l1, l2, ib;
41   int ld, ii, ip, is, nq, nr;
42   int ido, ipm, nfm1;
43   int nl=n;
44   int nf=0;
45
46  L101:
47   j++;
48   if (j < 4)
49     ntry=ntryh[j];
50   else
51     ntry+=2;
52
53  L104:
54   nq=nl/ntry;
55   nr=nl-ntry*nq;
56   if (nr!=0) goto L101;
57
58   nf++;
59   ifac[nf+1]=ntry;
60   nl=nq;
61   if(ntry!=2)goto L107;
62   if(nf==1)goto L107;
63
64   for (i=1;i<nf;i++){
65     ib=nf-i+1;
66     ifac[ib+1]=ifac[ib];
67   }
68   ifac[2] = 2;
69
70  L107:
71   if(nl!=1)goto L104;
72   ifac[0]=n;
73   ifac[1]=nf;
74   argh=tpi/n;
75   is=0;
76   nfm1=nf-1;
77   l1=1;
78
79   if(nfm1==0)return;
80
81   for (k1=0;k1<nfm1;k1++){
82     ip=ifac[k1+2];
83     ld=0;
84     l2=l1*ip;
85     ido=n/l2;
86     ipm=ip-1;
87
88     for (j=0;j<ipm;j++){
89       ld+=l1;
90       i=is;
91       argld=(float)ld*argh;
92       fi=0.f;
93       for (ii=2;ii<ido;ii+=2){
94         fi+=1.f;
95         arg=fi*argld;
96         wa[i++]=cos(arg);
97         wa[i++]=sin(arg);
98       }
99       is+=ido;
100     }
101     l1=l2;
102   }
103 }
104
105 static void fdrffti(int n, float *wsave, int *ifac){
106
107   if (n == 1) return;
108   drfti1(n, wsave+n, ifac);
109 }
110
111 static void dradf2(int ido,int l1,float *cc,float *ch,float *wa1){
112   int i,k;
113   float ti2,tr2;
114   int t0,t1,t2,t3,t4,t5,t6;
115
116   t1=0;
117   t0=(t2=l1*ido);
118   t3=ido<<1;
119   for(k=0;k<l1;k++){
120     ch[t1<<1]=cc[t1]+cc[t2];
121     ch[(t1<<1)+t3-1]=cc[t1]-cc[t2];
122     t1+=ido;
123     t2+=ido;
124   }
125     
126   if(ido<2)return;
127   if(ido==2)goto L105;
128
129   t1=0;
130   t2=t0;
131   for(k=0;k<l1;k++){
132     t3=t2;
133     t4=(t1<<1)+(ido<<1);
134     t5=t1;
135     t6=t1+t1;
136     for(i=2;i<ido;i+=2){
137       t3+=2;
138       t4-=2;
139       t5+=2;
140       t6+=2;
141       tr2=wa1[i-2]*cc[t3-1]+wa1[i-1]*cc[t3];
142       ti2=wa1[i-2]*cc[t3]-wa1[i-1]*cc[t3-1];
143       ch[t6]=cc[t5]+ti2;
144       ch[t4]=ti2-cc[t5];
145       ch[t6-1]=cc[t5-1]+tr2;
146       ch[t4-1]=cc[t5-1]-tr2;
147     }
148     t1+=ido;
149     t2+=ido;
150   }
151
152   if(ido%2==1)return;
153
154  L105:
155   t3=(t2=(t1=ido)-1);
156   t2+=t0;
157   for(k=0;k<l1;k++){
158     ch[t1]=-cc[t2];
159     ch[t1-1]=cc[t3];
160     t1+=ido<<1;
161     t2+=ido;
162     t3+=ido;
163   }
164 }
165
166 static void dradf4(int ido,int l1,float *cc,float *ch,float *wa1,
167             float *wa2,float *wa3){
168   static float hsqt2 = .70710678118654752f;
169   int i,k,t0,t1,t2,t3,t4,t5,t6;
170   float ci2,ci3,ci4,cr2,cr3,cr4,ti1,ti2,ti3,ti4,tr1,tr2,tr3,tr4;
171   t0=l1*ido;
172   
173   t1=t0;
174   t4=t1<<1;
175   t2=t1+(t1<<1);
176   t3=0;
177
178   for(k=0;k<l1;k++){
179     tr1=cc[t1]+cc[t2];
180     tr2=cc[t3]+cc[t4];
181
182     ch[t5=t3<<2]=tr1+tr2;
183     ch[(ido<<2)+t5-1]=tr2-tr1;
184     ch[(t5+=(ido<<1))-1]=cc[t3]-cc[t4];
185     ch[t5]=cc[t2]-cc[t1];
186
187     t1+=ido;
188     t2+=ido;
189     t3+=ido;
190     t4+=ido;
191   }
192
193   if(ido<2)return;
194   if(ido==2)goto L105;
195
196
197   t1=0;
198   for(k=0;k<l1;k++){
199     t2=t1;
200     t4=t1<<2;
201     t5=(t6=ido<<1)+t4;
202     for(i=2;i<ido;i+=2){
203       t3=(t2+=2);
204       t4+=2;
205       t5-=2;
206
207       t3+=t0;
208       cr2=wa1[i-2]*cc[t3-1]+wa1[i-1]*cc[t3];
209       ci2=wa1[i-2]*cc[t3]-wa1[i-1]*cc[t3-1];
210       t3+=t0;
211       cr3=wa2[i-2]*cc[t3-1]+wa2[i-1]*cc[t3];
212       ci3=wa2[i-2]*cc[t3]-wa2[i-1]*cc[t3-1];
213       t3+=t0;
214       cr4=wa3[i-2]*cc[t3-1]+wa3[i-1]*cc[t3];
215       ci4=wa3[i-2]*cc[t3]-wa3[i-1]*cc[t3-1];
216
217       tr1=cr2+cr4;
218       tr4=cr4-cr2;
219       ti1=ci2+ci4;
220       ti4=ci2-ci4;
221
222       ti2=cc[t2]+ci3;
223       ti3=cc[t2]-ci3;
224       tr2=cc[t2-1]+cr3;
225       tr3=cc[t2-1]-cr3;
226
227       ch[t4-1]=tr1+tr2;
228       ch[t4]=ti1+ti2;
229
230       ch[t5-1]=tr3-ti4;
231       ch[t5]=tr4-ti3;
232
233       ch[t4+t6-1]=ti4+tr3;
234       ch[t4+t6]=tr4+ti3;
235
236       ch[t5+t6-1]=tr2-tr1;
237       ch[t5+t6]=ti1-ti2;
238     }
239     t1+=ido;
240   }
241   if(ido&1)return;
242
243  L105:
244   
245   t2=(t1=t0+ido-1)+(t0<<1);
246   t3=ido<<2;
247   t4=ido;
248   t5=ido<<1;
249   t6=ido;
250
251   for(k=0;k<l1;k++){
252     ti1=-hsqt2*(cc[t1]+cc[t2]);
253     tr1=hsqt2*(cc[t1]-cc[t2]);
254
255     ch[t4-1]=tr1+cc[t6-1];
256     ch[t4+t5-1]=cc[t6-1]-tr1;
257
258     ch[t4]=ti1-cc[t1+t0];
259     ch[t4+t5]=ti1+cc[t1+t0];
260
261     t1+=ido;
262     t2+=ido;
263     t4+=t3;
264     t6+=ido;
265   }
266 }
267
268 static void dradfg(int ido,int ip,int l1,int idl1,float *cc,float *c1,
269                           float *c2,float *ch,float *ch2,float *wa){
270
271   static float tpi=6.283185307179586f;
272   int idij,ipph,i,j,k,l,ic,ik,is;
273   int t0,t1,t2,t3,t4,t5,t6,t7,t8,t9,t10;
274   float dc2,ai1,ai2,ar1,ar2,ds2;
275   int nbd;
276   float dcp,arg,dsp,ar1h,ar2h;
277   int idp2,ipp2;
278   
279   arg=tpi/(float)ip;
280   dcp=cos(arg);
281   dsp=sin(arg);
282   ipph=(ip+1)>>1;
283   ipp2=ip;
284   idp2=ido;
285   nbd=(ido-1)>>1;
286   t0=l1*ido;
287   t10=ip*ido;
288
289   if(ido==1)goto L119;
290   for(ik=0;ik<idl1;ik++)ch2[ik]=c2[ik];
291
292   t1=0;
293   for(j=1;j<ip;j++){
294     t1+=t0;
295     t2=t1;
296     for(k=0;k<l1;k++){
297       ch[t2]=c1[t2];
298       t2+=ido;
299     }
300   }
301
302   is=-ido;
303   t1=0;
304   if(nbd>l1){
305     for(j=1;j<ip;j++){
306       t1+=t0;
307       is+=ido;
308       t2= -ido+t1;
309       for(k=0;k<l1;k++){
310         idij=is-1;
311         t2+=ido;
312         t3=t2;
313         for(i=2;i<ido;i+=2){
314           idij+=2;
315           t3+=2;
316           ch[t3-1]=wa[idij-1]*c1[t3-1]+wa[idij]*c1[t3];
317           ch[t3]=wa[idij-1]*c1[t3]-wa[idij]*c1[t3-1];
318         }
319       }
320     }
321   }else{
322
323     for(j=1;j<ip;j++){
324       is+=ido;
325       idij=is-1;
326       t1+=t0;
327       t2=t1;
328       for(i=2;i<ido;i+=2){
329         idij+=2;
330         t2+=2;
331         t3=t2;
332         for(k=0;k<l1;k++){
333           ch[t3-1]=wa[idij-1]*c1[t3-1]+wa[idij]*c1[t3];
334           ch[t3]=wa[idij-1]*c1[t3]-wa[idij]*c1[t3-1];
335           t3+=ido;
336         }
337       }
338     }
339   }
340
341   t1=0;
342   t2=ipp2*t0;
343   if(nbd<l1){
344     for(j=1;j<ipph;j++){
345       t1+=t0;
346       t2-=t0;
347       t3=t1;
348       t4=t2;
349       for(i=2;i<ido;i+=2){
350         t3+=2;
351         t4+=2;
352         t5=t3-ido;
353         t6=t4-ido;
354         for(k=0;k<l1;k++){
355           t5+=ido;
356           t6+=ido;
357           c1[t5-1]=ch[t5-1]+ch[t6-1];
358           c1[t6-1]=ch[t5]-ch[t6];
359           c1[t5]=ch[t5]+ch[t6];
360           c1[t6]=ch[t6-1]-ch[t5-1];
361         }
362       }
363     }
364   }else{
365     for(j=1;j<ipph;j++){
366       t1+=t0;
367       t2-=t0;
368       t3=t1;
369       t4=t2;
370       for(k=0;k<l1;k++){
371         t5=t3;
372         t6=t4;
373         for(i=2;i<ido;i+=2){
374           t5+=2;
375           t6+=2;
376           c1[t5-1]=ch[t5-1]+ch[t6-1];
377           c1[t6-1]=ch[t5]-ch[t6];
378           c1[t5]=ch[t5]+ch[t6];
379           c1[t6]=ch[t6-1]-ch[t5-1];
380         }
381         t3+=ido;
382         t4+=ido;
383       }
384     }
385   }
386
387 L119:
388   for(ik=0;ik<idl1;ik++)c2[ik]=ch2[ik];
389
390   t1=0;
391   t2=ipp2*idl1;
392   for(j=1;j<ipph;j++){
393     t1+=t0;
394     t2-=t0;
395     t3=t1-ido;
396     t4=t2-ido;
397     for(k=0;k<l1;k++){
398       t3+=ido;
399       t4+=ido;
400       c1[t3]=ch[t3]+ch[t4];
401       c1[t4]=ch[t4]-ch[t3];
402     }
403   }
404
405   ar1=1.f;
406   ai1=0.f;
407   t1=0;
408   t2=ipp2*idl1;
409   t3=(ip-1)*idl1;
410   for(l=1;l<ipph;l++){
411     t1+=idl1;
412     t2-=idl1;
413     ar1h=dcp*ar1-dsp*ai1;
414     ai1=dcp*ai1+dsp*ar1;
415     ar1=ar1h;
416     t4=t1;
417     t5=t2;
418     t6=t3;
419     t7=idl1;
420
421     for(ik=0;ik<idl1;ik++){
422       ch2[t4++]=c2[ik]+ar1*c2[t7++];
423       ch2[t5++]=ai1*c2[t6++];
424     }
425
426     dc2=ar1;
427     ds2=ai1;
428     ar2=ar1;
429     ai2=ai1;
430
431     t4=idl1;
432     t5=(ipp2-1)*idl1;
433     for(j=2;j<ipph;j++){
434       t4+=idl1;
435       t5-=idl1;
436
437       ar2h=dc2*ar2-ds2*ai2;
438       ai2=dc2*ai2+ds2*ar2;
439       ar2=ar2h;
440
441       t6=t1;
442       t7=t2;
443       t8=t4;
444       t9=t5;
445       for(ik=0;ik<idl1;ik++){
446         ch2[t6++]+=ar2*c2[t8++];
447         ch2[t7++]+=ai2*c2[t9++];
448       }
449     }
450   }
451
452   t1=0;
453   for(j=1;j<ipph;j++){
454     t1+=idl1;
455     t2=t1;
456     for(ik=0;ik<idl1;ik++)ch2[ik]+=c2[t2++];
457   }
458
459   if(ido<l1)goto L132;
460
461   t1=0;
462   t2=0;
463   for(k=0;k<l1;k++){
464     t3=t1;
465     t4=t2;
466     for(i=0;i<ido;i++)cc[t4++]=ch[t3++];
467     t1+=ido;
468     t2+=t10;
469   }
470
471   goto L135;
472
473  L132:
474   for(i=0;i<ido;i++){
475     t1=i;
476     t2=i;
477     for(k=0;k<l1;k++){
478       cc[t2]=ch[t1];
479       t1+=ido;
480       t2+=t10;
481     }
482   }
483
484  L135:
485   t1=0;
486   t2=ido<<1;
487   t3=0;
488   t4=ipp2*t0;
489   for(j=1;j<ipph;j++){
490
491     t1+=t2;
492     t3+=t0;
493     t4-=t0;
494
495     t5=t1;
496     t6=t3;
497     t7=t4;
498
499     for(k=0;k<l1;k++){
500       cc[t5-1]=ch[t6];
501       cc[t5]=ch[t7];
502       t5+=t10;
503       t6+=ido;
504       t7+=ido;
505     }
506   }
507
508   if(ido==1)return;
509   if(nbd<l1)goto L141;
510
511   t1=-ido;
512   t3=0;
513   t4=0;
514   t5=ipp2*t0;
515   for(j=1;j<ipph;j++){
516     t1+=t2;
517     t3+=t2;
518     t4+=t0;
519     t5-=t0;
520     t6=t1;
521     t7=t3;
522     t8=t4;
523     t9=t5;
524     for(k=0;k<l1;k++){
525       for(i=2;i<ido;i+=2){
526         ic=idp2-i;
527         cc[i+t7-1]=ch[i+t8-1]+ch[i+t9-1];
528         cc[ic+t6-1]=ch[i+t8-1]-ch[i+t9-1];
529         cc[i+t7]=ch[i+t8]+ch[i+t9];
530         cc[ic+t6]=ch[i+t9]-ch[i+t8];
531       }
532       t6+=t10;
533       t7+=t10;
534       t8+=ido;
535       t9+=ido;
536     }
537   }
538   return;
539
540  L141:
541
542   t1=-ido;
543   t3=0;
544   t4=0;
545   t5=ipp2*t0;
546   for(j=1;j<ipph;j++){
547     t1+=t2;
548     t3+=t2;
549     t4+=t0;
550     t5-=t0;
551     for(i=2;i<ido;i+=2){
552       t6=idp2+t1-i;
553       t7=i+t3;
554       t8=i+t4;
555       t9=i+t5;
556       for(k=0;k<l1;k++){
557         cc[t7-1]=ch[t8-1]+ch[t9-1];
558         cc[t6-1]=ch[t8-1]-ch[t9-1];
559         cc[t7]=ch[t8]+ch[t9];
560         cc[t6]=ch[t9]-ch[t8];
561         t6+=t10;
562         t7+=t10;
563         t8+=ido;
564         t9+=ido;
565       }
566     }
567   }
568 }
569
570 static void drftf1(int n,float *c,float *ch,float *wa,int *ifac){
571   int i,k1,l1,l2;
572   int na,kh,nf;
573   int ip,iw,ido,idl1,ix2,ix3;
574
575   nf=ifac[1];
576   na=1;
577   l2=n;
578   iw=n;
579
580   for(k1=0;k1<nf;k1++){
581     kh=nf-k1;
582     ip=ifac[kh+1];
583     l1=l2/ip;
584     ido=n/l2;
585     idl1=ido*l1;
586     iw-=(ip-1)*ido;
587     na=1-na;
588
589     if(ip!=4)goto L102;
590
591     ix2=iw+ido;
592     ix3=ix2+ido;
593     if(na!=0)
594       dradf4(ido,l1,ch,c,wa+iw-1,wa+ix2-1,wa+ix3-1);
595     else
596       dradf4(ido,l1,c,ch,wa+iw-1,wa+ix2-1,wa+ix3-1);
597     goto L110;
598
599  L102:
600     if(ip!=2)goto L104;
601     if(na!=0)goto L103;
602
603     dradf2(ido,l1,c,ch,wa+iw-1);
604     goto L110;
605
606   L103:
607     dradf2(ido,l1,ch,c,wa+iw-1);
608     goto L110;
609
610   L104:
611     if(ido==1)na=1-na;
612     if(na!=0)goto L109;
613
614     dradfg(ido,ip,l1,idl1,c,c,c,ch,ch,wa+iw-1);
615     na=1;
616     goto L110;
617
618   L109:
619     dradfg(ido,ip,l1,idl1,ch,ch,ch,c,c,wa+iw-1);
620     na=0;
621
622   L110:
623     l2=l1;
624   }
625
626   if(na==1)return;
627
628   for(i=0;i<n;i++)c[i]=ch[i];
629 }
630
631 static void dradb2(int ido,int l1,float *cc,float *ch,float *wa1){
632   int i,k,t0,t1,t2,t3,t4,t5,t6;
633   float ti2,tr2;
634
635   t0=l1*ido;
636   
637   t1=0;
638   t2=0;
639   t3=(ido<<1)-1;
640   for(k=0;k<l1;k++){
641     ch[t1]=cc[t2]+cc[t3+t2];
642     ch[t1+t0]=cc[t2]-cc[t3+t2];
643     t2=(t1+=ido)<<1;
644   }
645
646   if(ido<2)return;
647   if(ido==2)goto L105;
648
649   t1=0;
650   t2=0;
651   for(k=0;k<l1;k++){
652     t3=t1;
653     t5=(t4=t2)+(ido<<1);
654     t6=t0+t1;
655     for(i=2;i<ido;i+=2){
656       t3+=2;
657       t4+=2;
658       t5-=2;
659       t6+=2;
660       ch[t3-1]=cc[t4-1]+cc[t5-1];
661       tr2=cc[t4-1]-cc[t5-1];
662       ch[t3]=cc[t4]-cc[t5];
663       ti2=cc[t4]+cc[t5];
664       ch[t6-1]=wa1[i-2]*tr2-wa1[i-1]*ti2;
665       ch[t6]=wa1[i-2]*ti2+wa1[i-1]*tr2;
666     }
667     t2=(t1+=ido)<<1;
668   }
669
670   if(ido%2==1)return;
671
672 L105:
673   t1=ido-1;
674   t2=ido-1;
675   for(k=0;k<l1;k++){
676     ch[t1]=cc[t2]+cc[t2];
677     ch[t1+t0]=-(cc[t2+1]+cc[t2+1]);
678     t1+=ido;
679     t2+=ido<<1;
680   }
681 }
682
683 static void dradb3(int ido,int l1,float *cc,float *ch,float *wa1,
684                           float *wa2){
685   static float taur = -.5f;
686   static float taui = .8660254037844386f;
687   int i,k,t0,t1,t2,t3,t4,t5,t6,t7,t8,t9,t10;
688   float ci2,ci3,di2,di3,cr2,cr3,dr2,dr3,ti2,tr2;
689   t0=l1*ido;
690
691   t1=0;
692   t2=t0<<1;
693   t3=ido<<1;
694   t4=ido+(ido<<1);
695   t5=0;
696   for(k=0;k<l1;k++){
697     tr2=cc[t3-1]+cc[t3-1];
698     cr2=cc[t5]+(taur*tr2);
699     ch[t1]=cc[t5]+tr2;
700     ci3=taui*(cc[t3]+cc[t3]);
701     ch[t1+t0]=cr2-ci3;
702     ch[t1+t2]=cr2+ci3;
703     t1+=ido;
704     t3+=t4;
705     t5+=t4;
706   }
707
708   if(ido==1)return;
709
710   t1=0;
711   t3=ido<<1;
712   for(k=0;k<l1;k++){
713     t7=t1+(t1<<1);
714     t6=(t5=t7+t3);
715     t8=t1;
716     t10=(t9=t1+t0)+t0;
717
718     for(i=2;i<ido;i+=2){
719       t5+=2;
720       t6-=2;
721       t7+=2;
722       t8+=2;
723       t9+=2;
724       t10+=2;
725       tr2=cc[t5-1]+cc[t6-1];
726       cr2=cc[t7-1]+(taur*tr2);
727       ch[t8-1]=cc[t7-1]+tr2;
728       ti2=cc[t5]-cc[t6];
729       ci2=cc[t7]+(taur*ti2);
730       ch[t8]=cc[t7]+ti2;
731       cr3=taui*(cc[t5-1]-cc[t6-1]);
732       ci3=taui*(cc[t5]+cc[t6]);
733       dr2=cr2-ci3;
734       dr3=cr2+ci3;
735       di2=ci2+cr3;
736       di3=ci2-cr3;
737       ch[t9-1]=wa1[i-2]*dr2-wa1[i-1]*di2;
738       ch[t9]=wa1[i-2]*di2+wa1[i-1]*dr2;
739       ch[t10-1]=wa2[i-2]*dr3-wa2[i-1]*di3;
740       ch[t10]=wa2[i-2]*di3+wa2[i-1]*dr3;
741     }
742     t1+=ido;
743   }
744 }
745
746 static void dradb4(int ido,int l1,float *cc,float *ch,float *wa1,
747                           float *wa2,float *wa3){
748   static float sqrt2=1.414213562373095f;
749   int i,k,t0,t1,t2,t3,t4,t5,t6,t7,t8;
750   float ci2,ci3,ci4,cr2,cr3,cr4,ti1,ti2,ti3,ti4,tr1,tr2,tr3,tr4;
751   t0=l1*ido;
752   
753   t1=0;
754   t2=ido<<2;
755   t3=0;
756   t6=ido<<1;
757   for(k=0;k<l1;k++){
758     t4=t3+t6;
759     t5=t1;
760     tr3=cc[t4-1]+cc[t4-1];
761     tr4=cc[t4]+cc[t4]; 
762     tr1=cc[t3]-cc[(t4+=t6)-1];
763     tr2=cc[t3]+cc[t4-1];
764     ch[t5]=tr2+tr3;
765     ch[t5+=t0]=tr1-tr4;
766     ch[t5+=t0]=tr2-tr3;
767     ch[t5+=t0]=tr1+tr4;
768     t1+=ido;
769     t3+=t2;
770   }
771
772   if(ido<2)return;
773   if(ido==2)goto L105;
774
775   t1=0;
776   for(k=0;k<l1;k++){
777     t5=(t4=(t3=(t2=t1<<2)+t6))+t6;
778     t7=t1;
779     for(i=2;i<ido;i+=2){
780       t2+=2;
781       t3+=2;
782       t4-=2;
783       t5-=2;
784       t7+=2;
785       ti1=cc[t2]+cc[t5];
786       ti2=cc[t2]-cc[t5];
787       ti3=cc[t3]-cc[t4];
788       tr4=cc[t3]+cc[t4];
789       tr1=cc[t2-1]-cc[t5-1];
790       tr2=cc[t2-1]+cc[t5-1];
791       ti4=cc[t3-1]-cc[t4-1];
792       tr3=cc[t3-1]+cc[t4-1];
793       ch[t7-1]=tr2+tr3;
794       cr3=tr2-tr3;
795       ch[t7]=ti2+ti3;
796       ci3=ti2-ti3;
797       cr2=tr1-tr4;
798       cr4=tr1+tr4;
799       ci2=ti1+ti4;
800       ci4=ti1-ti4;
801
802       ch[(t8=t7+t0)-1]=wa1[i-2]*cr2-wa1[i-1]*ci2;
803       ch[t8]=wa1[i-2]*ci2+wa1[i-1]*cr2;
804       ch[(t8+=t0)-1]=wa2[i-2]*cr3-wa2[i-1]*ci3;
805       ch[t8]=wa2[i-2]*ci3+wa2[i-1]*cr3;
806       ch[(t8+=t0)-1]=wa3[i-2]*cr4-wa3[i-1]*ci4;
807       ch[t8]=wa3[i-2]*ci4+wa3[i-1]*cr4;
808     }
809     t1+=ido;
810   }
811
812   if(ido%2 == 1)return;
813
814  L105:
815
816   t1=ido;
817   t2=ido<<2;
818   t3=ido-1;
819   t4=ido+(ido<<1);
820   for(k=0;k<l1;k++){
821     t5=t3;
822     ti1=cc[t1]+cc[t4];
823     ti2=cc[t4]-cc[t1];
824     tr1=cc[t1-1]-cc[t4-1];
825     tr2=cc[t1-1]+cc[t4-1];
826     ch[t5]=tr2+tr2;
827     ch[t5+=t0]=sqrt2*(tr1-ti1);
828     ch[t5+=t0]=ti2+ti2;
829     ch[t5+=t0]=-sqrt2*(tr1+ti1);
830
831     t3+=ido;
832     t1+=t2;
833     t4+=t2;
834   }
835 }
836
837 static void dradbg(int ido,int ip,int l1,int idl1,float *cc,float *c1,
838             float *c2,float *ch,float *ch2,float *wa){
839   static float tpi=6.283185307179586f;
840   int idij,ipph,i,j,k,l,ik,is,t0,t1,t2,t3,t4,t5,t6,t7,t8,t9,t10,
841       t11,t12;
842   float dc2,ai1,ai2,ar1,ar2,ds2;
843   int nbd;
844   float dcp,arg,dsp,ar1h,ar2h;
845   int ipp2;
846
847   t10=ip*ido;
848   t0=l1*ido;
849   arg=tpi/(float)ip;
850   dcp=cos(arg);
851   dsp=sin(arg);
852   nbd=(ido-1)>>1;
853   ipp2=ip;
854   ipph=(ip+1)>>1;
855   if(ido<l1)goto L103;
856   
857   t1=0;
858   t2=0;
859   for(k=0;k<l1;k++){
860     t3=t1;
861     t4=t2;
862     for(i=0;i<ido;i++){
863       ch[t3]=cc[t4];
864       t3++;
865       t4++;
866     }
867     t1+=ido;
868     t2+=t10;
869   }
870   goto L106;
871
872  L103:
873   t1=0;
874   for(i=0;i<ido;i++){
875     t2=t1;
876     t3=t1;
877     for(k=0;k<l1;k++){
878       ch[t2]=cc[t3];
879       t2+=ido;
880       t3+=t10;
881     }
882     t1++;
883   }
884
885  L106:
886   t1=0;
887   t2=ipp2*t0;
888   t7=(t5=ido<<1);
889   for(j=1;j<ipph;j++){
890     t1+=t0;
891     t2-=t0;
892     t3=t1;
893     t4=t2;
894     t6=t5;
895     for(k=0;k<l1;k++){
896       ch[t3]=cc[t6-1]+cc[t6-1];
897       ch[t4]=cc[t6]+cc[t6];
898       t3+=ido;
899       t4+=ido;
900       t6+=t10;
901     }
902     t5+=t7;
903   }
904
905   if (ido == 1)goto L116;
906   if(nbd<l1)goto L112;
907
908   t1=0;
909   t2=ipp2*t0;
910   t7=0;
911   for(j=1;j<ipph;j++){
912     t1+=t0;
913     t2-=t0;
914     t3=t1;
915     t4=t2;
916
917     t7+=(ido<<1);
918     t8=t7;
919     for(k=0;k<l1;k++){
920       t5=t3;
921       t6=t4;
922       t9=t8;
923       t11=t8;
924       for(i=2;i<ido;i+=2){
925         t5+=2;
926         t6+=2;
927         t9+=2;
928         t11-=2;
929         ch[t5-1]=cc[t9-1]+cc[t11-1];
930         ch[t6-1]=cc[t9-1]-cc[t11-1];
931         ch[t5]=cc[t9]-cc[t11];
932         ch[t6]=cc[t9]+cc[t11];
933       }
934       t3+=ido;
935       t4+=ido;
936       t8+=t10;
937     }
938   }
939   goto L116;
940
941  L112:
942   t1=0;
943   t2=ipp2*t0;
944   t7=0;
945   for(j=1;j<ipph;j++){
946     t1+=t0;
947     t2-=t0;
948     t3=t1;
949     t4=t2;
950     t7+=(ido<<1);
951     t8=t7;
952     t9=t7;
953     for(i=2;i<ido;i+=2){
954       t3+=2;
955       t4+=2;
956       t8+=2;
957       t9-=2;
958       t5=t3;
959       t6=t4;
960       t11=t8;
961       t12=t9;
962       for(k=0;k<l1;k++){
963         ch[t5-1]=cc[t11-1]+cc[t12-1];
964         ch[t6-1]=cc[t11-1]-cc[t12-1];
965         ch[t5]=cc[t11]-cc[t12];
966         ch[t6]=cc[t11]+cc[t12];
967         t5+=ido;
968         t6+=ido;
969         t11+=t10;
970         t12+=t10;
971       }
972     }
973   }
974
975 L116:
976   ar1=1.f;
977   ai1=0.f;
978   t1=0;
979   t9=(t2=ipp2*idl1);
980   t3=(ip-1)*idl1;
981   for(l=1;l<ipph;l++){
982     t1+=idl1;
983     t2-=idl1;
984
985     ar1h=dcp*ar1-dsp*ai1;
986     ai1=dcp*ai1+dsp*ar1;
987     ar1=ar1h;
988     t4=t1;
989     t5=t2;
990     t6=0;
991     t7=idl1;
992     t8=t3;
993     for(ik=0;ik<idl1;ik++){
994       c2[t4++]=ch2[t6++]+ar1*ch2[t7++];
995       c2[t5++]=ai1*ch2[t8++];
996     }
997     dc2=ar1;
998     ds2=ai1;
999     ar2=ar1;
1000     ai2=ai1;
1001
1002     t6=idl1;
1003     t7=t9-idl1;
1004     for(j=2;j<ipph;j++){
1005       t6+=idl1;
1006       t7-=idl1;
1007       ar2h=dc2*ar2-ds2*ai2;
1008       ai2=dc2*ai2+ds2*ar2;
1009       ar2=ar2h;
1010       t4=t1;
1011       t5=t2;
1012       t11=t6;
1013       t12=t7;
1014       for(ik=0;ik<idl1;ik++){
1015         c2[t4++]+=ar2*ch2[t11++];
1016         c2[t5++]+=ai2*ch2[t12++];
1017       }
1018     }
1019   }
1020
1021   t1=0;
1022   for(j=1;j<ipph;j++){
1023     t1+=idl1;
1024     t2=t1;
1025     for(ik=0;ik<idl1;ik++)ch2[ik]+=ch2[t2++];
1026   }
1027
1028   t1=0;
1029   t2=ipp2*t0;
1030   for(j=1;j<ipph;j++){
1031     t1+=t0;
1032     t2-=t0;
1033     t3=t1;
1034     t4=t2;
1035     for(k=0;k<l1;k++){
1036       ch[t3]=c1[t3]-c1[t4];
1037       ch[t4]=c1[t3]+c1[t4];
1038       t3+=ido;
1039       t4+=ido;
1040     }
1041   }
1042
1043   if(ido==1)goto L132;
1044   if(nbd<l1)goto L128;
1045
1046   t1=0;
1047   t2=ipp2*t0;
1048   for(j=1;j<ipph;j++){
1049     t1+=t0;
1050     t2-=t0;
1051     t3=t1;
1052     t4=t2;
1053     for(k=0;k<l1;k++){
1054       t5=t3;
1055       t6=t4;
1056       for(i=2;i<ido;i+=2){
1057         t5+=2;
1058         t6+=2;
1059         ch[t5-1]=c1[t5-1]-c1[t6];
1060         ch[t6-1]=c1[t5-1]+c1[t6];
1061         ch[t5]=c1[t5]+c1[t6-1];
1062         ch[t6]=c1[t5]-c1[t6-1];
1063       }
1064       t3+=ido;
1065       t4+=ido;
1066     }
1067   }
1068   goto L132;
1069
1070  L128:
1071   t1=0;
1072   t2=ipp2*t0;
1073   for(j=1;j<ipph;j++){
1074     t1+=t0;
1075     t2-=t0;
1076     t3=t1;
1077     t4=t2;
1078     for(i=2;i<ido;i+=2){
1079       t3+=2;
1080       t4+=2;
1081       t5=t3;
1082       t6=t4;
1083       for(k=0;k<l1;k++){
1084         ch[t5-1]=c1[t5-1]-c1[t6];
1085         ch[t6-1]=c1[t5-1]+c1[t6];
1086         ch[t5]=c1[t5]+c1[t6-1];
1087         ch[t6]=c1[t5]-c1[t6-1];
1088         t5+=ido;
1089         t6+=ido;
1090       }
1091     }
1092   }
1093
1094 L132:
1095   if(ido==1)return;
1096
1097   for(ik=0;ik<idl1;ik++)c2[ik]=ch2[ik];
1098
1099   t1=0;
1100   for(j=1;j<ip;j++){
1101     t2=(t1+=t0);
1102     for(k=0;k<l1;k++){
1103       c1[t2]=ch[t2];
1104       t2+=ido;
1105     }
1106   }
1107
1108   if(nbd>l1)goto L139;
1109
1110   is= -ido-1;
1111   t1=0;
1112   for(j=1;j<ip;j++){
1113     is+=ido;
1114     t1+=t0;
1115     idij=is;
1116     t2=t1;
1117     for(i=2;i<ido;i+=2){
1118       t2+=2;
1119       idij+=2;
1120       t3=t2;
1121       for(k=0;k<l1;k++){
1122         c1[t3-1]=wa[idij-1]*ch[t3-1]-wa[idij]*ch[t3];
1123         c1[t3]=wa[idij-1]*ch[t3]+wa[idij]*ch[t3-1];
1124         t3+=ido;
1125       }
1126     }
1127   }
1128   return;
1129
1130  L139:
1131   is= -ido-1;
1132   t1=0;
1133   for(j=1;j<ip;j++){
1134     is+=ido;
1135     t1+=t0;
1136     t2=t1;
1137     for(k=0;k<l1;k++){
1138       idij=is;
1139       t3=t2;
1140       for(i=2;i<ido;i+=2){
1141         idij+=2;
1142         t3+=2;
1143         c1[t3-1]=wa[idij-1]*ch[t3-1]-wa[idij]*ch[t3];
1144         c1[t3]=wa[idij-1]*ch[t3]+wa[idij]*ch[t3-1];
1145       }
1146       t2+=ido;
1147     }
1148   }
1149 }
1150
1151 static void drftb1(int n, float *c, float *ch, float *wa, int *ifac){
1152   int i,k1,l1,l2;
1153   int na;
1154   int nf,ip,iw,ix2,ix3,ido,idl1;
1155
1156   nf=ifac[1];
1157   na=0;
1158   l1=1;
1159   iw=1;
1160
1161   for(k1=0;k1<nf;k1++){
1162     ip=ifac[k1 + 2];
1163     l2=ip*l1;
1164     ido=n/l2;
1165     idl1=ido*l1;
1166     if(ip!=4)goto L103;
1167     ix2=iw+ido;
1168     ix3=ix2+ido;
1169
1170     if(na!=0)
1171       dradb4(ido,l1,ch,c,wa+iw-1,wa+ix2-1,wa+ix3-1);
1172     else
1173       dradb4(ido,l1,c,ch,wa+iw-1,wa+ix2-1,wa+ix3-1);
1174     na=1-na;
1175     goto L115;
1176
1177   L103:
1178     if(ip!=2)goto L106;
1179
1180     if(na!=0)
1181       dradb2(ido,l1,ch,c,wa+iw-1);
1182     else
1183       dradb2(ido,l1,c,ch,wa+iw-1);
1184     na=1-na;
1185     goto L115;
1186
1187   L106:
1188     if(ip!=3)goto L109;
1189
1190     ix2=iw+ido;
1191     if(na!=0)
1192       dradb3(ido,l1,ch,c,wa+iw-1,wa+ix2-1);
1193     else
1194       dradb3(ido,l1,c,ch,wa+iw-1,wa+ix2-1);
1195     na=1-na;
1196     goto L115;
1197
1198   L109:
1199 /*    The radix five case can be translated later..... */
1200 /*    if(ip!=5)goto L112;
1201
1202     ix2=iw+ido;
1203     ix3=ix2+ido;
1204     ix4=ix3+ido;
1205     if(na!=0)
1206       dradb5(ido,l1,ch,c,wa+iw-1,wa+ix2-1,wa+ix3-1,wa+ix4-1);
1207     else
1208       dradb5(ido,l1,c,ch,wa+iw-1,wa+ix2-1,wa+ix3-1,wa+ix4-1);
1209     na=1-na;
1210     goto L115;
1211
1212   L112:*/
1213     if(na!=0)
1214       dradbg(ido,ip,l1,idl1,ch,ch,ch,c,c,wa+iw-1);
1215     else
1216       dradbg(ido,ip,l1,idl1,c,c,c,ch,ch,wa+iw-1);
1217     if(ido==1)na=1-na;
1218
1219   L115:
1220     l1=l2;
1221     iw+=(ip-1)*ido;
1222   }
1223
1224   if(na==0)return;
1225
1226   for(i=0;i<n;i++)c[i]=ch[i];
1227 }
1228
1229 void drft_forward(struct drft_lookup *l,float *data){
1230   if(l->n==1)return;
1231   drftf1(l->n,data,l->trigcache,l->trigcache+l->n,l->splitcache);
1232 }
1233
1234 void drft_backward(struct drft_lookup *l,float *data){
1235   if (l->n==1)return;
1236   drftb1(l->n,data,l->trigcache,l->trigcache+l->n,l->splitcache);
1237 }
1238
1239 void drft_init(struct drft_lookup *l,int n)
1240 {
1241   l->n=n;
1242   l->trigcache=(float*)speex_alloc(3*n*sizeof(*l->trigcache));
1243   l->splitcache=(int*)speex_alloc(32*sizeof(*l->splitcache));
1244   fdrffti(n, l->trigcache, l->splitcache);
1245 }
1246
1247 void drft_clear(struct drft_lookup *l)
1248 {
1249   if(l)
1250   {
1251     if(l->trigcache)
1252       speex_free(l->trigcache);
1253     if(l->splitcache)
1254       speex_free(l->splitcache);
1255   }
1256 }