#include <Wire.h>

#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
Adafruit_SSD1306 display = Adafruit_SSD1306(128, 64, &Wire,-1);


#include "arduinoFFT.h"
arduinoFFT FFT = arduinoFFT(); // Create FFT object We then define the variables specific to the signal.

const uint16_t samples = 64; //This value MUST ALWAYS be a power of 2
double signalFrequency = 0;
const double samplingFrequency = 16000;
const uint8_t amplitude = 100;

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


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


void setup() {
   Serial.begin(115200);
   while(!Serial);
   Serial.println("Ready");

   if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) 
    Serial.println(F("SSD1306 allocation failed")); 
  else 
    { display.display(); delay(1000);}

}

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

   if (signalFrequency==0) 
    signalFrequency=20;
   else {
    signalFrequency+=100;
    if (signalFrequency>20000) 
      signalFrequency=20;
   }

   // 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 */
   FFT.Windowing(vReal, samples, FFT_WIN_TYP_HAMMING, FFT_FORWARD);
  //  Serial.println("Weighed data:");
  //  PrintVector(vReal, samples, SCL_TIME);
   FFT.Compute(vReal, vImag, samples, FFT_FORWARD); //Compute FFT
  //  Serial.println("Computed Real values:");
  //  PrintVector(vReal, samples, SCL_INDEX);
  //  Serial.println("Computed Imaginary values:");
  //  PrintVector(vImag, samples, SCL_INDEX);
   FFT.ComplexToMagnitude(vReal, vImag, samples); // Compute magnitudes
   Serial.println("Computed magnitudes:");
   PrintVector(vReal, (samples >> 1), SCL_FREQUENCY);
   double x = FFT.MajorPeak(vReal, samples, samplingFrequency);
   Serial.println(x, 6);
   //while(1);          /* Run Once */


    spectrum();


    delay(100);    /* Repeat after delay */
}

void spectrum() {
    display.clearDisplay();
    for (uint16_t i = 0; i < (samples >> 1); i++) {
      display.fillRect(i, 64-((int)vReal[i]/amplitude), 1, 64, WHITE);
    }
    display.display();
}

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, 6);
   if(scaleType==SCL_FREQUENCY)
      Serial.print("Hz");
      Serial.print(" ");
      Serial.println(vData[i], 4);
   }
   Serial.println();
}