//ifndef _BTNRH_FFT_H
//define _BTNRH_FFT_H
#include "BTNRH_rfft.h"
#include <math.h>
//#include "chapro.h"
//#include "cha_ff.h"
/***********************************************************/
// FFT functions adapted from G. D. Bergland, "Subroutines FAST and FSST," (1979).
// In IEEE Acoustics, Speech, and Signal Processing Society.
// "Programs for Digital Signal Processing," IEEE Press, New York,
namespace BTNRH_FFT {
static __inline int
ilog2(int n)
{
int m;
for (m = 1; m < 32; m++)
if (n == (1 << m))
return (m);
return (-1);
}
static __inline int
bitrev(int ii, int m)
{
// Was: register int jj; RSL 9 Oct 2024
int jj;
jj = ii & 1;
--m;
while (--m > 0) {
ii >>= 1;
jj <<= 1;
jj |= ii & 1;
}
return (jj);
}
static __inline void
rad2(int ii, float *x0, float *x1)
{
int k;
float t;
for (k = 0; k < ii; k++) {
t = x0[k] + x1[k];
x1[k] = x0[k] - x1[k];
x0[k] = t;
}
}
static __inline void
reorder1(int m, float *x)
{
int j, k, kl, n;
float t;
k = 4;
kl = 2;
n = 1 << m;
for (j = 4; j <= n; j += 2) {
if (k > j) {
t = x[j - 1];
x[j - 1] = x[k - 1];
x[k - 1] = t;
}
k -= 2;
if (k <= kl) {
k = 2 * j;
kl = j;
}
}
}
static __inline void
reorder2(int m, float *x)
{
int ji, ij, n;
float t;
n = 1 << m;
for (ij = 0; ij <= (n - 2); ij += 2) {
ji = bitrev(ij >> 1, m) << 1;
if (ij < ji) {
t = x[ij];
x[ij] = x[ji];
x[ji] = t;
t = x[ij + 1];
x[ij + 1] = x[ji + 1];
x[ji + 1] = t;
}
}
}
/***********************************************************/
// rcfft
static void
rcrad4(int ii, int nn,
float *x0, float *x1, float *x2, float *x3,
float *x4, float *x5, float *x6, float *x7)
{
double arg, tpiovn;
float c1, c2, c3, s1, s2, s3, pr, pi, r1, r5;
float t0, t1, t2, t3, t4, t5, t6, t7;
int i0, i4, j, j0, ji, jl, jr, jlast, k, k0, kl, m, n, ni;
n = nn / 4;
for (m = 1; (1 << m) < n; m++)
continue;
tpiovn = 2 * M_PI / nn;
ji = 3;
jl = 2;
jr = 2;
ni = (n + 1) / 2;
for (i0 = 0; i0 < ni; i0++) {
if (i0 == 0) {
for (k = 0; k < ii; k++) {
t0 = x0[k] + x2[k];
t1 = x1[k] + x3[k];
x2[k] = x0[k] - x2[k];
x3[k] = x1[k] - x3[k];
x0[k] = t0 + t1;
x1[k] = t0 - t1;
}
if (nn > 4) {
k0 = ii * 4;
kl = k0 + ii;
for (k = k0; k < kl; k++) {
pr = (float) (M_SQRT1_2 * (x1[k] - x3[k]));
pi = (float) (M_SQRT1_2 * (x1[k] + x3[k]));
x3[k] = x2[k] + pi;
x1[k] = pi - x2[k];
x2[k] = x0[k] - pr;
x0[k] += pr;
}
}
} else {
arg = tpiovn * bitrev(i0, m);
c1 = cosf(arg);
s1 = sinf(arg);
c2 = c1 * c1 - s1 * s1;
s2 = c1 * s1 + c1 * s1;
c3 = c1 * c2 - s1 * s2;
s3 = c2 * s1 + s2 * c1;
i4 = ii * 4;
j0 = jr * i4;
k0 = ji * i4;
jlast = j0 + ii;
for (j = j0; j < jlast; j++) {
k = k0 + j - j0;
r1 = x1[j] * c1 - x5[k] * s1;
r5 = x1[j] * s1 + x5[k] * c1;
t2 = x2[j] * c2 - x6[k] * s2;
t6 = x2[j] * s2 + x6[k] * c2;
t3 = x3[j] * c3 - x7[k] * s3;
t7 = x3[j] * s3 + x7[k] * c3;
t0 = x0[j] + t2;
t4 = x4[k] + t6;
t2 = x0[j] - t2;
t6 = x4[k] - t6;
t1 = r1 + t3;
t5 = r5 + t7;
t3 = r1 - t3;
t7 = r5 - t7;
x0[j] = t0 + t1;
x7[k] = t4 + t5;
x6[k] = t0 - t1;
x1[j] = t5 - t4;
x2[j] = t2 - t7;
x5[k] = t6 + t3;
x4[k] = t2 + t7;
x3[j] = t3 - t6;
}
jr += 2;
ji -= 2;
if (ji <= jl) {
ji = 2 * jr - 1;
jl = jr;
}
}
}
}
//-----------------------------------------------------------
static int
rcfft2(float *x, int m)
{
int ii, nn, m2, it, n;
n = 1 << m;;
m2 = m / 2;
// radix 2
if (m <= m2 * 2) {
nn = 1;
} else {
nn = 2;
ii = n / nn;
rad2(ii, x, x + ii);
}
// radix 4
if (m2 != 0) {
for (it = 0; it < m2; it++) {
nn = nn * 4;
ii = n / nn;
rcrad4(ii, nn, x, x + ii, x + 2 * ii, x + 3 * ii,
x, x + ii, x + 2 * ii, x + 3 * ii);
}
}
// re-order
reorder1(m, x);
reorder2(m, x);
for (it = 3; it < n; it += 2)
x[it] = -x[it];
x[n] = x[1];
x[1] = 0.0;
x[n + 1] = 0.0;
return (0);
}
/***********************************************************/
// rcfft
static void
crrad4(int jj, int nn,
float *x0, float *x1, float *x2, float *x3,
float *x4, float *x5, float *x6, float *x7)
{
double arg, tpiovn;
float c1, c2, c3, s1, s2, s3;
float t0, t1, t2, t3, t4, t5, t6, t7;
int ii, j, j0, ji, jr, jl, jlast, j4, k, k0, kl, m, n, ni;
tpiovn = 2 * M_PI / nn;
ji = 3;
jl = 2;
jr = 2;
n = nn / 4;
for (m = 1; (1 << m) < n; m++)
continue;
ni = (n + 1) / 2;
for (ii = 0; ii < ni; ii++) {
if (ii == 0) {
for (k = 0; k < jj; k++) {
t0 = x0[k] + x1[k];
t1 = x0[k] - x1[k];
t2 = x2[k] * 2;
t3 = x3[k] * 2;
x0[k] = t0 + t2;
x2[k] = t0 - t2;
x1[k] = t1 + t3;
x3[k] = t1 - t3;
}
if (nn > 4) {
k0 = jj * 4;
kl = k0 + jj;
for (k = k0; k < kl; k++) {
t2 = x0[k] - x2[k];
t3 = x1[k] + x3[k];
x0[k] = (x0[k] + x2[k]) * 2;
x2[k] = (x3[k] - x1[k]) * 2;
x1[k] = (float) ((t2 + t3) * M_SQRT2);
x3[k] = (float) ((t3 - t2) * M_SQRT2);
}
}
} else {
arg = tpiovn * bitrev(ii, m);
c1 = cosf(arg);
s1 = -sinf(arg);
c2 = c1 * c1 - s1 * s1;
s2 = c1 * s1 + c1 * s1;
c3 = c1 * c2 - s1 * s2;
s3 = c2 * s1 + s2 * c1;
j4 = jj * 4;
j0 = jr * j4;
k0 = ji * j4;
jlast = j0 + jj;
for (j = j0; j < jlast; j++) {
k = k0 + j - j0;
t0 = x0[j] + x6[k];
t1 = x7[k] - x1[j];
t2 = x0[j] - x6[k];
t3 = x7[k] + x1[j];
t4 = x2[j] + x4[k];
t5 = x5[k] - x3[j];
t6 = x5[k] + x3[j];
t7 = x4[k] - x2[j];
x0[j] = t0 + t4;
x4[k] = t1 + t5;
x1[j] = (t2 + t6) * c1 - (t3 + t7) * s1;
x5[k] = (t2 + t6) * s1 + (t3 + t7) * c1;
x2[j] = (t0 - t4) * c2 - (t1 - t5) * s2;
x6[k] = (t0 - t4) * s2 + (t1 - t5) * c2;
x3[j] = (t2 - t6) * c3 - (t3 - t7) * s3;
x7[k] = (t2 - t6) * s3 + (t3 - t7) * c3;
}
jr += 2;
ji -= 2;
if (ji <= jl) {
ji = 2 * jr - 1;
jl = jr;
}
}
}
}
//-----------------------------------------------------------
static int
crfft2(float *x, int m)
{
int n, i, it, nn, jj, m2;
n = 1 << m;
x[1] = x[n];
m2 = m / 2;
// re-order
for (i = 3; i < n; i += 2)
x[i] = -x[i];
reorder2(m, x);
reorder1(m, x);
// radix 4
if (m2 != 0) {
nn = 4 * n;
for (it = 0; it < m2; it++) {
nn = nn / 4;
jj = n / nn;
crrad4(jj, nn, x, x + jj, x + 2 * jj, x + 3 * jj,
x, x + jj, x + 2 * jj, x + 3 * jj);
}
}
// radix 2
if (m > m2 * 2) {
jj = n / 2;
rad2(jj, x, x + jj);
}
return (0);
}
/***********************************************************/
// real-to-complex FFT
//FUNC(void)
void cha_fft_rc(float *x, int n)
{
int m;
// assume n is a power of two
m = ilog2(n);
rcfft2(x, m);
}
// complex-to-real inverse FFT
//FUNC(void)
void cha_fft_cr(float *x, int n)
{
int i, m;
// assume n is a power of two
m = ilog2(n);
crfft2(x, m);
// scale inverse by 1/n
for (i = 0; i < n; i++) {
x[i] /= n;
}
}
};
//endif