Add an option to disable editor update during drags of the
[ardour.git] / libs / rubberband / src / FFT.cpp
1 /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
2
3 /*
4     Rubber Band
5     An audio time-stretching and pitch-shifting library.
6     Copyright 2007-2008 Chris Cannam.
7     
8     This program is free software; you can redistribute it and/or
9     modify it under the terms of the GNU General Public License as
10     published by the Free Software Foundation; either version 2 of the
11     License, or (at your option) any later version.  See the file
12     COPYING included with this distribution for more information.
13 */
14
15 #include "FFT.h"
16 #include "Thread.h"
17 #include "Profiler.h"
18
19 //#define FFT_MEASUREMENT 1
20
21 #ifndef HAVE_FFTW3
22 #define HAVE_FFTW3 // for Ardour
23 #endif
24
25 #ifdef HAVE_FFTW3
26 #include <fftw3.h>
27 #endif
28
29 #include <cstdlib>
30
31 #ifdef USE_KISSFFT
32 #include "bsd-3rdparty/kissfft/kiss_fftr.h"
33 #endif
34
35 #ifndef HAVE_FFTW3
36 #ifndef USE_KISSFFT
37 #ifndef USE_BUILTIN_FFT
38 #error No FFT implementation selected!
39 #endif
40 #endif
41 #endif
42
43 #include <cmath>
44 #include <iostream>
45 #include <map>
46 #include <cstdio>
47 #include <cstdlib>
48 #include <vector>
49
50 namespace RubberBand {
51
52 class FFTImpl
53 {
54 public:
55     virtual ~FFTImpl() { }
56
57     virtual void initFloat() = 0;
58     virtual void initDouble() = 0;
59
60     virtual void forward(const double *R__ realIn, double *R__ realOut, double *R__ imagOut) = 0;
61     virtual void forwardPolar(const double *R__ realIn, double *R__ magOut, double *R__ phaseOut) = 0;
62     virtual void forwardMagnitude(const double *R__ realIn, double *R__ magOut) = 0;
63
64     virtual void forward(const float *R__ realIn, float *R__ realOut, float *R__ imagOut) = 0;
65     virtual void forwardPolar(const float *R__ realIn, float *R__ magOut, float *R__ phaseOut) = 0;
66     virtual void forwardMagnitude(const float *R__ realIn, float *R__ magOut) = 0;
67
68     virtual void inverse(const double *R__ realIn, const double *R__ imagIn, double *R__ realOut) = 0;
69     virtual void inversePolar(const double *R__ magIn, const double *R__ phaseIn, double *R__ realOut) = 0;
70     virtual void inverseCepstral(const double *R__ magIn, double *R__ cepOut) = 0;
71
72     virtual void inverse(const float *R__ realIn, const float *R__ imagIn, float *R__ realOut) = 0;
73     virtual void inversePolar(const float *R__ magIn, const float *R__ phaseIn, float *R__ realOut) = 0;
74     virtual void inverseCepstral(const float *R__ magIn, float *R__ cepOut) = 0;
75
76     virtual float *getFloatTimeBuffer() = 0;
77     virtual double *getDoubleTimeBuffer() = 0;
78 };    
79
80 namespace FFTs {
81
82
83 #ifdef HAVE_FFTW3
84
85 // Define FFTW_DOUBLE_ONLY to make all uses of FFTW functions be
86 // double-precision (so "float" FFTs are calculated by casting to
87 // doubles and using the double-precision FFTW function).
88 //
89 // Define FFTW_FLOAT_ONLY to make all uses of FFTW functions be
90 // single-precision (so "double" FFTs are calculated by casting to
91 // floats and using the single-precision FFTW function).
92 //
93 // Neither of these flags is terribly desirable -- FFTW_FLOAT_ONLY
94 // obviously loses you precision, and neither is handled in the most
95 // efficient way so any performance improvement will be small at best.
96 // The only real reason to define either flag would be to avoid
97 // linking against both fftw3 and fftw3f libraries.
98
99 //#define FFTW_DOUBLE_ONLY 1
100 //#define FFTW_FLOAT_ONLY 1
101
102 #if defined(FFTW_DOUBLE_ONLY) && defined(FFTW_FLOAT_ONLY)
103 // Can't meaningfully define both
104 #undef FFTW_DOUBLE_ONLY
105 #undef FFTW_FLOAT_ONLY
106 #endif
107
108 #ifdef FFTW_DOUBLE_ONLY
109 #define fft_float_type double
110 #define fftwf_complex fftw_complex
111 #define fftwf_plan fftw_plan
112 #define fftwf_plan_dft_r2c_1d fftw_plan_dft_r2c_1d
113 #define fftwf_plan_dft_c2r_1d fftw_plan_dft_c2r_1d
114 #define fftwf_destroy_plan fftw_destroy_plan
115 #define fftwf_malloc fftw_malloc
116 #define fftwf_free fftw_free
117 #define fftwf_execute fftw_execute
118 #define atan2f atan2
119 #define sqrtf sqrt
120 #define cosf cos
121 #define sinf sin
122 #else
123 #define fft_float_type float
124 #endif /* FFTW_DOUBLE_ONLY */
125
126 #ifdef FFTW_FLOAT_ONLY
127 #define fft_double_type float
128 #define fftw_complex fftwf_complex
129 #define fftw_plan fftwf_plan
130 #define fftw_plan_dft_r2c_1d fftwf_plan_dft_r2c_1d
131 #define fftw_plan_dft_c2r_1d fftwf_plan_dft_c2r_1d
132 #define fftw_destroy_plan fftwf_destroy_plan
133 #define fftw_malloc fftwf_malloc
134 #define fftw_free fftwf_free
135 #define fftw_execute fftwf_execute
136 #define atan2 atan2f
137 #define sqrt sqrtf
138 #define cos cosf
139 #define sin sinf
140 #else
141 #define fft_double_type double
142 #endif /* FFTW_FLOAT_ONLY */
143
144 class D_FFTW : public FFTImpl
145 {
146 public:
147     D_FFTW(int size) : m_fplanf(0)
148 #ifdef FFTW_DOUBLE_ONLY
149                               , m_frb(0)
150 #endif
151                               , m_dplanf(0)
152 #ifdef FFTW_FLOAT_ONLY
153                               , m_drb(0)
154 #endif
155                               , m_size(size)
156     {
157     }
158
159     ~D_FFTW() {
160         if (m_fplanf) {
161             bool save = false;
162             m_extantMutex.lock();
163             if (m_extantf > 0 && --m_extantf == 0) save = true;
164             m_extantMutex.unlock();
165 #ifndef FFTW_DOUBLE_ONLY
166             if (save) saveWisdom('f');
167 #endif
168             fftwf_destroy_plan(m_fplanf);
169             fftwf_destroy_plan(m_fplani);
170             fftwf_free(m_fbuf);
171             fftwf_free(m_fpacked);
172 #ifdef FFTW_DOUBLE_ONLY
173             if (m_frb) fftw_free(m_frb);
174 #endif
175         }
176         if (m_dplanf) {
177             bool save = false;
178             m_extantMutex.lock();
179             if (m_extantd > 0 && --m_extantd == 0) save = true;
180             m_extantMutex.unlock();
181 #ifndef FFTW_FLOAT_ONLY
182             if (save) saveWisdom('d');
183 #endif
184             fftw_destroy_plan(m_dplanf);
185             fftw_destroy_plan(m_dplani);
186             fftw_free(m_dbuf);
187             fftw_free(m_dpacked);
188 #ifdef FFTW_FLOAT_ONLY
189             if (m_drb) fftwf_free(m_drb);
190 #endif
191         }
192     }
193
194     void initFloat() {
195         if (m_fplanf) return;
196         bool load = false;
197         m_extantMutex.lock();
198         if (m_extantf++ == 0) load = true;
199         m_extantMutex.unlock();
200 #ifdef FFTW_DOUBLE_ONLY
201         if (load) loadWisdom('d');
202 #else
203         if (load) loadWisdom('f');
204 #endif
205         m_fbuf = (fft_float_type *)fftw_malloc(m_size * sizeof(fft_float_type));
206         m_fpacked = (fftwf_complex *)fftw_malloc
207             ((m_size/2 + 1) * sizeof(fftwf_complex));
208         m_fplanf = fftwf_plan_dft_r2c_1d
209             (m_size, m_fbuf, m_fpacked, FFTW_MEASURE);
210         m_fplani = fftwf_plan_dft_c2r_1d
211             (m_size, m_fpacked, m_fbuf, FFTW_MEASURE);
212     }
213
214     void initDouble() {
215         if (m_dplanf) return;
216         bool load = false;
217         m_extantMutex.lock();
218         if (m_extantd++ == 0) load = true;
219         m_extantMutex.unlock();
220 #ifdef FFTW_FLOAT_ONLY
221         if (load) loadWisdom('f');
222 #else
223         if (load) loadWisdom('d');
224 #endif
225         m_dbuf = (fft_double_type *)fftw_malloc(m_size * sizeof(fft_double_type));
226         m_dpacked = (fftw_complex *)fftw_malloc
227             ((m_size/2 + 1) * sizeof(fftw_complex));
228         m_dplanf = fftw_plan_dft_r2c_1d
229             (m_size, m_dbuf, m_dpacked, FFTW_MEASURE);
230         m_dplani = fftw_plan_dft_c2r_1d
231             (m_size, m_dpacked, m_dbuf, FFTW_MEASURE);
232     }
233
234     void loadWisdom(char type) { wisdom(false, type); }
235     void saveWisdom(char type) { wisdom(true, type); }
236
237     void wisdom(bool save, char type) {
238
239 #ifdef FFTW_DOUBLE_ONLY
240         if (type == 'f') return;
241 #endif
242 #ifdef FFTW_FLOAT_ONLY
243         if (type == 'd') return;
244 #endif
245
246         const char *home = getenv("HOME");
247         if (!home) return;
248
249         char fn[256];
250         snprintf(fn, 256, "%s/%s.%c", home, ".rubberband.wisdom", type);
251
252         FILE *f = fopen(fn, save ? "wb" : "rb");
253         if (!f) return;
254
255         if (save) {
256             switch (type) {
257 #ifdef FFTW_DOUBLE_ONLY
258             case 'f': break;
259 #else
260             case 'f': fftwf_export_wisdom_to_file(f); break;
261 #endif
262 #ifdef FFTW_FLOAT_ONLY
263             case 'd': break;
264 #else
265             case 'd': fftw_export_wisdom_to_file(f); break;
266 #endif
267             default: break;
268             }
269         } else {
270             switch (type) {
271 #ifdef FFTW_DOUBLE_ONLY
272             case 'f': break;
273 #else
274             case 'f': fftwf_import_wisdom_from_file(f); break;
275 #endif
276 #ifdef FFTW_FLOAT_ONLY
277             case 'd': break;
278 #else
279             case 'd': fftw_import_wisdom_from_file(f); break;
280 #endif
281             default: break;
282             }
283         }
284
285         fclose(f);
286     }
287
288     void packFloat(const float *R__ re, const float *R__ im) {
289         const int hs = m_size/2;
290         fftwf_complex *const R__ fpacked = m_fpacked; 
291         for (int i = 0; i <= hs; ++i) {
292             fpacked[i][0] = re[i];
293         }
294         if (im) {
295             for (int i = 0; i <= hs; ++i) {
296                 fpacked[i][1] = im[i];
297             }
298         } else {
299             for (int i = 0; i <= hs; ++i) {
300                 fpacked[i][1] = 0.f;
301             }
302         }                
303     }
304
305     void packDouble(const double *R__ re, const double *R__ im) {
306         const int hs = m_size/2;
307         fftw_complex *const R__ dpacked = m_dpacked; 
308         for (int i = 0; i <= hs; ++i) {
309             dpacked[i][0] = re[i];
310         }
311         if (im) {
312             for (int i = 0; i <= hs; ++i) {
313                 dpacked[i][1] = im[i];
314             }
315         } else {
316             for (int i = 0; i <= hs; ++i) {
317                 dpacked[i][1] = 0.0;
318             }
319         }
320     }
321
322     void unpackFloat(float *R__ re, float *R__ im) {
323         const int hs = m_size/2;
324         for (int i = 0; i <= hs; ++i) {
325             re[i] = m_fpacked[i][0];
326         }
327         if (im) {
328             for (int i = 0; i <= hs; ++i) {
329                 im[i] = m_fpacked[i][1];
330             }
331         }
332     }        
333
334     void unpackDouble(double *R__ re, double *R__ im) {
335         const int hs = m_size/2;
336         for (int i = 0; i <= hs; ++i) {
337             re[i] = m_dpacked[i][0];
338         }
339         if (im) {
340             for (int i = 0; i <= hs; ++i) {
341                 im[i] = m_dpacked[i][1];
342             }
343         }
344     }        
345
346     void forward(const double *R__ realIn, double *R__ realOut, double *R__ imagOut) {
347         if (!m_dplanf) initDouble();
348         const int sz = m_size;
349         fft_double_type *const R__ dbuf = m_dbuf;
350 #ifndef FFTW_FLOAT_ONLY
351         if (realIn != dbuf) 
352 #endif
353             for (int i = 0; i < sz; ++i) {
354                 dbuf[i] = realIn[i];
355             }
356         fftw_execute(m_dplanf);
357         unpackDouble(realOut, imagOut);
358     }
359
360     void forwardPolar(const double *R__ realIn, double *R__ magOut, double *R__ phaseOut) {
361         if (!m_dplanf) initDouble();
362         fft_double_type *const R__ dbuf = m_dbuf;
363         const int sz = m_size;
364 #ifndef FFTW_FLOAT_ONLY
365         if (realIn != dbuf)
366 #endif
367             for (int i = 0; i < sz; ++i) {
368                 dbuf[i] = realIn[i];
369             }
370         fftw_execute(m_dplanf);
371         const int hs = m_size/2;
372         for (int i = 0; i <= hs; ++i) {
373             magOut[i] = sqrt(m_dpacked[i][0] * m_dpacked[i][0] +
374                              m_dpacked[i][1] * m_dpacked[i][1]);
375         }
376         for (int i = 0; i <= hs; ++i) {
377             phaseOut[i] = atan2(m_dpacked[i][1], m_dpacked[i][0]);
378         }
379     }
380
381     void forwardMagnitude(const double *R__ realIn, double *R__ magOut) {
382         if (!m_dplanf) initDouble();
383         fft_double_type *const R__ dbuf = m_dbuf;
384         const int sz = m_size;
385 #ifndef FFTW_FLOAT_ONLY
386         if (realIn != m_dbuf)
387 #endif
388             for (int i = 0; i < sz; ++i) {
389                 dbuf[i] = realIn[i];
390             }
391         fftw_execute(m_dplanf);
392         const int hs = m_size/2;
393         for (int i = 0; i <= hs; ++i) {
394             magOut[i] = sqrt(m_dpacked[i][0] * m_dpacked[i][0] +
395                              m_dpacked[i][1] * m_dpacked[i][1]);
396         }
397     }
398
399     void forward(const float *R__ realIn, float *R__ realOut, float *R__ imagOut) {
400         if (!m_fplanf) initFloat();
401         fft_float_type *const R__ fbuf = m_fbuf;
402         const int sz = m_size;
403 #ifndef FFTW_DOUBLE_ONLY
404         if (realIn != fbuf)
405 #endif
406             for (int i = 0; i < sz; ++i) {
407                 fbuf[i] = realIn[i];
408             }
409         fftwf_execute(m_fplanf);
410         unpackFloat(realOut, imagOut);
411     }
412
413     void forwardPolar(const float *R__ realIn, float *R__ magOut, float *R__ phaseOut) {
414         if (!m_fplanf) initFloat();
415         fft_float_type *const R__ fbuf = m_fbuf;
416         const int sz = m_size;
417 #ifndef FFTW_DOUBLE_ONLY
418         if (realIn != fbuf) 
419 #endif
420             for (int i = 0; i < sz; ++i) {
421                 fbuf[i] = realIn[i];
422             }
423         fftwf_execute(m_fplanf);
424         const int hs = m_size/2;
425         for (int i = 0; i <= hs; ++i) {
426             magOut[i] = sqrtf(m_fpacked[i][0] * m_fpacked[i][0] +
427                               m_fpacked[i][1] * m_fpacked[i][1]);
428         }
429         for (int i = 0; i <= hs; ++i) {
430             phaseOut[i] = atan2f(m_fpacked[i][1], m_fpacked[i][0]) ;
431         }
432     }
433
434     void forwardMagnitude(const float *R__ realIn, float *R__ magOut) {
435         if (!m_fplanf) initFloat();
436         fft_float_type *const R__ fbuf = m_fbuf;
437         const int sz = m_size;
438 #ifndef FFTW_DOUBLE_ONLY
439         if (realIn != fbuf)
440 #endif
441             for (int i = 0; i < sz; ++i) {
442                 fbuf[i] = realIn[i];
443             }
444         fftwf_execute(m_fplanf);
445         const int hs = m_size/2;
446         for (int i = 0; i <= hs; ++i) {
447             magOut[i] = sqrtf(m_fpacked[i][0] * m_fpacked[i][0] +
448                               m_fpacked[i][1] * m_fpacked[i][1]);
449         }
450     }
451
452     void inverse(const double *R__ realIn, const double *R__ imagIn, double *R__ realOut) {
453         if (!m_dplanf) initDouble();
454         packDouble(realIn, imagIn);
455         fftw_execute(m_dplani);
456         const int sz = m_size;
457         fft_double_type *const R__ dbuf = m_dbuf;
458 #ifndef FFTW_FLOAT_ONLY
459         if (realOut != dbuf) 
460 #endif
461             for (int i = 0; i < sz; ++i) {
462                 realOut[i] = dbuf[i];
463             }
464     }
465
466     void inversePolar(const double *R__ magIn, const double *R__ phaseIn, double *R__ realOut) {
467         if (!m_dplanf) initDouble();
468         const int hs = m_size/2;
469         fftw_complex *const R__ dpacked = m_dpacked;
470         for (int i = 0; i <= hs; ++i) {
471             dpacked[i][0] = magIn[i] * cos(phaseIn[i]);
472         }
473         for (int i = 0; i <= hs; ++i) {
474             dpacked[i][1] = magIn[i] * sin(phaseIn[i]);
475         }
476         fftw_execute(m_dplani);
477         const int sz = m_size;
478         fft_double_type *const R__ dbuf = m_dbuf;
479 #ifndef FFTW_FLOAT_ONLY
480         if (realOut != dbuf)
481 #endif
482             for (int i = 0; i < sz; ++i) {
483                 realOut[i] = dbuf[i];
484             }
485     }
486
487     void inverseCepstral(const double *R__ magIn, double *R__ cepOut) {
488         if (!m_dplanf) initDouble();
489         fft_double_type *const R__ dbuf = m_dbuf;
490         fftw_complex *const R__ dpacked = m_dpacked;
491         const int hs = m_size/2;
492         for (int i = 0; i <= hs; ++i) {
493             dpacked[i][0] = log(magIn[i] + 0.000001);
494         }
495         for (int i = 0; i <= hs; ++i) {
496             dpacked[i][1] = 0.0;
497         }
498         fftw_execute(m_dplani);
499         const int sz = m_size;
500 #ifndef FFTW_FLOAT_ONLY
501         if (cepOut != dbuf)
502 #endif
503             for (int i = 0; i < sz; ++i) {
504                 cepOut[i] = dbuf[i];
505             }
506     }
507
508     void inverse(const float *R__ realIn, const float *R__ imagIn, float *R__ realOut) {
509         if (!m_fplanf) initFloat();
510         packFloat(realIn, imagIn);
511         fftwf_execute(m_fplani);
512         const int sz = m_size;
513         fft_float_type *const R__ fbuf = m_fbuf;
514 #ifndef FFTW_DOUBLE_ONLY
515         if (realOut != fbuf)
516 #endif
517             for (int i = 0; i < sz; ++i) {
518                 realOut[i] = fbuf[i];
519             }
520     }
521
522     void inversePolar(const float *R__ magIn, const float *R__ phaseIn, float *R__ realOut) {
523         if (!m_fplanf) initFloat();
524         const int hs = m_size/2;
525         fftwf_complex *const R__ fpacked = m_fpacked;
526         for (int i = 0; i <= hs; ++i) {
527             fpacked[i][0] = magIn[i] * cosf(phaseIn[i]);
528         }
529         for (int i = 0; i <= hs; ++i) {
530             fpacked[i][1] = magIn[i] * sinf(phaseIn[i]);
531         }
532         fftwf_execute(m_fplani);
533         const int sz = m_size;
534         fft_float_type *const R__ fbuf = m_fbuf;
535 #ifndef FFTW_DOUBLE_ONLY
536         if (realOut != fbuf)
537 #endif
538             for (int i = 0; i < sz; ++i) {
539                 realOut[i] = fbuf[i];
540             }
541     }
542
543     void inverseCepstral(const float *R__ magIn, float *R__ cepOut) {
544         if (!m_fplanf) initFloat();
545         const int hs = m_size/2;
546         fftwf_complex *const R__ fpacked = m_fpacked;
547         for (int i = 0; i <= hs; ++i) {
548             fpacked[i][0] = logf(magIn[i] + 0.000001f);
549         }
550         for (int i = 0; i <= hs; ++i) {
551             fpacked[i][1] = 0.f;
552         }
553         fftwf_execute(m_fplani);
554         const int sz = m_size;
555         fft_float_type *const R__ fbuf = m_fbuf;
556 #ifndef FFTW_DOUBLE_ONLY
557         if (cepOut != fbuf)
558 #endif
559             for (int i = 0; i < sz; ++i) {
560                 cepOut[i] = fbuf[i];
561             }
562     }
563
564     float *getFloatTimeBuffer() {
565         initFloat();
566 #ifdef FFTW_DOUBLE_ONLY
567         if (!m_frb) m_frb = (float *)fftw_malloc(m_size * sizeof(float));
568         return m_frb;
569 #else
570         return m_fbuf;
571 #endif
572     }
573
574     double *getDoubleTimeBuffer() {
575         initDouble();
576 #ifdef FFTW_FLOAT_ONLY
577         if (!m_drb) m_drb = (double *)fftwf_malloc(m_size * sizeof(double));
578         return m_drb;
579 #else
580         return m_dbuf;
581 #endif
582     }
583
584 private:
585     fftwf_plan m_fplanf;
586     fftwf_plan m_fplani;
587 #ifdef FFTW_DOUBLE_ONLY
588     float *m_frb;
589     double *m_fbuf;
590 #else
591     float *m_fbuf;
592 #endif
593     fftwf_complex *m_fpacked;
594     fftw_plan m_dplanf;
595     fftw_plan m_dplani;
596 #ifdef FFTW_FLOAT_ONLY
597     float *m_dbuf;
598     double *m_drb;
599 #else
600     double *m_dbuf;
601 #endif
602     fftw_complex * m_dpacked;
603     const int m_size;
604     static int m_extantf;
605     static int m_extantd;
606     static Mutex m_extantMutex;
607 };
608
609 int
610 D_FFTW::m_extantf = 0;
611
612 int
613 D_FFTW::m_extantd = 0;
614
615 Mutex
616 D_FFTW::m_extantMutex;
617
618 #endif /* HAVE_FFTW3 */
619
620 #ifdef USE_KISSFFT
621
622 class D_KISSFFT : public FFTImpl
623 {
624 public:
625     D_KISSFFT(int size) :
626         m_size(size),
627         m_frb(0),
628         m_drb(0),
629         m_fplanf(0),  
630         m_fplani(0)
631     {
632 #ifdef FIXED_POINT
633 #error KISSFFT is not configured for float values
634 #endif
635         if (sizeof(kiss_fft_scalar) != sizeof(float)) {
636             std::cerr << "ERROR: KISSFFT is not configured for float values"
637                       << std::endl;
638         }
639
640         m_fbuf = new kiss_fft_scalar[m_size + 2];
641         m_fpacked = new kiss_fft_cpx[m_size + 2];
642         m_fplanf = kiss_fftr_alloc(m_size, 0, NULL, NULL);
643         m_fplani = kiss_fftr_alloc(m_size, 1, NULL, NULL);
644     }
645
646     ~D_KISSFFT() {
647         kiss_fftr_free(m_fplanf);
648         kiss_fftr_free(m_fplani);
649         kiss_fft_cleanup();
650
651         delete[] m_fbuf;
652         delete[] m_fpacked;
653
654         if (m_frb) delete[] m_frb;
655         if (m_drb) delete[] m_drb;
656     }
657
658     void initFloat() { }
659     void initDouble() { }
660
661     void packFloat(const float *R__ re, const float *R__ im) {
662         const int hs = m_size/2;
663         for (int i = 0; i <= hs; ++i) {
664             m_fpacked[i].r = re[i];
665             m_fpacked[i].i = im[i];
666         }
667     }
668
669     void unpackFloat(float *R__ re, float *R__ im) {
670         const int hs = m_size/2;
671         for (int i = 0; i <= hs; ++i) {
672             re[i] = m_fpacked[i].r;
673             im[i] = m_fpacked[i].i;
674         }
675     }        
676
677     void packDouble(const double *R__ re, const double *R__ im) {
678         const int hs = m_size/2;
679         for (int i = 0; i <= hs; ++i) {
680             m_fpacked[i].r = float(re[i]);
681             m_fpacked[i].i = float(im[i]);
682         }
683     }
684
685     void unpackDouble(double *R__ re, double *R__ im) {
686         const int hs = m_size/2;
687         for (int i = 0; i <= hs; ++i) {
688             re[i] = double(m_fpacked[i].r);
689             im[i] = double(m_fpacked[i].i);
690         }
691     }        
692
693     void forward(const double *R__ realIn, double *R__ realOut, double *R__ imagOut) {
694
695         for (int i = 0; i < m_size; ++i) {
696             m_fbuf[i] = float(realIn[i]);
697         }
698
699         kiss_fftr(m_fplanf, m_fbuf, m_fpacked);
700         unpackDouble(realOut, imagOut);
701     }
702
703     void forwardPolar(const double *R__ realIn, double *R__ magOut, double *R__ phaseOut) {
704
705         for (int i = 0; i < m_size; ++i) {
706             m_fbuf[i] = float(realIn[i]);
707         }
708
709         kiss_fftr(m_fplanf, m_fbuf, m_fpacked);
710
711         const int hs = m_size/2;
712
713         for (int i = 0; i <= hs; ++i) {
714             magOut[i] = sqrt(double(m_fpacked[i].r) * double(m_fpacked[i].r) +
715                              double(m_fpacked[i].i) * double(m_fpacked[i].i));
716         }
717
718         for (int i = 0; i <= hs; ++i) {
719             phaseOut[i] = atan2(double(m_fpacked[i].i), double(m_fpacked[i].r));
720         }
721     }
722
723     void forwardMagnitude(const double *R__ realIn, double *R__ magOut) {
724
725         for (int i = 0; i < m_size; ++i) {
726             m_fbuf[i] = float(realIn[i]);
727         }
728
729         kiss_fftr(m_fplanf, m_fbuf, m_fpacked);
730
731         const int hs = m_size/2;
732
733         for (int i = 0; i <= hs; ++i) {
734             magOut[i] = sqrt(double(m_fpacked[i].r) * double(m_fpacked[i].r) +
735                              double(m_fpacked[i].i) * double(m_fpacked[i].i));
736         }
737     }
738
739     void forward(const float *R__ realIn, float *R__ realOut, float *R__ imagOut) {
740
741         kiss_fftr(m_fplanf, realIn, m_fpacked);
742         unpackFloat(realOut, imagOut);
743     }
744
745     void forwardPolar(const float *R__ realIn, float *R__ magOut, float *R__ phaseOut) {
746
747         kiss_fftr(m_fplanf, realIn, m_fpacked);
748
749         const int hs = m_size/2;
750
751         for (int i = 0; i <= hs; ++i) {
752             magOut[i] = sqrtf(m_fpacked[i].r * m_fpacked[i].r +
753                               m_fpacked[i].i * m_fpacked[i].i);
754         }
755
756         for (int i = 0; i <= hs; ++i) {
757             phaseOut[i] = atan2f(m_fpacked[i].i, m_fpacked[i].r);
758         }
759     }
760
761     void forwardMagnitude(const float *R__ realIn, float *R__ magOut) {
762
763         kiss_fftr(m_fplanf, realIn, m_fpacked);
764
765         const int hs = m_size/2;
766
767         for (int i = 0; i <= hs; ++i) {
768             magOut[i] = sqrtf(m_fpacked[i].r * m_fpacked[i].r +
769                               m_fpacked[i].i * m_fpacked[i].i);
770         }
771     }
772
773     void inverse(const double *R__ realIn, const double *R__ imagIn, double *R__ realOut) {
774
775         packDouble(realIn, imagIn);
776
777         kiss_fftri(m_fplani, m_fpacked, m_fbuf);
778
779         for (int i = 0; i < m_size; ++i) {
780             realOut[i] = m_fbuf[i];
781         }
782     }
783
784     void inversePolar(const double *R__ magIn, const double *R__ phaseIn, double *R__ realOut) {
785
786         const int hs = m_size/2;
787
788         for (int i = 0; i <= hs; ++i) {
789             m_fpacked[i].r = float(magIn[i] * cos(phaseIn[i]));
790             m_fpacked[i].i = float(magIn[i] * sin(phaseIn[i]));
791         }
792
793         kiss_fftri(m_fplani, m_fpacked, m_fbuf);
794
795         for (int i = 0; i < m_size; ++i) {
796             realOut[i] = m_fbuf[i];
797         }
798     }
799
800     void inverseCepstral(const double *R__ magIn, double *R__ cepOut) {
801
802         const int hs = m_size/2;
803
804         for (int i = 0; i <= hs; ++i) {
805             m_fpacked[i].r = float(log(magIn[i] + 0.000001));
806             m_fpacked[i].i = 0.0f;
807         }
808
809         kiss_fftri(m_fplani, m_fpacked, m_fbuf);
810
811         for (int i = 0; i < m_size; ++i) {
812             cepOut[i] = m_fbuf[i];
813         }
814     }
815     
816     void inverse(const float *R__ realIn, const float *R__ imagIn, float *R__ realOut) {
817
818         packFloat(realIn, imagIn);
819         kiss_fftri(m_fplani, m_fpacked, realOut);
820     }
821
822     void inversePolar(const float *R__ magIn, const float *R__ phaseIn, float *R__ realOut) {
823
824         const int hs = m_size/2;
825
826         for (int i = 0; i <= hs; ++i) {
827             m_fpacked[i].r = magIn[i] * cosf(phaseIn[i]);
828             m_fpacked[i].i = magIn[i] * sinf(phaseIn[i]);
829         }
830
831         kiss_fftri(m_fplani, m_fpacked, realOut);
832     }
833
834     void inverseCepstral(const float *R__ magIn, float *R__ cepOut) {
835
836         const int hs = m_size/2;
837
838         for (int i = 0; i <= hs; ++i) {
839             m_fpacked[i].r = logf(magIn[i] + 0.000001f);
840             m_fpacked[i].i = 0.0f;
841         }
842
843         kiss_fftri(m_fplani, m_fpacked, cepOut);
844     }
845     
846     float *getFloatTimeBuffer() {
847         if (!m_frb) m_frb = new float[m_size];
848         return m_frb;
849     }
850
851     double *getDoubleTimeBuffer() {
852         if (!m_drb) m_drb = new double[m_size];
853         return m_drb;
854     }
855
856 private:
857     const int m_size;
858     float* m_frb;
859     double* m_drb;
860     kiss_fftr_cfg m_fplanf;
861     kiss_fftr_cfg m_fplani;
862     kiss_fft_scalar *m_fbuf;
863     kiss_fft_cpx *m_fpacked;
864 };
865
866 #endif /* USE_KISSFFT */
867
868 #ifdef USE_BUILTIN_FFT
869
870 class D_Cross : public FFTImpl
871 {
872 public:
873     D_Cross(int size) : m_size(size), m_table(0), m_frb(0), m_drb(0) {
874         
875         m_a = new double[size];
876         m_b = new double[size];
877         m_c = new double[size];
878         m_d = new double[size];
879
880         m_table = new int[m_size];
881     
882         int bits;
883         int i, j, k, m;
884
885         for (i = 0; ; ++i) {
886             if (m_size & (1 << i)) {
887                 bits = i;
888                 break;
889             }
890         }
891         
892         for (i = 0; i < m_size; ++i) {
893             
894             m = i;
895             
896             for (j = k = 0; j < bits; ++j) {
897                 k = (k << 1) | (m & 1);
898                 m >>= 1;
899             }
900             
901             m_table[i] = k;
902         }
903     }
904
905     ~D_Cross() {
906         delete[] m_table;
907         delete[] m_a;
908         delete[] m_b;
909         delete[] m_c;
910         delete[] m_d;
911         delete[] m_frb;
912         delete[] m_drb;
913     }
914
915     void initFloat() { }
916     void initDouble() { }
917
918     void forward(const double *R__ realIn, double *R__ realOut, double *R__ imagOut) {
919         basefft(false, realIn, 0, m_c, m_d);
920         const int hs = m_size/2;
921         for (int i = 0; i <= hs; ++i) realOut[i] = m_c[i];
922         if (imagOut) {
923             for (int i = 0; i <= hs; ++i) imagOut[i] = m_d[i];
924         }
925     }
926
927     void forwardPolar(const double *R__ realIn, double *R__ magOut, double *R__ phaseOut) {
928         basefft(false, realIn, 0, m_c, m_d);
929         const int hs = m_size/2;
930         for (int i = 0; i <= hs; ++i) {
931             magOut[i] = sqrt(m_c[i] * m_c[i] + m_d[i] * m_d[i]);
932             phaseOut[i] = atan2(m_d[i], m_c[i]) ;
933         }
934     }
935
936     void forwardMagnitude(const double *R__ realIn, double *R__ magOut) {
937         basefft(false, realIn, 0, m_c, m_d);
938         const int hs = m_size/2;
939         for (int i = 0; i <= hs; ++i) {
940             magOut[i] = sqrt(m_c[i] * m_c[i] + m_d[i] * m_d[i]);
941         }
942     }
943
944     void forward(const float *R__ realIn, float *R__ realOut, float *R__ imagOut) {
945         for (int i = 0; i < m_size; ++i) m_a[i] = realIn[i];
946         basefft(false, m_a, 0, m_c, m_d);
947         const int hs = m_size/2;
948         for (int i = 0; i <= hs; ++i) realOut[i] = m_c[i];
949         if (imagOut) {
950             for (int i = 0; i <= hs; ++i) imagOut[i] = m_d[i];
951         }
952     }
953
954     void forwardPolar(const float *R__ realIn, float *R__ magOut, float *R__ phaseOut) {
955         for (int i = 0; i < m_size; ++i) m_a[i] = realIn[i];
956         basefft(false, m_a, 0, m_c, m_d);
957         const int hs = m_size/2;
958         for (int i = 0; i <= hs; ++i) {
959             magOut[i] = sqrt(m_c[i] * m_c[i] + m_d[i] * m_d[i]);
960             phaseOut[i] = atan2(m_d[i], m_c[i]) ;
961         }
962     }
963
964     void forwardMagnitude(const float *R__ realIn, float *R__ magOut) {
965         for (int i = 0; i < m_size; ++i) m_a[i] = realIn[i];
966         basefft(false, m_a, 0, m_c, m_d);
967         const int hs = m_size/2;
968         for (int i = 0; i <= hs; ++i) {
969             magOut[i] = sqrt(m_c[i] * m_c[i] + m_d[i] * m_d[i]);
970         }
971     }
972
973     void inverse(const double *R__ realIn, const double *R__ imagIn, double *R__ realOut) {
974         const int hs = m_size/2;
975         for (int i = 0; i <= hs; ++i) {
976             double real = realIn[i];
977             double imag = imagIn[i];
978             m_a[i] = real;
979             m_b[i] = imag;
980             if (i > 0) {
981                 m_a[m_size-i] = real;
982                 m_b[m_size-i] = -imag;
983             }
984         }
985         basefft(true, m_a, m_b, realOut, m_d);
986     }
987
988     void inversePolar(const double *R__ magIn, const double *R__ phaseIn, double *R__ realOut) {
989         const int hs = m_size/2;
990         for (int i = 0; i <= hs; ++i) {
991             double real = magIn[i] * cos(phaseIn[i]);
992             double imag = magIn[i] * sin(phaseIn[i]);
993             m_a[i] = real;
994             m_b[i] = imag;
995             if (i > 0) {
996                 m_a[m_size-i] = real;
997                 m_b[m_size-i] = -imag;
998             }
999         }
1000         basefft(true, m_a, m_b, realOut, m_d);
1001     }
1002
1003     void inverseCepstral(const double *R__ magIn, double *R__ cepOut) {
1004         const int hs = m_size/2;
1005         for (int i = 0; i <= hs; ++i) {
1006             double real = log(magIn[i] + 0.000001);
1007             m_a[i] = real;
1008             m_b[i] = 0.0;
1009             if (i > 0) {
1010                 m_a[m_size-i] = real;
1011                 m_b[m_size-i] = 0.0;
1012             }
1013         }
1014         basefft(true, m_a, m_b, cepOut, m_d);
1015     }
1016
1017     void inverse(const float *R__ realIn, const float *R__ imagIn, float *R__ realOut) {
1018         const int hs = m_size/2;
1019         for (int i = 0; i <= hs; ++i) {
1020             float real = realIn[i];
1021             float imag = imagIn[i];
1022             m_a[i] = real;
1023             m_b[i] = imag;
1024             if (i > 0) {
1025                 m_a[m_size-i] = real;
1026                 m_b[m_size-i] = -imag;
1027             }
1028         }
1029         basefft(true, m_a, m_b, m_c, m_d);
1030         for (int i = 0; i < m_size; ++i) realOut[i] = m_c[i];
1031     }
1032
1033     void inversePolar(const float *R__ magIn, const float *R__ phaseIn, float *R__ realOut) {
1034         const int hs = m_size/2;
1035         for (int i = 0; i <= hs; ++i) {
1036             float real = magIn[i] * cosf(phaseIn[i]);
1037             float imag = magIn[i] * sinf(phaseIn[i]);
1038             m_a[i] = real;
1039             m_b[i] = imag;
1040             if (i > 0) {
1041                 m_a[m_size-i] = real;
1042                 m_b[m_size-i] = -imag;
1043             }
1044         }
1045         basefft(true, m_a, m_b, m_c, m_d);
1046         for (int i = 0; i < m_size; ++i) realOut[i] = m_c[i];
1047     }
1048
1049     void inverseCepstral(const float *R__ magIn, float *R__ cepOut) {
1050         const int hs = m_size/2;
1051         for (int i = 0; i <= hs; ++i) {
1052             float real = logf(magIn[i] + 0.000001);
1053             m_a[i] = real;
1054             m_b[i] = 0.0;
1055             if (i > 0) {
1056                 m_a[m_size-i] = real;
1057                 m_b[m_size-i] = 0.0;
1058             }
1059         }
1060         basefft(true, m_a, m_b, m_c, m_d);
1061         for (int i = 0; i < m_size; ++i) cepOut[i] = m_c[i];
1062     }
1063
1064     float *getFloatTimeBuffer() {
1065         if (!m_frb) m_frb = new float[m_size];
1066         return m_frb;
1067     }
1068
1069     double *getDoubleTimeBuffer() {
1070         if (!m_drb) m_drb = new double[m_size];
1071         return m_drb;
1072     }
1073
1074 private:
1075     const int m_size;
1076     int *m_table;
1077     float *m_frb;
1078     double *m_drb;
1079     double *m_a;
1080     double *m_b;
1081     double *m_c;
1082     double *m_d;
1083     void basefft(bool inverse, const double *R__ ri, const double *R__ ii, double *R__ ro, double *R__ io);
1084 };
1085
1086 void
1087 D_Cross::basefft(bool inverse, const double *R__ ri, const double *R__ ii, double *R__ ro, double *R__ io)
1088 {
1089     if (!ri || !ro || !io) return;
1090
1091     int i, j, k, m;
1092     int blockSize, blockEnd;
1093
1094     double tr, ti;
1095
1096     double angle = 2.0 * M_PI;
1097     if (inverse) angle = -angle;
1098
1099     const int n = m_size;
1100
1101     if (ii) {
1102         for (i = 0; i < n; ++i) {
1103             ro[m_table[i]] = ri[i];
1104         }
1105         for (i = 0; i < n; ++i) {
1106             io[m_table[i]] = ii[i];
1107         }
1108     } else {
1109         for (i = 0; i < n; ++i) {
1110             ro[m_table[i]] = ri[i];
1111         }
1112         for (i = 0; i < n; ++i) {
1113             io[m_table[i]] = 0.0;
1114         }
1115     }
1116
1117     blockEnd = 1;
1118
1119     for (blockSize = 2; blockSize <= n; blockSize <<= 1) {
1120
1121         double delta = angle / (double)blockSize;
1122         double sm2 = -sin(-2 * delta);
1123         double sm1 = -sin(-delta);
1124         double cm2 = cos(-2 * delta);
1125         double cm1 = cos(-delta);
1126         double w = 2 * cm1;
1127         double ar[3], ai[3];
1128
1129         for (i = 0; i < n; i += blockSize) {
1130
1131             ar[2] = cm2;
1132             ar[1] = cm1;
1133
1134             ai[2] = sm2;
1135             ai[1] = sm1;
1136
1137             for (j = i, m = 0; m < blockEnd; j++, m++) {
1138
1139                 ar[0] = w * ar[1] - ar[2];
1140                 ar[2] = ar[1];
1141                 ar[1] = ar[0];
1142
1143                 ai[0] = w * ai[1] - ai[2];
1144                 ai[2] = ai[1];
1145                 ai[1] = ai[0];
1146
1147                 k = j + blockEnd;
1148                 tr = ar[0] * ro[k] - ai[0] * io[k];
1149                 ti = ar[0] * io[k] + ai[0] * ro[k];
1150
1151                 ro[k] = ro[j] - tr;
1152                 io[k] = io[j] - ti;
1153
1154                 ro[j] += tr;
1155                 io[j] += ti;
1156             }
1157         }
1158
1159         blockEnd = blockSize;
1160     }
1161
1162 /* fftw doesn't rescale, so nor will we
1163
1164     if (inverse) {
1165
1166         double denom = (double)n;
1167
1168         for (i = 0; i < n; i++) {
1169             ro[i] /= denom;
1170             io[i] /= denom;
1171         }
1172     }
1173 */
1174 }
1175
1176 #endif /* USE_BUILTIN_FFT */
1177
1178 } /* end namespace FFTs */
1179
1180 int
1181 FFT::m_method = -1;
1182
1183 FFT::FFT(int size, int debugLevel)
1184 {
1185     if ((size < 2) ||
1186         (size & (size-1))) {
1187         std::cerr << "FFT::FFT(" << size << "): power-of-two sizes only supported, minimum size 2" << std::endl;
1188         throw InvalidSize;
1189     }
1190
1191     if (m_method == -1) {
1192         m_method = 3;
1193 #ifdef USE_KISSFFT
1194         m_method = 2;
1195 #endif
1196 #ifdef HAVE_FFTW3
1197         m_method = 1;
1198 #endif
1199     }
1200
1201     switch (m_method) {
1202
1203     case 0:
1204         std::cerr << "FFT::FFT(" << size << "): WARNING: Selected implemention not available" << std::endl;
1205 #ifdef USE_BUILTIN_FFT
1206         d = new FFTs::D_Cross(size);
1207 #else
1208         std::cerr << "FFT::FFT(" << size << "): ERROR: Fallback implementation not available!" << std::endl;
1209         abort();
1210 #endif
1211         break;
1212
1213     case 1:
1214 #ifdef HAVE_FFTW3
1215         if (debugLevel > 0) {
1216             std::cerr << "FFT::FFT(" << size << "): using FFTW3 implementation"
1217                       << std::endl;
1218         }
1219         d = new FFTs::D_FFTW(size);
1220 #else
1221         std::cerr << "FFT::FFT(" << size << "): WARNING: Selected implemention not available" << std::endl;
1222 #ifdef USE_BUILTIN_FFT
1223         d = new FFTs::D_Cross(size);
1224 #else
1225         std::cerr << "FFT::FFT(" << size << "): ERROR: Fallback implementation not available!" << std::endl;
1226         abort();
1227 #endif
1228 #endif
1229         break;
1230
1231     case 2:
1232 #ifdef USE_KISSFFT
1233         if (debugLevel > 0) {
1234             std::cerr << "FFT::FFT(" << size << "): using KISSFFT implementation"
1235                       << std::endl;
1236         }
1237         d = new FFTs::D_KISSFFT(size);
1238 #else
1239         std::cerr << "FFT::FFT(" << size << "): WARNING: Selected implemention not available" << std::endl;
1240 #ifdef USE_BUILTIN_FFT
1241         d = new FFTs::D_Cross(size);
1242 #else
1243         std::cerr << "FFT::FFT(" << size << "): ERROR: Fallback implementation not available!" << std::endl;
1244         abort();
1245 #endif
1246 #endif
1247         break;
1248
1249     default:
1250 #ifdef USE_BUILTIN_FFT
1251         std::cerr << "FFT::FFT(" << size << "): WARNING: using slow built-in implementation" << std::endl;
1252         d = new FFTs::D_Cross(size);
1253 #else
1254         std::cerr << "FFT::FFT(" << size << "): ERROR: Fallback implementation not available!" << std::endl;
1255         abort();
1256 #endif
1257         break;
1258     }
1259 }
1260
1261 FFT::~FFT()
1262 {
1263     delete d;
1264 }
1265
1266 void
1267 FFT::forward(const double *R__ realIn, double *R__ realOut, double *R__ imagOut)
1268 {
1269     d->forward(realIn, realOut, imagOut);
1270 }
1271
1272 void
1273 FFT::forwardPolar(const double *R__ realIn, double *R__ magOut, double *R__ phaseOut)
1274 {
1275     d->forwardPolar(realIn, magOut, phaseOut);
1276 }
1277
1278 void
1279 FFT::forwardMagnitude(const double *R__ realIn, double *R__ magOut)
1280 {
1281     d->forwardMagnitude(realIn, magOut);
1282 }
1283
1284 void
1285 FFT::forward(const float *R__ realIn, float *R__ realOut, float *R__ imagOut)
1286 {
1287     d->forward(realIn, realOut, imagOut);
1288 }
1289
1290 void
1291 FFT::forwardPolar(const float *R__ realIn, float *R__ magOut, float *R__ phaseOut)
1292 {
1293     d->forwardPolar(realIn, magOut, phaseOut);
1294 }
1295
1296 void
1297 FFT::forwardMagnitude(const float *R__ realIn, float *R__ magOut)
1298 {
1299     d->forwardMagnitude(realIn, magOut);
1300 }
1301
1302 void
1303 FFT::inverse(const double *R__ realIn, const double *R__ imagIn, double *R__ realOut)
1304 {
1305     d->inverse(realIn, imagIn, realOut);
1306 }
1307
1308 void
1309 FFT::inversePolar(const double *R__ magIn, const double *R__ phaseIn, double *R__ realOut)
1310 {
1311     d->inversePolar(magIn, phaseIn, realOut);
1312 }
1313
1314 void
1315 FFT::inverseCepstral(const double *R__ magIn, double *R__ cepOut)
1316 {
1317     d->inverseCepstral(magIn, cepOut);
1318 }
1319
1320 void
1321 FFT::inverse(const float *R__ realIn, const float *R__ imagIn, float *R__ realOut)
1322 {
1323     d->inverse(realIn, imagIn, realOut);
1324 }
1325
1326 void
1327 FFT::inversePolar(const float *R__ magIn, const float *R__ phaseIn, float *R__ realOut)
1328 {
1329     d->inversePolar(magIn, phaseIn, realOut);
1330 }
1331
1332 void
1333 FFT::inverseCepstral(const float *R__ magIn, float *R__ cepOut)
1334 {
1335     d->inverseCepstral(magIn, cepOut);
1336 }
1337
1338 void
1339 FFT::initFloat() 
1340 {
1341     d->initFloat();
1342 }
1343
1344 void
1345 FFT::initDouble() 
1346 {
1347     d->initDouble();
1348 }
1349
1350 float *
1351 FFT::getFloatTimeBuffer()
1352 {
1353     return d->getFloatTimeBuffer();
1354 }
1355
1356 double *
1357 FFT::getDoubleTimeBuffer()
1358 {
1359     return d->getDoubleTimeBuffer();
1360 }
1361
1362
1363 void
1364 FFT::tune()
1365 {
1366 }
1367
1368
1369 }