#include "Arduino.h"
#include "arduinoFFT.h"
#include "ProfileTimer.h"

arduinoFFT FFT = arduinoFFT(); // Create FFT object

/* We then define the variables specific to the signal */

const uint16_t samples = 512; //This value MUST ALWAYS be a power of 2
const double signalFrequency = 1000;
const double samplingFrequency = 10000;
const uint8_t amplitude = 100;

double vReal[samples];
double vImag[samples];

unsigned long t1;

#define SCL_INDEX 0x00
#define SCL_TIME 0x01
#define SCL_FREQUENCY 0x02
#define SCL_PLOT 0x03

/* ====================================================
   =                                                  =
   ==================================================== */
void PrintVector(double *vData, uint16_t bufferSize, uint8_t scaleType)
{
   for (uint16_t i = 0; i < bufferSize; i++)
   {
      double abscissa;
      /* Print abscissa value */
      switch (scaleType)
      {
         case SCL_INDEX:
         abscissa = (i * 1.0);
          break;
      case SCL_TIME:
         abscissa = ((i * 1.0) / samplingFrequency);
          break;
      case SCL_FREQUENCY:
         abscissa = ((i * 1.0 * samplingFrequency) / samples);
      break;
      }
      Serial.print(abscissa, 4);
      Serial.print(" ");
      if(scaleType==SCL_FREQUENCY) {
        Serial.print("Hz");
        Serial.print(" ");
        Serial.println(vData[i], 4);
      }
   }
   Serial.println();
}

/* ====================================================
   =                                                  =
   ==================================================== */
void PrintExecTime(long elapsed)
{
   elapsed = micros()-elapsed;
   Serial.print("--> Execution (us): ");Serial.println(elapsed, DEC);
}


/* ====================================================
   =                                                  =
   ==================================================== */
void setup() {
  // put your setup code here, to run once:
   Serial.begin(115200);
   while(!Serial);
   Serial.println("Ready");
   Serial.print("size of vReal & vImag: ");
   Serial.println(sizeof(vReal)+sizeof(vImag));
   
   delay(5000);
}

/* ====================================================
   =                                                  =
   ==================================================== */
void loop()
{
   /* Build raw data */

   // Number of signal cycles that the sampling will read
   double cycles = (((samples-1) * signalFrequency) / samplingFrequency);
   
   for (uint16_t i = 0; i < samples; i++) 
   {
      /* Build data with positive and negative values*/
      vReal[i] = int8_t((amplitude * (sin((i * (twoPi * cycles)) / samples))) / 2.0);
   // vReal[i] = uint8_t((amplitude * (sin((i * (twoPi * cycles)) / samples) + 1.0)) / 2.0);
   
      /* Build data displaced on the Y axis to include only positive values*/
      /* Imaginary part must be zeroed in case of looping to avoid wrong calculations and overflows */
      vImag[i] = 0.0;
   }

   /* Print the results of the simulated sampling according to time */
   Serial.println("Data:");
   PrintVector(vReal, samples, SCL_TIME);

   /* Weigh data */
   //-------------------------------------------------------------------
   //t1 = micros();
   {
   ProfileTimer tx1 ("FFT Windowing");
   FFT.Windowing(vReal, samples, FFT_WIN_TYP_HAMMING, FFT_FORWARD);
   }
   //PrintExecTime(t1);
   
   Serial.println("Weighed data:");
   PrintVector(vReal, samples, SCL_TIME);
   
   //-------------------------------------------------------------------
   //t1 = micros();
   {
   ProfileTimer tx2 ("FFT Compute");
   FFT.Compute(vReal, vImag, samples, FFT_FORWARD); //Compute FFT
   }
   //PrintExecTime(t1);
   
   Serial.println("Computed Real values:");
   PrintVector(vReal, samples, SCL_INDEX);
   Serial.println("Computed Imaginary values:");
   PrintVector(vImag, samples, SCL_INDEX);
   
   //-------------------------------------------------------------------
   //t1 = micros();
   {
   ProfileTimer tx3 ("FFT Complex to Magnitude");
   FFT.ComplexToMagnitude(vReal, vImag, samples); // Compute magnitudes
   }
   //PrintExecTime(t1);
   Serial.println("Computed magnitudes:");
   PrintVector(vReal, (samples >> 1), SCL_FREQUENCY);
   double x;
   {
   ProfileTimer tx4 ("FFT Major Peak");
   x = FFT.MajorPeak(vReal, samples, samplingFrequency);
   }
   Serial.print("Major peak: ");
   Serial.println(x, 6);

   while(1);          /* Run Once */
   // delay(2000);    /* Repeat after delay */
}

/* ====================================================
   =                                                  =
   ==================================================== */