#include <SPI.h>
#include <Adafruit_GFX.h>                             // graphics library for display
#include <Adafruit_SSD1306.h>                         // SSD1306 OLED display library
#include "arduinoFFT.h"



#ifdef __arm__
// should use uinstd.h to define sbrk but Due causes a conflict
extern "C" char* sbrk(int incr);
#else  // __ARM__
extern char *__brkval;
#endif  // __arm__


#define OLED_RESET 4                                  // OLED library likes to see this but it isn't used here with I2C
#define OLED_ADDR 0x3D                               // OLED I2C address
#define audioIn A0                                    // audio input port


#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

/*
#define BUTTON_PIN 2
volatile boolean toggleChannel = false;
volatile boolean sendWave = false;
unsigned long lastSampleTime = 0;
*/



arduinoFFT FFT = arduinoFFT(); /* Create FFT object */
/*
These values can be changed in order to evaluate the functions
*/

const uint16_t samples = 128; //This value MUST ALWAYS be a power of 2
const double samplingFrequency = 4000; //Hz, must be less than 10000 due to ADC

unsigned int sampling_period_us;
unsigned long microseconds;

/*
These are the input and output vectors
Input vectors receive computed results from FFT
*/
double vReal[samples];
double vImag[samples];
byte ylim = 60; 


#define SCL_INDEX 0x00          // constants to tell function what the print vector to do, doesnt change algorithm
#define SCL_TIME 0x01
#define SCL_FREQUENCY 0x02
#define SCL_PLOT 0x03



void setup() {
  // Open serial communications and wait for port to open:
  Serial.begin(9600);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB port only
  }

  /*
  pinMode(BUTTON_PIN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(BUTTON_PIN), toggle, FALLING);
  */
  
  sampling_period_us = round(1000000*(1.0/samplingFrequency));              // change based on our design 
  Serial.print("Sampling Period = ");
  Serial.println(sampling_period_us);




  printfreeMem(0);
 
 // initialize OLED display with address 0x3C for 128x64
  if (!display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR)) {
    Serial.println(F("SSD1306 allocation failed"));
    while (true);
  }
  
  delay(2000);         // wait for initializing

  display.clearDisplay();                             // blank the display
  printfreeMem(1);
  display.setTextSize(1);                             // configure text properties
  display.setTextColor(WHITE);
  analogReference(DEFAULT);  


  
   Serial.println("done!");
}




void loop(void) {
  printfreeMem(2);
  display.clearDisplay();                             // clear display
  display.setCursor(0, 0);                            // set cursor to top left corner of the display
  display.print("Audio Spectrum");                    // print a header on the top row of the display
  display.display();                                   // update the display


 printfreeMem(3);
  /*SAMPLING*/
  microseconds = micros();
  for(int i=0; i<samples; i++)
  {
      vReal[i] = analogRead(audioIn);       // reading from A0
      vImag[i] = 0;
      while(micros() - microseconds < sampling_period_us){
        //empty loop
      }
      microseconds += sampling_period_us;
  }
  /* Print the results of the sampling according to time */
  Serial.println("Data:");
  PrintVector(vReal, samples, SCL_TIME);
  FFT.Windowing(vReal, samples, FFT_WIN_TYP_HAMMING, FFT_FORWARD);	/* Weigh data */
  Serial.println("Weighed data:");
  printfreeMem(4);
  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:");
  printfreeMem(5);
  PrintVector(vReal, (samples >> 1), SCL_FREQUENCY);
  double x = FFT.MajorPeak(vReal, samples, samplingFrequency);
  Serial.println(x, 6); //Print out what frequency is the most dominant.

  display.clearDisplay();                             // clear display
  display.setCursor(0, 0);                            // set cursor to top left corner of the display

  for (byte i = 0; i < samples; i++) {
    int dat = sqrt(vReal[i] * vReal[i] + vImag[i] * vImag[i]);     // frequency magnitude is the square root of the sum of the squares of the real and imaginary parts of a vector
    display.drawLine(i * 2, ylim, i * 2, ylim - dat, WHITE); // draw bars for each frequency bin from 0 Hz to 4.5 KHz
  };

  display.display();  

  while(1);
}

//MEMORY LIMITATIONS???
//https://forum.arduino.cc/t/arduino-uno-ssd1306-allocation-failed-need-help-downsizing/604698/5
//https://cdn-learn.adafruit.com/downloads/pdf/memories-of-an-arduino.pdf
//https://www.instructables.com/Playing-Wave-file-using-arduino/
//https://simple-circuit.com/arduino-wave-audio-player-sd-card/


void printfreeMem(int index) {
   Serial.print("Freemem ");
   Serial.print(index);
   Serial.print(": ");
   Serial.println(freeMemory());
}


int freeMemory() {
  char top;
#ifdef __arm__
  return &top - reinterpret_cast<char*>(sbrk(0));
#elif defined(CORE_TEENSY) || (ARDUINO > 103 && ARDUINO != 151)
  return &top - __brkval;
#else  // __arm__
  return __brkval ? &top - __brkval : &top - __malloc_heap_start;
#endif  // __arm__
}

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();
}


// frequency on x and intensity on y