use super::complex::{Complex, Scaler};
use core::f64::consts::PI;
#[allow(unused_imports)]
use num_traits::real::Real;
pub struct KissFastFourierTransform<'a> {
inverse: bool,
twiddle: &'a [Complex],
nfft: usize,
factors: [usize; 64],
}
impl<'a> KissFastFourierTransform<'a> {
pub fn new(nfft: usize, inverse: bool, buf: &'a mut [Complex]) -> (Self, &'a mut [Complex]) {
let (twiddle, buf) = buf.split_at_mut(nfft);
for (i, tw) in twiddle.iter_mut().enumerate() {
let mut phase = -2.0 * PI * (i as f64) / (nfft as f64);
if inverse {
phase *= -1.0;
}
tw.r = phase.cos() as Scaler;
tw.i = phase.sin() as Scaler;
}
let mut factors: [usize; 64] = [0; 64];
Self::kf_factor(nfft, &mut factors);
(
Self {
nfft,
inverse,
twiddle,
factors,
},
buf,
)
}
fn kf_factor(mut n: usize, factors: &mut [usize]) {
let mut p = 4;
let floor_sqrt = (n as Scaler).sqrt().floor();
let mut i = 0;
loop {
while (n % p) != 0 {
match p {
4 => p = 2,
2 => p = 3,
_ => p += 2,
};
if p as Scaler > floor_sqrt {
p = n; }
}
n /= p;
factors[i] = p;
i += 1;
factors[i] = n;
i += 1;
if n <= 1 {
break;
}
}
}
pub fn transform(&self, fin: &[Complex], fout: &mut [Complex]) {
self.kiss_fft_stride(fin, fout, 1);
}
fn kiss_fft_stride(&self, fin: &[Complex], fout: &mut [Complex], in_stride: usize) {
self.kf_work(fout, fin, 1, in_stride, 0, 0, 0);
}
fn kf_work(
&self,
fout: &mut [Complex],
fin: &[Complex],
fstride: usize,
in_stride: usize,
mut factor_idx: usize,
mut fin_idx: usize,
mut fout_idx: usize,
) {
let p = self.factors[factor_idx]; let m = self.factors[factor_idx + 1]; factor_idx += 2;
let fout_idx_begin = fout_idx;
let fout_idx_end = fout_idx + p * m;
if m == 1 {
for (fount_n, fin_n) in fout[fout_idx..fout_idx_end]
.iter_mut()
.zip(fin[fin_idx..].iter().step_by(fstride * in_stride))
{
*fount_n = *fin_n;
}
} else {
loop {
self.kf_work(fout, fin, fstride * p, in_stride, factor_idx, fin_idx, fout_idx);
fin_idx += fstride * in_stride;
fout_idx += m;
if fout_idx == fout_idx_end {
break;
}
}
}
let fout_slice = &mut fout[fout_idx_begin..];
match p {
2 => self.kf_bfly2(fout_slice, fstride, m),
3 => self.kf_bfly3(fout_slice, fstride, m),
4 => self.kf_bfly4(fout_slice, fstride, m),
5 => self.kf_bfly5(fout_slice, fstride, m),
_ => self.kf_bfly_generic(fout_slice, fstride, m, p),
}
}
fn kf_bfly2(&self, fout: &mut [Complex], fstride: usize, m: usize) {
let (fout, fout2) = fout.split_at_mut(m);
for i in 0..m {
let t = fout2[i] * self.twiddle[i * fstride];
fout2[i] = fout[i] - t;
fout[i] += t;
}
}
fn kf_bfly4(&self, fout: &mut [Complex], fstride: usize, m: usize) {
let m2 = m * 2;
let m3 = m * 3;
let mut tw1 = 0;
let mut tw2 = 0;
let mut tw3 = 0;
for i in 0..m {
let scratch0 = fout[i + m] * self.twiddle[tw1];
let scratch1 = fout[i + m2] * self.twiddle[tw2];
let scratch2 = fout[i + m3] * self.twiddle[tw3];
let scratch5 = fout[i] - scratch1;
fout[i] += scratch1;
let scratch3 = scratch0 + scratch2;
let scratch4 = scratch0 - scratch2;
fout[i + m2] = fout[i] - scratch3;
fout[i] += scratch3;
tw1 += fstride;
tw2 += fstride * 2;
tw3 += fstride * 3;
if self.inverse {
fout[i + m] = Complex::new(scratch5.r - scratch4.i, scratch5.i + scratch4.r);
fout[i + m3] = Complex::new(scratch5.r + scratch4.i, scratch5.i - scratch4.r)
} else {
fout[i + m] = Complex::new(scratch5.r + scratch4.i, scratch5.i - scratch4.r);
fout[i + m3] = Complex::new(scratch5.r - scratch4.i, scratch5.i + scratch4.r)
}
}
}
fn kf_bfly3(&self, fout: &mut [Complex], fstride: usize, m: usize) {
let m2 = m * 2;
let epi3 = self.twiddle[fstride * m];
let mut tw1 = 0;
let mut tw2 = 0;
for i in 0..m {
let scratch1 = fout[i + m] * self.twiddle[tw1];
let scratch2 = fout[i + m2] * self.twiddle[tw2];
let scratch3 = scratch1 + scratch2;
let mut scratch0 = scratch1 - scratch2;
tw1 += fstride;
tw2 += fstride * 2;
let fouti = fout[i];
fout[i + m] = Complex {
r: fouti.r - (scratch3.r * 0.5),
i: fouti.i - (scratch3.i * 0.5),
};
scratch0 *= epi3.i;
fout[i] += scratch3;
let foutm = fout[i + m];
fout[i + m2] = Complex::new(foutm.r + scratch0.i, foutm.i - scratch0.r);
fout[i + m] = Complex::new(foutm.r - scratch0.i, foutm.i + scratch0.r);
}
}
fn kf_bfly5(&self, fout: &mut [Complex], fstride: usize, m: usize) {
let ya = self.twiddle[fstride * m];
let yb = self.twiddle[fstride * 2 * m];
let m1 = m;
let m2 = m * 2;
let m3 = m * 3;
let m4 = m * 4;
for i in 0..m {
let scratch0 = fout[i];
let scratch1 = fout[i + m1] * self.twiddle[i * fstride];
let scratch2 = fout[i + m2] * self.twiddle[i * 2 * fstride];
let scratch3 = fout[i + m3] * self.twiddle[i * 3 * fstride];
let scratch4 = fout[i + m4] * self.twiddle[i * 4 * fstride];
let scratch7 = scratch1 + scratch4;
let scratch10 = scratch1 - scratch4;
let scratch8 = scratch2 + scratch3;
let scratch9 = scratch2 - scratch3;
fout[i].r += scratch7.r + scratch8.r;
fout[i].i += scratch7.i + scratch8.i;
let scratch5 = Complex {
r: scratch0.r + (scratch7.r * ya.r) + (scratch8.r * yb.r),
i: scratch0.i + (scratch7.i * ya.r) + (scratch8.i * yb.r),
};
let scratch6 = Complex {
r: (scratch10.i * ya.i) + (scratch9.i * yb.i),
i: -(scratch10.r * ya.i) - (scratch9.r * yb.i),
};
fout[i + m1] = scratch5 - scratch6;
fout[i + m4] = scratch5 + scratch6;
let scratch11 = Complex {
r: scratch0.r + (scratch7.r * yb.r) + (scratch8.r * ya.r),
i: scratch0.i + (scratch7.i * yb.r) + (scratch8.i * ya.r),
};
let scratch12 = Complex {
r: -(scratch10.i * yb.i) + (scratch9.i * ya.i),
i: (scratch10.r * yb.i) - (scratch9.r * ya.i),
};
fout[i + m2] = scratch11 + scratch12;
fout[i + m3] = scratch11 - scratch12;
}
}
fn kf_bfly_generic(&self, fout: &mut [Complex], fstride: usize, m: usize, p: usize) {
let mut k;
let mut scratch = [Complex::default(); 480];
for u in 0..m {
k = u;
for scratch_q1 in &mut scratch[..m] {
*scratch_q1 = fout[k];
k += m;
}
k = u;
for _ in 0..p {
let mut twidx = 0;
fout[k] = scratch[0];
for scratch_q in &mut scratch[1..p] {
twidx += fstride * k;
if twidx >= self.nfft {
twidx -= self.nfft;
}
let t = *scratch_q * self.twiddle[twidx];
fout[k] += t;
}
k += m;
}
}
}
}
#[cfg(test)]
mod tests {
extern crate std;
use super::*;
use heapless::Vec;
#[test]
fn kissfft_non_inverse() {
#[rustfmt::skip] let i = [
1.855615, -116.9618, 14.5051155, 174.72545, -120.26939, -170.81615, 50.498768, 29.882944, 10.935101,
49.613552, -237.16743, 457.65805, 166.74161, -21.331354, -293.06955, -421.02695, -43.089565, -236.74684,
-158.98152, -89.15972, -239.22202, 63.367065, -10.9238825, 63.89845, -22.31455, -23.430895, 107.762054,
66.95872, 21.984243, -14.36995, 171.00829, 210.63702, 52.828213, -172.75896, -62.871365, -64.52872,
15.716181, -13.96337, -8.880015, -14.489226, 13.111881, -19.8435, -2.8529816, -49.936447, -58.36318,
39.232166, 25.754477, -121.766396, 104.3299, 28.673597, 3.1306546, -26.594135, 49.70436, -9.31528,
-15.198294, 40.402843, -40.498585, 22.088074, -42.21869, 29.077738, -39.54687, 4.737952, -33.50602,
42.880722, -37.41781, -10.550791, 2.7522528, 4.6168604, -4.7794275, -3.046289, -12.713707, 8.274482,
-17.892733, 2.5210698, -2.371197, 1.1339488, 2.3877633, -7.4157476, 9.761844, -25.59829, -6.217507,
-12.569953, 0.78294086, -9.83709, 5.5962806, 2.8984036, -11.539186, 2.994965, -5.26814, -0.11291276,
-10.728054, -3.672079, -12.828666, 6.368055, 8.137624, -2.7822683, 11.612916, 0.3329181, -6.4356623,
-9.272606, 0.0, 6.5034328, 0.0, 6.4365535, -5.6137466, -11.167647, 1.4393897, -1.3767548, -8.357129,
-4.210443, 0.0, -1.1256523, 0.0, 14.775933, 0.0, 5.0709095, 4.606481, 9.646306, -4.669522, 0.0, -4.9205875,
4.692957, -0.32278347, -0.38587618, 0.0, 0.0, -9.644276, 19.285547, 5.3756537, 0.0, -4.2342334, 5.469494,
0.0, 2.746046, -9.667411, 5.591327, 4.1758385, 10.586916, 4.107868, 0.0, 4.2955832, -2.8468177, -1.4351681,
4.918048, 1.3821423, 0.3282499, -15.087164, 9.986603, 2.7261941, -9.2014265, 15.278996, -6.504345,
9.124997, -10.493915, 13.827347, -15.75687, 6.722817, -2.5194824, 1.3646965, 5.03867, 8.2033415,
-1.8157947, -19.182993, 24.903381, -12.887679, 3.9698439, 11.647475, 7.5046215, 20.741192, -5.567567,
-0.43412608, 3.9644055, 18.985268, 8.338453, -11.85746, 34.65426, -16.266304, -10.606885, 29.178452,
45.139923, -1.6687171, 6.683766, 12.766115, 17.879122, 18.518923, -53.55422, 41.24579, -22.229282,
36.130154, 2.5491073, -20.51049, -96.19117, -13.34205, 103.88622, -50.21851, 41.127357, 56.53239,
25.900309, -24.370209, 35.459366, 8.997509, 6.981312, 18.012835, -4.197822, 4.049564, 43.40604, 62.774452,
53.30865, -138.9981, -236.68228, -114.54468, 34.831924, -105.52745, -84.97478, 20.664078, 75.2232,
-126.548836, 66.99325, 54.068527, 268.94693, -291.25183, 3.4395313, 74.424446, 142.78764, 158.3345,
127.41658, 208.42499, -67.842674, 14.712677, -289.51437, -79.78527, 75.367485, 36.252354, -178.21283,
58.44521, -41.093452, 13.163652, -219.22577, 119.97857, 45.07003,
];
#[rustfmt::skip] let r = [
-2268.1362, 15884.554, -1042.859, -8541.555, 4453.659, 5090.536, -1259.022, -640.3463, -205.43816, -829.74,
3573.6746, -6274.2817, -2096.7207, 247.70819, 3161.0686, 4239.2007, 406.76834, 2103.396, 1333.8778,
708.5689, 1805.6552, -455.3847, 74.90909, -418.95355, 140.14635, 141.19998, -624.07874, -373.19193,
-118.077156, 74.46896, -856.05585, -1019.6481, -247.5412, 784.3248, 276.79922, 275.7283, -65.22747,
56.331127, 34.845627, 55.340305, 49.49789, 71.93409, -83.632904, 172.19783, 196.44048, -41.29998,
3.1031866, 298.14954, -320.20282, -86.086266, -9.197882, -1.1975169, -63.744102, 25.702515, -17.486677,
-107.08196, 122.926926, -38.912254, 88.469284, -88.17956, 95.2541, 40.969517, 26.432041, -97.815445,
83.85707, 23.235506, -5.957274, 28.303192, 9.999217, 6.2674932, -10.877085, -16.472115, 35.045555,
-4.859095, 4.4979696, -2.1173005, -4.389146, 13.42149, -17.397625, 18.72114, 10.74872, -4.2159653,
-26.653864, 16.260973, -9.115551, 14.897083, -1.0913692, -4.670179, 8.0977125, 0.171099, -2.5562663,
-12.994965, 0.40452194, -9.120969, 6.395524, 3.8759496, -1.9462404, -0.45119837, -5.160027, -1.4159701,
0.0, 5.066773, 0.0, 5.151467, -4.553532, -9.180352, 9.565273, -9.57449, 9.768746, 4.8569202, 0.0,
-9.607248, 0.0, 4.4450912, 0.0, 4.7572193, -4.846271, 0.0071697235, 4.785559, 0.0, 4.912543, 4.76258,
9.640907, 9.638588, 0.0, 0.0, -0.1980052, 0.52224445, -4.832303, 0.0, -4.836194, -4.7258277, 0.0, 14.49062,
-0.32784557, -4.581033, 5.1653657, 0.051658154, 5.2195835, 0.0, 5.607607, 9.907007, -1.9252065, -7.228178,
1.9056323, 0.45886463, -3.4534411, -3.7460012, 3.9735973, -13.601632, 4.2780867, 8.922779, 14.076463,
-16.422688, 21.954798, -0.7839532, 10.990813, -4.180201, -23.11197, 8.612942, 14.235487, -3.1992202,
-7.7281303, 18.339417, 3.4489067, 7.441694, -12.700758, 14.522442, 5.011389, -11.128785, -0.88213927,
8.190503, 2.1540232, 17.818396, -25.776266, 116.01507, -36.615303, -24.30349, 16.492369, 107.23625,
-4.0379806, -0.41632032, 14.90279, 28.331722, 48.361664, -142.64403, 53.24363, -61.64924, 25.688564,
7.3704824, 18.976349, -290.37704, -41.177204, 327.9252, -162.19847, 135.97911, 191.42513, 89.86247,
-86.68163, 225.87943, 33.689384, 26.84368, 71.16971, -17.054327, 16.928648, 186.84947, 278.48138,
243.92023, -656.57544, -1155.2651, -578.32733, 182.10963, -571.9864, -478.1029, 120.85015, 457.95102,
-803.2506, 444.11896, 375.0647, 1956.2184, -2226.3508, 27.700428, 633.2471, 1287.5594, 1518.4009,
1304.6234, 2288.822, -803.1884, 188.91817, -4060.6724, -1232.7405, 1296.0394, 702.736, -3957.8958,
1519.2118, -1287.4844, 518.92303, -11649.14, 9776.242, 7869.8496,
];
let fin: Vec<Complex, 240> = i.iter().zip(r.iter()).map(|(i, r)| Complex { i: *i, r: *r }).collect();
let mut fout = [Complex { r: 0., i: 0. }; 240];
let mut buf = [Complex { r: 0., i: 0. }; 960];
let (kiss, _) = KissFastFourierTransform::new(240, false, &mut buf);
kiss.transform(&fin, &mut fout);
#[rustfmt::skip] let i_expected = [
-896.3453, -4488.7217, -7348.4097, -8887.922, -9056.146, -8633.422, -7771.1133, -6862.5684, -5966.8647,
-4794.6694, -3277.541, -2362.5103, -2648.3076, -4637.6445, -7722.6855, -10517.553, -12167.851, -11895.811,
-10294.406, -6730.038, -3189.6895, -212.61719, 1695.5674, 3190.8154, 3974.3643, 4835.683, 5128.774,
4992.3984, 4730.4004, 4389.5146, 4860.956, 6220.8086, 8196.442, 9534.402, 9965.566, 10040.663, 8795.088,
6908.338, 5099.788, 4367.3115, 5261.6016, 7660.9775, 10635.146, 12834.795, 14190.124, 15401.457, 15966.678,
15080.604, 11752.904, 6528.422, -47.00586, -6731.8564, -12396.52, -15380.836, -14591.621, -11277.795,
-7294.1143, -4296.804, -2964.048, -2926.015, -4714.9014, -7532.2715, -9407.004, -10135.502, -9732.238,
-7904.591, -6192.056, -6940.635, -9606.486, -12903.244, -15492.834, -18961.992, -23103.61, -27405.797,
-29388.904, -29414.887, -28263.479, -26479.133, -25115.285, -23378.51, -20494.865, -16748.465, -14825.195,
-13741.304, -12949.355, -11320.935, -9801.74, -8669.303, -7205.6455, -4593.545, -1380.3279, 1307.0344,
3371.2295, 3574.2065, 457.13086, -4329.633, -10391.41, -17537.012, -24489.14, -29612.652, -31701.77,
-30683.785, -27333.87, -23373.05, -19662.098, -15592.686, -10969.874, -9652.483, -12050.671, -15667.203,
-17695.102, -17627.69, -17042.242, -15240.029, -10514.387, -4804.282, -2072.7874, -449.5752, 695.19995,
979.40796, -337.52313, -980.9072, -455.32568, 1171.4072, 3089.063, 7586.956, 13284.129, 16304.463,
17308.146, 17924.953, 17094.623, 13899.211, 10404.032, 9899.697, 13289.42, 17761.877, 21285.863, 25417.158,
29363.082, 31639.887, 31055.436, 27179.994, 21219.563, 13975.247, 7321.411, 1677.0073, -2168.726,
-3824.5117, -2663.2678, 221.7998, 3101.007, 6073.8594, 8232.202, 9236.254, 10254.33, 12356.3545, 13808.623,
14356.297, 15419.96, 18305.484, 22229.963, 24700.785, 26010.158, 27290.873, 28570.363, 29864.484,
28959.229, 25460.232, 21015.799, 16974.258, 14256.016, 11488.241, 8214.137, 6183.765, 6754.65, 9111.895,
10397.212, 9749.779, 8525.632, 6088.3584, 4015.7473, 2787.0684, 3240.4868, 5434.22, 9199.211, 13330.116,
15496.234, 14242.875, 9488.965, 3375.684, -3224.5352, -9347.34, -13943.14, -15577.625, -15884.67,
-15076.272, -13680.928, -11553.199, -9096.786, -6496.449, -4688.635, -4697.0884, -5807.9595, -7819.5557,
-9396.357, -10362.897, -10247.072, -8893.734, -6961.0557, -5423.092, -4749.4805, -4762.9385, -4830.626,
-5091.946, -4845.26, -4570.2734, -3763.0854, -2455.878, -831.5654, 1664.6982, 4887.417, 8525.93, 11249.24,
12326.012, 11468.231, 9091.9, 6077.5947, 3478.9658, 2179.4658, 2611.1597, 4045.4658, 5397.168, 6509.3906,
7333.2676, 8053.2095, 8909.406, 9262.348, 8129.1543, 6024.133, 2698.7732,
];
#[rustfmt::skip] let r_expected = [
22463.031, 20687.656, 18938.639, 16552.432, 13054.607, 8478.361, 4482.206, 1863.2534, 2557.4702, 8585.197,
19899.19, 32245.17, 39807.28, 40962.65, 38021.574, 33295.973, 27790.02, 21848.932, 16815.977, 13104.005,
9667.043, 5442.6763, 775.18945, -2316.7344, -968.5049, 5868.5225, 15741.287, 24789.06, 30714.914, 33193.49,
33878.58, 34844.12, 35693.54, 35565.125, 33593.215, 29715.025, 23825.605, 15314.43, 7737.08, 3866.3477,
4978.002, 8941.834, 12872.077, 14767.818, 14871.5625, 15655.988, 18796.977, 23558.57, 28997.191, 33544.406,
36017.28, 34786.617, 29209.95, 23260.598, 19942.143, 18162.797, 14536.174, 7955.586, -627.1543, -9538.855,
-19004.139, -26946.303, -30561.172, -30699.252, -28782.04, -25127.66, -23012.48, -25992.322, -31707.596,
-35531.313, -35675.34, -35293.832, -36190.34, -37733.305, -37519.57, -35751.14, -34812.695, -35263.25,
-36943.71, -37698.863, -36002.227, -34473.12, -36260.977, -40381.52, -43735.594, -44696.824, -44037.0,
-43954.566, -44084.18, -42908.184, -40703.773, -37639.47, -33198.063, -28522.533, -24583.81, -19996.504,
-15642.409, -13006.758, -11780.721, -11306.558, -10891.8545, -11192.351, -12074.218, -14598.984, -18563.43,
-20779.35, -20379.64, -20249.375, -20987.2, -20362.086, -16455.709, -9746.61, -4073.0903, 255.51074,
6277.117, 13193.121, 17535.482, 18126.527, 16800.584, 15386.298, 14923.699, 15798.797, 17766.568,
18348.041, 15587.701, 9876.861, 3142.5303, -1758.3931, -6900.569, -13342.086, -18483.709, -20936.006,
-20818.42, -20057.115, -20887.691, -20048.611, -16067.883, -13299.018, -11584.943, -10958.364, -10984.371,
-11256.848, -12123.95, -14255.818, -17959.262, -22185.246, -26248.416, -30952.955, -35588.305, -39435.88,
-41809.953, -43599.926, -44190.477, -43873.47, -44160.754, -44626.99, -42515.285, -38196.797, -34631.055,
-34777.13, -37341.22, -37635.313, -35930.1, -34778.68, -34911.813, -36671.19, -37997.477, -37040.59,
-35457.625, -35227.188, -35935.273, -34238.93, -28718.543, -23645.973, -23746.943, -27160.195, -30056.264,
-30512.848, -29239.018, -23755.016, -14422.357, -4779.3516, 3928.8828, 11605.482, 16692.398, 18875.734,
21315.664, 26091.303, 32388.385, 36319.844, 34928.332, 31071.982, 26291.984, 21370.008, 17027.232,
14918.146, 14491.955, 14206.109, 11248.764, 6760.414, 4022.662, 4988.824, 11078.184, 19751.332, 27246.023,
31910.879, 34627.688, 35954.293, 35219.836, 34289.316, 33597.43, 32285.844, 28411.428, 20499.744,
10343.416, 2068.959, -2232.4482, -1369.6509, 3216.06, 7660.507, 11380.53, 14872.581, 19196.238, 24769.77,
30682.438, 35799.902, 40010.426, 40863.52, 36800.28, 26381.797, 14009.543, 4786.0664, 1523.4565, 2811.4375,
6413.793, 10949.314, 15123.091, 17686.055, 19688.299, 21926.379,
];
let i_actual: Vec<f32, 240> = fout.iter().map(|x| x.i).collect();
let r_actual: Vec<f32, 240> = fout.iter().map(|x| x.r).collect();
assert_eq!(i_actual, i_expected);
assert_eq!(r_actual, r_expected);
}
}