//Using ESP32 to process data with FFT (Fast-Fourier Transform)
//https://iot-kmutnb.github.io/blogs/esp32/esp32_fft/

#include "esp_dsp.h" // Include the  Espressif's ESP-DSP library.

uint32_t Fs   = 10000;  // Sampling frequency (Hz)
uint32_t freq = 1000;  // Signal frequency (Hz)

const uint32_t N = 1024;
float samples[N];    // Array of samples
float wind_cf[N];    // Array of window coefficients
float y_cf[2*N];     // Array of working complex values
float y_result[N/2]; // FFT  result

void setup() {
  Serial.begin(115200);
  Serial.print("\n\n\n"); 
  Serial.println("ESP32 - FFT Demo with ESP-DSP lib...");
  initFFT();
}

void loop() {
  createSamples(); // Create input samples.
  fft();           // Perform FFT.
  delay(5000);
}

// void createSamples() {
//   for (uint32_t i=0; i < N; i++) {
//     float noise = 0.0f;
//     //noise = (float)random(0, 100) / 1000.0;
//     samples[i] = sin(2*PI*freq*i/Fs)/2 + noise;
//   }
// }

void createSamples() {
  for (uint32_t i=0; i < N; i++) {
    float noise = 0.0f;
    noise = (float)random(0, 100) / 1000.0; // 0..0.1
    samples[i] = 1.0*sin(2*PI*(1*freq)*i/Fs)/2
               + 0.5*sin(2*PI*(3*freq)*i/Fs)/2
               + noise;
  }
}

void initFFT() {
    esp_err_t ret;
    // Initialize the ESP32-DSP radix-2 FFT routine.
    // Initialize the coefficients table for complex FFT calculation.
    ret = dsps_fft2r_init_fc32( NULL/*use internal buffer*/,
                                CONFIG_DSP_MAX_FFT_SIZE );
    if (ret != ESP_OK) {
      Serial.printf( "Not possible to initialize FFT. Error = %i", ret );
      return;
    }
    // Generate the coefficients of the Hann window.
    dsps_wind_hann_f32( wind_cf, N );
}

void fft() {
    // Convert two input vectors to one complex vector.
    for (int i=0 ; i < N ; i++) {
      y_cf[i*2 + 0] = samples[i] * wind_cf[i]; // Re[..]
      y_cf[i*2 + 1] = 0; // Im[..]
    }

    uint32_t start_cyc = dsp_get_cpu_cycle_count();
    // FFT
    // y_cf = Re[0], Im[0], … Re[N-1], Im[N-1] as input data.
    // The result of FFT will be stored to this array.
    dsps_fft2r_fc32( y_cf, N );
    // Bit reverse 
    dsps_bit_rev_fc32( y_cf, N );
    // Convert one complex vector to two complex vectors
    dsps_cplx2reC_fc32( y_cf, N );

    uint32_t stop_cyc = dsp_get_cpu_cycle_count();
    uint32_t cyc_diff = stop_cyc - start_cyc;
    uint32_t time_diff_us = cyc_diff/ESP.getCpuFreqMHz();

    float re, im;
    for ( uint32_t i=0 ; i < N/2; i++) {
      re = y_cf[i*2];
      im = y_cf[i*2 + 1];
      y_result[i] = 10 * log10f( (re*re + im*im)/N );
    }
    // Show spectrum in w x h window from 0..N/2 samples.
    Serial.println("FFT-based Magnitude Spectrum of Signal");
    dsps_view( y_result, N/2, 40/*width*/, 10/*height*/,
               -40/*min*/, 60/*max*/, '|');
    Serial.printf( "%lu-point FFT exec time: %lu cycles\n", N, cyc_diff );
    Serial.printf( "Time diff: %lu usec\n\n", time_diff_us );
    delay(2000);
}