diff --git a/tests/examples/test-fft.cpp b/tests/examples/test-fft.cpp
index 1531742..fb759e0 100644
--- a/tests/examples/test-fft.cpp
+++ b/tests/examples/test-fft.cpp
@@ -1,13 +1,50 @@
 #include <math.hpp>
 #include <cmath>
+#include <fstream>
+#include <ctime>
+
+void create_plot_file(std::string filename, const math::csignal& tfd) {
+  std::ofstream data_file(filename + ".data");
+  for (int i=0; i<tfd.size(); ++i) {
+    data_file << tfd[i].real()
+              << " "
+              << tfd[i].imag()
+              << std::endl;
+  }
+  data_file.close();
+}
 
 int main(int argc, char** argv) {
   math::csignal s;
+  double fe = 6000;
+  double f0 = 400;
+  int n = 30;
+  if (argc > 1) {
+    n = atoi(argv[1]);
+  }
 
   for (int i=0; i<100; ++i) {
-    s.push_back(std::sin(2*math::pi()*50*i/100));
+    s.push_back(math::complex(std::sin(2*math::pi()*f0*float(i)/fe), 0));
   }
-  math::csignal tfd = math::fft(s);
 
+  math::csignal tfd;
+  clock_t begin = std::clock();
+
+  for (int i=0; i<n; ++i) {
+    tfd = math::fft(s, 2000);
+  }
+
+  clock_t end = clock();
+  std::cout << "Time to compute " << n << " fft: "<< double(end-begin) / CLOCKS_PER_SEC << std::endl;
+  std::cout << "Average time: " << double(end-begin) / CLOCKS_PER_SEC / n << std::endl;
+
+  math::csignal mod;
+  for (int i=0; i<tfd.size(); ++i) {
+    double R = tfd[i].real();
+    double I = tfd[i].imag();
+    double a = std::sqrt(R*R + I*I);
+    mod.push_back(math::complex(float(i)/tfd.size()*fe, a));
+  }
+  create_plot_file("graph", mod);
   return 0;
 }
diff --git a/tests/src/math.hpp b/tests/src/math.hpp
index fe5c556..a853c34 100644
--- a/tests/src/math.hpp
+++ b/tests/src/math.hpp
@@ -4,7 +4,6 @@
 #include <complex>
 #include <opencv2/opencv.hpp>
 #include <iterator>
-#include <cmath>
 
 namespace math {
 
@@ -44,41 +43,45 @@ namespace math {
   csignal fft_rec(const csignal& input) {
     int size = input.size();
 
-    if (size == 1) {
+    if (size <= 1) {
       return input;
     } else {
       csignal odd;
       csignal even;
-      std::back_insert_iterator<csignal> odd_back_it(odd);
-      std::back_insert_iterator<csignal> even_back_it(even);
+      auto odd_back_it = std::back_inserter(odd);
+      auto even_back_it = std::back_inserter(even);
       bool insert_in_even = false;
 
       for (auto it = input.begin(); it != input.end(); ++it) {
         if (insert_in_even) {
-          *even_back_it++ = *it;
+          *(even_back_it++) = *it;
           insert_in_even = false;
         } else {
-          *odd_back_it++ = *it;
+          *(odd_back_it++) = *it;
           insert_in_even = true;
         }
       }
 
       csignal odd_fft = fft_rec(odd);
       csignal even_fft = fft_rec(even);
-      csignal res;
-      res.reserve(size);
+      csignal res(size, complex());
 
-      for (int k = 0; k<size/2; ++k) {
-        complex t = std::exp(complex(0, -2*pi()*k/size)) * odd[k];
-        res[k] = even[k] + t;
-        res[size/2+k] = even[k] - t;
+      for (int k=0; k<size/2; ++k) {
+        complex t = std::exp(complex(0, -2*pi()*k/size)) * odd_fft[k];
+        res[k] = even_fft[k] + t;
+        res[size/2+k] = even_fft[k] - t;
       }
       return res;
     }
   }
 
-  csignal fft(const csignal& input) {
-    int opt_size = 1 << (int)std::ceil(std::log(input.size())/std::log(2));
+  csignal fft(const csignal& input, int N=0) {
+    int opt_size;
+    if (N < input.size()) {
+      opt_size = 1 << (int)std::ceil(std::log(input.size())/std::log(2));
+    } else {
+      opt_size = 1 << (int)std::ceil(std::log(N)/std::log(2));
+    }
     csignal sig(input);
     for (int i=0; i<opt_size-input.size(); ++i) {
       sig.push_back(complex(0, 0));