diff --git a/src/bfsk.rs b/src/bfsk.rs
new file mode 100644
index 0000000..a16f831
--- /dev/null
+++ b/src/bfsk.rs
@@ -0,0 +1,60 @@
+// 2-FSK Modulator
+
+use crate::complex::Complex;
+use crate::fft::FFT;
+use crate::nco::Nco;
+
+pub struct BFSKMod<'a, T: Iterator<Item = bool>> {
+    samples_per_bit: u32,
+    bandwidth: f32,
+    bit_stream: &'a mut T,
+
+    // State
+    oscillator: Nco,
+    sample_index: u32,
+}
+
+impl<'a, T> BFSKMod<'a, T>
+where
+    T: Iterator<Item = bool>,
+{
+    pub fn new(samples_per_bit: u32, bandwidth: f32, bit_stream: &'a mut T) -> Self {
+        BFSKMod {
+            samples_per_bit,
+            bandwidth,
+            oscillator: Nco::new(0.0),
+            bit_stream,
+            sample_index: samples_per_bit,
+        }
+    }
+
+    pub fn step_modulate(&mut self) -> Option<Complex<f32>> {
+        if self.sample_index == self.samples_per_bit {
+            self.sample_index = 0;
+            let bit = self.bit_stream.next()?;
+
+            let frequency = if bit {
+                self.bandwidth / 2.0
+            } else {
+                -self.bandwidth / 2.0
+            };
+            self.oscillator.set_frequency(frequency);
+        }
+
+        self.sample_index += 1;
+        self.oscillator.step();
+        Some(self.oscillator.cexp())
+    }
+}
+
+// FSK Demodulator (dumb non coherent + no symbol timing recovery)
+pub struct BFSKDem {
+    samples_per_bit: u32,
+    deviation: f32,
+
+    // State
+    sample_index: u32,
+    fft: FFT,
+}
+
+impl BFSKDem {}
diff --git a/src/complex.rs b/src/complex.rs
index 73658ca..a8ab593 100644
--- a/src/complex.rs
+++ b/src/complex.rs
@@ -1,22 +1,22 @@
-use std::{fmt::Display, ops::{Add, Div, Mul, Neg, Sub}};
+use std::{
+    f32::consts::PI,
+    fmt::Display,
+    ops::{Add, Div, Mul, Neg, Sub},
+};
 
 #[derive(Copy, Clone, Debug)]
