Finalizes fft, working-ish bfsk

src/fft/rader2.rs

@@ -1,26 +1,29 @@
 // Implementation of raders's fft for prime sized ffts
-/*
 
 use std::f32::consts::PI;
 
 use crate::{
     complex::Complex32,
-    fft::{create_fft, dft::NaiveDFT, is_prime, DFTAlgorithm, FFTDirection},
+    fft::{
+        DFTAlgorithm, FFTDirection, create_fft, dft::NaiveDFT, is_prime, mixed_radix::MixedRadixFFT,
+    },
 };
 
-pub struct RaderFFT {
+pub struct Rader2FFT {
     permutations: Box<[usize]>,
     convolution_operand: Box<[Complex32]>,
 
+    convolution_fft_input: Box<[Complex32]>,
     convolution_ifft: Box<dyn DFTAlgorithm>,
     convolution_fft: Box<dyn DFTAlgorithm>,
 
     output: Box<[Complex32]>,
 
+    sub_size: usize,
     size: usize,
 }
 
-impl DFTAlgorithm for RaderFFT {
+impl DFTAlgorithm for Rader2FFT {
     fn create(size: usize, direction: FFTDirection) -> Self
     where
         Self: Sized,
@@ -30,54 +33,69 @@ impl DFTAlgorithm for RaderFFT {
         // Primitive root and its powers
         let g = compute_prime_primitive_root(size);
         let permutations: Box<[usize]> = (0..(size - 1)).map(|i| exp_mod(g, i + 1, size)).collect();
+        let sub_size = next_pow2(2 * size - 3);
+        println!("{}", sub_size);
 
         // Compute fourrier transform of twiddle factors
-        let mut convolution_fft = create_fft(size - 1, FFTDirection::Forward);
-        //let mut convolution_fft = Box::new(NaiveDFT::create(size - 1, FFTDirection::Forward));
-        let mut convolution_operand = (0..(size - 1))
-            .map(|i| {Complex32::cexp(-2. * direction.sign() * PI * (permutations[i] as f32) / (size as f32))})
+        let twiddle_factors = (0..sub_size)
+            .map(|i| {
+                Complex32::cexp(
+                    -2. * PI * direction.sign() * (permutations[i % (size - 1)] as f32)
+                        / (size as f32),
+                )
+            })
             .collect::<Vec<Complex32>>();
-        convolution_fft.execute(&convolution_operand);
-        convolution_operand = Vec::from(convolution_fft.get_output());
 
-        RaderFFT {
+        let mut convolution_fft = create_fft(sub_size, FFTDirection::Forward);
+        convolution_fft.execute(&twiddle_factors);
+        Rader2FFT {
             permutations,
-            convolution_operand: convolution_operand.into(),
+            convolution_operand: convolution_fft.get_output().iter().copied().collect(),
 
-            convolution_ifft: create_fft(size - 1, FFTDirection::Inverse),
-            //convolution_ifft: Box::new(NaiveDFT::create(size - 1, FFTDirection::Inverse)),
             convolution_fft,
+            convolution_ifft: create_fft(sub_size, FFTDirection::Inverse),
+            convolution_fft_input: vec![Complex32::zero(); sub_size].into(),
 
             output: vec![Complex32::zero(); size].into(),
             size,
+            sub_size,
         }
     }
 
     fn execute(&mut self, input: &[Complex32]) {
         // Compute fft of input signal
-        for i in 0..(self.size - 1) {
+
+        self.convolution_fft_input[0] = input[self.permutations[self.size - 2]];
+        for i in 0..(self.sub_size - self.size + 1) {
+            self.convolution_fft_input[i + 1] = Complex32::zero();
+        }
+        for i in 1..(self.size - 1) {
+            // reverse sequence
             let k = self.permutations[self.size - 1 - i - 1];
-            // Using output as staging buffer
-            self.output[i] = input[k];
+            self.convolution_fft_input[i + self.sub_size - self.size + 1] = input[k];
         }
 
-        self.convolution_fft.execute(&self.output);
+        self.convolution_fft.execute(&self.convolution_fft_input);
 
         // Compute convolution by multiplying in freq domain
-        for i in 0..(self.size - 1) {
+        for i in 0..self.sub_size {
             // Using output as staging buffer
-            self.output[i] = self.convolution_fft.get_output()[i] * self.convolution_operand[i];
+            self.convolution_fft_input[i] =
+                self.convolution_fft.get_output()[i] * self.convolution_operand[i];
         }
 
-        self.convolution_ifft.execute(&self.output);
+        self.convolution_ifft.execute(&self.convolution_fft_input);
 
-        self.output[0] = input[0];
+        self.output[0] = Complex32::zero();
+        for x in input {
+            self.output[0] = self.output[0] + *x;
+        }
 
         for i in 0..(self.size - 1) {
             // Actually compute the output
             let k = self.permutations[i];
-            self.output[k] = (self.convolution_ifft.get_output()[i] / (self.size - 1) as f32) + input[0];
-            self.output[0] = self.output[0] + input[i + 1];
+            self.output[k] =
+                (self.convolution_ifft.get_output()[i] / (self.sub_size) as f32) + input[0];
         }
     }
 
@@ -131,4 +149,15 @@ pub fn exp_mod(mut n: usize, mut exp: usize, m: usize) -> usize {
     r
 }
 
-*/
+pub fn next_pow2(mut n: usize) -> usize {
+    if n.count_ones() == 1 {
+        n
+    } else {
+        let mut p = 0;
+        while n > 0 {
+            n >>= 1;
+            p += 1;
+        }
+        1 << p
+    }
+}