-pub struct Complex<T>
-{
+pub struct Complex<T> {
     pub re: T,
-    pub im: T
+    pub im: T,
 }
 
-impl<T> Complex<T>
-{
-    pub fn new(re: T, im: T) -> Self
-    {
+impl<T> Complex<T> {
+    pub fn new(re: T, im: T) -> Self {
         Complex { re, im }
     }
 }
 
-impl<T: Clone + Add<Output = T>> Add<Complex<T>> for Complex<T>
-{
+impl<T: Clone + Add<Output = T>> Add<Complex<T>> for Complex<T> {
     type Output = Complex<T>;
 
     #[inline]
@@ -25,8 +25,7 @@ impl<T: Clone + Add<Output = T>> Add<Complex<T>> for Complex<T>
     }
 }
 
-impl<T: Clone + Sub<Output = T>> Sub<Complex<T>> for Complex<T>
-{
+impl<T: Clone + Sub<Output = T>> Sub<Complex<T>> for Complex<T> {
     type Output = Complex<T>;
 
     #[inline]
@@ -35,8 +34,7 @@ impl<T: Clone + Sub<Output = T>> Sub<Complex<T>> for Complex<T>
     }
 }
 
-impl<T: Clone + Add<Output = T>> Add<T> for Complex<T>
-{
+impl<T: Clone + Add<Output = T>> Add<T> for Complex<T> {
     type Output = Complex<T>;
 
     #[inline]
@@ -45,18 +43,21 @@ impl<T: Clone + Add<Output = T>> Add<T> for Complex<T>
     }
 }
 
-impl<T: Clone + Add<Output = T> + Mul<Output = T> + Sub<Output = T>> Mul<Complex<T>> for Complex<T>
+impl<T: Clone + Add<Output = T> + Mul<Output = T> + Sub<Output = T>> Mul<Complex<T>>
+    for Complex<T>
 {
     type Output = Complex<T>;
 
     #[inline]
     fn mul(self, rhs: Complex<T>) -> Self::Output {
-        self::Complex::new(self.re.clone() * rhs.re.clone() - self.im.clone() * rhs.im.clone(), self.re * rhs.im + self.im * rhs.re)
+        self::Complex::new(
+            self.re.clone() * rhs.re.clone() - self.im.clone() * rhs.im.clone(),
+            self.re * rhs.im + self.im * rhs.re,
+        )
     }
 }
 
-impl<T: Clone + Mul<Output = T>> Mul<T> for Complex<T>
-{
+impl<T: Clone + Mul<Output = T>> Mul<T> for Complex<T> {
     type Output = Complex<T>;
 
     #[inline]
@@ -65,8 +66,7 @@ impl<T: Clone + Mul<Output = T>> Mul<T> for Complex<T>
     }
 }
 
-impl<T: Clone + Neg<Output = T>> Neg for Complex<T>
-{
+impl<T: Clone + Neg<Output = T>> Neg for Complex<T> {
     type Output = Complex<T>;
 
     fn neg(self) -> Self::Output {
@@ -74,17 +74,42 @@ impl<T: Clone + Neg<Output = T>> Neg for Complex<T>
     }
 }
 
-impl<T: Clone + Neg<Output = T>> Complex<T>
-{
-    pub fn conj(&self) -> Complex<T>
-    {
+impl<T: Clone + Neg<Output = T>> Complex<T> {
+    pub fn conj(&self) -> Complex<T> {
         Self::new(self.re.clone(), -self.im.clone())
     }
 }
 
-impl<T: Display> Display for Complex<T>
-{
+impl<T: Display> Display for Complex<T> {
     fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
         write!(f, "{} + i{}", self.re, self.im)
     }
 }
+
+pub type Complex32 = Complex<f32>;
+impl Complex32 {
+    pub fn zero() -> Self {
+        Complex { re: 0.0, im: 0.0 }
+    }
+
+    pub fn cexp(angle: f32) -> Complex32 {
+        Complex32 {
+            re: angle.cos(),
+            im: angle.sin(),
+        }
+    }
+
+    pub fn mag(&self) -> f32 {
+        (self.re * self.re + self.im * self.im).sqrt()
+    }
+
+    pub fn arg(&self) -> f32 {
+        if self.im == 0. {
+            if self.re >= 0. { 0. } else { PI }
+        } else if self.re == 0. {
+            if self.im >= 0. { PI / 2.0 } else { -PI / 2.0 }
+        } else {
+            (self.im / self.re).atan()
+        }
+    }
+}
diff --git a/src/fft.rs b/src/fft.rs
new file mode 100644
index 0000000..c88d240
--- /dev/null
+++ b/src/fft.rs
@@ -0,0 +1,9 @@
+pub mod mixed_radix;
+pub mod radix2;
+use crate::complex::Complex32;
+
+pub trait DFT<'a> {
+    fn create(size: usize, input: &'a [Complex32], output: &'a mut [Complex32]) -> Self;
+
+    fn execute(&mut self);
+}
diff --git a/src/fft/mixed_radix.rs b/src/fft/mixed_radix.rs
new file mode 100644
index 0000000..2585b5e
--- /dev/null
+++ b/src/fft/mixed_radix.rs
@@ -0,0 +1,73 @@
+// To perform a mixed radix cooley tuckey fft
+
+use crate::{complex::Complex32, fft::DFT};
+
+pub struct MixedRadixFFT<'a> {
+    input_buffer: &'a [Complex32],
+    output_buffer: &'a mut [Complex32],
+    size: usize,
+
+    p: usize,
+    q: usize,
+}
+
+impl<'a> DFT<'a> for MixedRadixFFT<'a> {
+    fn create(size: usize, input: &'a [Complex32], output: &'a mut [Complex32]) -> Self {
+        let q = decide_radix_factor(size);
+        let p = size / q;
+
+        MixedRadixFFT {
+            input_buffer: input,
+            output_buffer: output,
+            size,
+
+            p,
+            q,
+        }
+    }
+
+    fn execute(&mut self) {}
+}
+
+// This will decide on a good factor to use for the mixed radix fft
+fn decide_radix_factor(n: usize) -> usize {
+    let factors = prime_factors(n);
+    let two_count = factors.iter().take_while(|i| **i == 2).count();
+
+    // If there is a lot of two, we can use them as q factor to be able to use radix2 later on
+    if two_count > 0 {
+        return 1 << two_count;
+    }
+
+    // Otherwise take next big prime
+    return *factors.iter().skip(two_count).next().unwrap();
+}
+
+// Utilities
+fn prime_factors(n: usize) -> Vec<usize> {
+    let mut factors = vec![];
+
+    let mut num = n;
+    // Divide num successively
+    while num != 1 {
+        // Try divisors from 2 up to n (included)
+        for i in 2..n + 1 {
+            // if i divides num, it is a prime factor (if it wasn't, then i would have prime
+            // factors that would divide into num before i)
+            if num % i == 0 {
+                factors.push(i);
+                num /= i;
+            }
+        }
+    }
+
+    // If n = 1 then it does not have any prime factors
+    // The prime factor decomposition theorem states that any integer
+    // greater than TWO has a unique decomposition
+
+    factors
+}
+
+fn is_prime(n: usize) -> bool {
+    prime_factors(n).len() == 1
+}
diff --git a/src/fft/radix2.rs b/src/fft/radix2.rs
new file mode 100644
index 0000000..a758e43
--- /dev/null
+++ b/src/fft/radix2.rs
@@ -0,0 +1,79 @@
+// Cooley-Tukey algorithm
+
+use crate::complex::Complex32;
+use crate::fft::DFT;
+use std::f32::consts::PI;
+
+pub struct Radix2FFT<'a> {
+    output_buffer: &'a mut [Complex32],
+    input_buffer: &'a [Complex32],
+    size: usize,
+    length: usize,
+}
+
+impl<'a> DFT<'a> for Radix2FFT<'a> {
+    // Size as power of two
+    fn create(size: usize, input: &'a [Complex32], output: &'a mut [Complex32]) -> Self {
+        if !is_power_of_two(size) {
+            panic!("Tried to create a Radix2 FFT with a non power of two size.");
+        }
+
+        Radix2FFT {
+            output_buffer: output,
+            input_buffer: input,
+            size: size.ilog2() as usize,
+            length: size,
+        }
+    }
+
+    fn execute(&mut self) {
+        // Reorder samples
+        for (i, x) in self.output_buffer.iter_mut().enumerate() {
+            *x = self.input_buffer[reverse_bits(i, self.size as u32)];
+        }
+
+        for step in 1..self.size {
+            let pol_length = 2usize.pow(step as u32);
+            let mid_point = pol_length / 2;
+            for s in (0..(self.length / pol_length)).map(|i| i * pol_length) {
+                for i in 0..mid_point {
+                    // Compute current polynomial at each unit root
+                    let a = self.output_buffer[s + i];
+                    let b = self.output_buffer[s + i + mid_point];
+                    let angle = 2. * PI * (i as f32) / (pol_length as f32);
+                    let phasor = Complex32::cexp(angle);
+                    self.output_buffer[i + s] = a + phasor * b;
+                    self.output_buffer[i + s + mid_point] = a - phasor * b;
+                }
+            }
+        }
+    }
+}
+
+// Utilities
+pub fn reverse_bits(n: usize, bit_count: u32) -> usize {
+    let mut num = n;
+    let mut output = 0usize;
+    for _ in 0..bit_count {
+        output <<= 1;
+        output |= if (num & 1) == 1 { 0 } else { 1 };
+        num >>= 1;
+    }
+
+    output
+}
+
+fn is_power_of_two(n: usize) -> bool {
+    if n == 0 {
+        return false;
+    }
+
+    let mut num = n;
+    while num != 1 {
+        if num % 2 != 0 {
+            return false;
+        }
+        num /= 2;
+    }
+    return true;
+}
diff --git a/src/main.rs b/src/main.rs
index 332d92e..7d74e2b 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -1,12 +1,21 @@
 use std::{
     f32::consts::PI,
     fs::File,
-    io::Read,
+    io::{Read, Write},
     ops::{Add, Div, Mul, Sub},
 };
 
+mod bfsk;
 mod complex;
+pub mod fft;
+mod nco;
+
+use bfsk::BFSKMod;
 use complex::Complex;
+use complex::Complex32;
+use nco::Nco;
+
+use crate::fft::FFT;
 
 // Utilities
 fn map<T>(input: T, in_min: T, in_max: T, out_min: T, out_max: T) -> T
@@ -20,142 +29,44 @@ fn euclid_mod(a: f32, m: f32) -> f32 {
     let r = a % m;
     if r < 0.0 { r + m } else { r }
 }
-
-struct Nco {
-    // Phase of NCO
-    theta: u32,  // 0 <=> 0, 0xFFFFFFFF <=> 2pi
-    dtheta: u32, // Dtheta = freq : f = dtheta/dt
+fn main() {
+    test();
 }
 
-impl Nco {
-    pub fn new(freq: f32) -> Self {
-        let mut nco = Nco {
-            theta: 0,
-            dtheta: 0,
-        };
-        nco.set_frequency(freq);
-        nco
-    }
+fn test() {
+    let freq1 = 2. * PI / 4.0;
+    let freq2 = 2. * PI / 8.0;
 
-    // Sets freq, freq in radian per sample
-    pub fn set_frequency(&mut self, freq: f32) {
-        self.dtheta = map(euclid_mod(freq, 2. * PI), 0., 2. * PI, 0., 0xFFFFFFFFu32 as f32).floor() as u32;
-    }
-
-    // Adjusts freq, freq in radian per sample
-    pub fn adjust_frequency(&mut self, freq_off: f32) {
-        self.set_frequency(self.get_frequency() + freq_off);
-    }
-
-    pub fn get_frequency(&self) -> f32 {
-        map(self.dtheta as f32, 0., 0xFFFFFFFFu32 as f32, 0., 2. * PI)
-    }
-
-    pub fn step(&mut self) {
-        let bef = self.theta as i64;
-        self.theta = self.theta.overflowing_add(self.dtheta).0;
-    }
-
-    pub fn step_n(&mut self, n: u32) {
-        self.theta = self
-            .theta
-            .overflowing_add(self.dtheta.overflowing_mul(n).0)
-            .0;
-    }
-
-    pub fn sin(&self) -> f32 {
-        map(self.theta as f32, 0., 0xFFFFFFFFu32 as f32, 0., 2. * PI).sin()
-    }
-
-    pub fn cos(&self) -> f32 {
-        map(self.theta as f32, 0., 0xFFFFFFFFu32 as f32, 0., 2. * PI).cos()
-    }
-
-    pub fn cexp(&self) -> Complex<f32>
-    {
-        Complex::new(self.cos(), self.sin())
-    }
-}
-
-struct BFSKMod<'a, T: Iterator<Item = bool>> {
-    samples_per_bit: u32,
-    bandwidth: f32,
-    bit_stream: &'a mut T,
-
-    // State
-    oscillator: Nco,
-    sample_index: u32,
-}
-
-impl<'a, T> BFSKMod<'a, T>
-where
-    T: Iterator<Item = bool>,
-{
-    pub fn new(samples_per_bit: u32, bandwidth: f32, bit_stream: &'a mut T) -> Self {
-        BFSKMod {
-            samples_per_bit,
-            bandwidth,
-            oscillator: Nco::new(0.0),
-            bit_stream,
-            sample_index: samples_per_bit,
-        }
-    }
-
-    pub fn step_modulate(&mut self) -> Option<Complex<f32>> {
-        if self.sample_index == self.samples_per_bit {
-            self.sample_index = 0;
-            let bit = self.bit_stream.next()?;
-
-            let frequency = if bit { self.bandwidth / 2.0 } else { -self.bandwidth / 2.0 };
-            self.oscillator.set_frequency(frequency);
-        }
-
-        self.sample_index += 1;
-        self.oscillator.step();
-        Some(self.oscillator.cexp())
-    }
-}
-
-fn main()
-{
-    modulate();
-}
-
-fn test()
-{
-    let sample_rate = 44100;
-    let f1 = -100.0; //HZ
-    let f2 = 500.0; //HZ
-
-    let spec = hound::WavSpec {
-        channels: 1,
-        sample_rate,
-        bits_per_sample: 16,
-        sample_format: hound::SampleFormat::Int,
-    };
-    let mut writer = hound::WavWriter::create("sine.wav", spec).unwrap();
-
-    let mut o1 = Nco::new(2. * PI * (f1 / sample_rate as f32));
-    let mut o2 = Nco::new(2. * PI * (f2 / sample_rate as f32));
-
-    for i in 0..sample_rate
-    {
-        let amplitude = i16::MAX as f32;
-        let sample = o1.cexp() * o2.cexp();
-        writer.write_sample((amplitude * sample.re) as i16).unwrap();
+    let mut o1 = Nco::new(freq1);
+    let mut o2 = Nco::new(freq2);
 
+    let mut vals = [Complex32::zero(); 8192];
+    for x in vals.iter_mut() {
+        *x = o1.cexp() + o2.cexp();
+        //*x = o2.cexp(); //+ o2.cexp();
+        //*x = *x * (1. / x.mag());
         o1.step();
         o2.step();
     }
 
-    writer.finalize().unwrap();
+    let mut fft = FFT::new(13);
+    let output = fft.run_fft(&vals);
+
+    let mut f = File::create("out.csv").unwrap();
+    for (i, v) in output.iter().enumerate() {
+        f.write_all(
+            format!("{},{},{},\n", i as f32 / 8192., v.mag(), v.arg())
+                .to_string()
+                .as_bytes(),
+        )
+        .unwrap();
+    }
 }
 
 fn modulate() {
     let sample_rate = 44100;
     let mut frequency = 2000.0; //HZ
     let mut bandwidth = 500.0; //HZ
-                               
 
     let path = "a.jpg";
     let file = File::open(path).unwrap();
@@ -176,7 +87,7 @@ fn modulate() {
     //let mut bit_stream = (0..22000).flat_map(|_| [true, false]);
 
     let baud_rate = 400;
-    println!("{} samples/bit", sample_rate/baud_rate);
+    println!("{} samples/bit", sample_rate / baud_rate);
     let mut bfsk = BFSKMod::new(
         sample_rate / baud_rate,
         2. * PI * (bandwidth / sample_rate as f32),
@@ -200,7 +111,9 @@ fn modulate() {
         let c_sample = lo.cexp() * sample;
 
         let filtered = prev + (c_sample - prev) * alpha;
-        writer.write_sample((amplitude * c_sample.re) as i16).unwrap();
+        writer
+            .write_sample((amplitude * c_sample.re) as i16)
+            .unwrap();
         lo.step();
     }
     writer.finalize().unwrap();
diff --git a/src/nco.rs b/src/nco.rs
new file mode 100644
index 0000000..5c8856a
--- /dev/null
+++ b/src/nco.rs
@@ -0,0 +1,99 @@
+// Numerically controlled oscillator
+
+use crate::complex::Complex;
+use std::f32::consts::PI;
+use std::ops::{Add, Div, Mul, Sub};
+
+// Utilities
+fn map<T>(input: T, in_min: T, in_max: T, out_min: T, out_max: T) -> T
+where
+    T: Clone + Add<Output = T> + Mul<Output = T> + Sub<Output = T> + Div<Output = T>,
+{
+    ((input - in_min.clone()) / (in_max - in_min)) * (out_max - out_min.clone()) + out_min
+}
+
+fn euclid_mod(a: f32, m: f32) -> f32 {
+    let r = a % m;
+    if r < 0.0 { r + m } else { r }
+}
+
+pub struct Nco {
+    // Phase of NCO
+    theta: u32,  // 0 <=> 0, 0xFFFFFFFF <=> 2pi
+    dtheta: u32, // Dtheta = freq : f = dtheta/dt
+}
+
+impl Nco {
+    pub fn new(freq: f32) -> Self {
+        let mut nco = Nco {
+            theta: 0,
+            dtheta: 0,
+        };
+        nco.set_frequency(freq);
+        nco
+    }
+
+    // Sets freq, freq in radian per sample
+    pub fn set_frequency(&mut self, freq: f32) {
+        self.dtheta = map(
+            euclid_mod(freq, 2. * PI),
+            0.,
+            2. * PI,
+            0.,
+            0xFFFFFFFFu32 as f32,
+        )
+        .floor() as u32;
+    }
+
+    // Adjusts freq, freq in radian per sample
+    pub fn adjust_frequency(&mut self, freq_off: f32) {
+        self.set_frequency(self.get_frequency() + freq_off);
+    }
+
+    pub fn adjust_phase(&mut self, phase_off: f32) {
+        let offset = map(
+            euclid_mod(phase_off, 2. * PI),
+            0.,
+            2. * PI,
+            0.,
+            0xFFFFFFFFu32 as f32,
+        )
+        .floor() as u32;
+        self.theta = self.theta.overflowing_add(offset).0;
+    }
+
+    pub fn get_frequency(&self) -> f32 {
+        map(self.dtheta as f32, 0., 0xFFFFFFFFu32 as f32, 0., 2. * PI)
+    }
+
+    pub fn step(&mut self) {
+        let bef = self.theta as i64;
+        self.theta = self.theta.overflowing_add(self.dtheta).0;
+    }
+
+    pub fn step_n(&mut self, n: u32) {
+        self.theta = self
+            .theta
+            .overflowing_add(self.dtheta.overflowing_mul(n).0)
+            .0;
+    }
+
+    pub fn sin(&self) -> f32 {
+        map(self.theta as f32, 0., 0xFFFFFFFFu32 as f32, 0., 2. * PI).sin()
+    }
+
+    pub fn cos(&self) -> f32 {
+        map(self.theta as f32, 0., 0xFFFFFFFFu32 as f32, 0., 2. * PI).cos()
+    }
+
+    pub fn cexp(&self) -> Complex<f32> {
+        //Complex::new(self.cos(), self.sin())
+        Complex::cexp(map(
+            self.theta as f32,
+            0.,
+            0xFFFFFFFFu32 as f32,
+            0.,
+            2. * PI,
+        ))
+    }
+}