/* ESP8266/32 Audio Spectrum Analyser on an SSD1306/SH1106 Display
 * The MIT License (MIT) Copyright (c) 2017 by David Bird. 
 * The formulation and display of an AUdio Spectrum using an ESp8266 or ESP32 and SSD1306 or SH1106 OLED Display using a Fast Fourier Transform
 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files 
 * (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, 
 * publish, distribute, but not to use it commercially for profit making or to sub-license and/or to sell copies of the Software or to 
 * permit persons to whom the Software is furnished to do so, subject to the following conditions:  
 * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES 
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE 
 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 
 * See more at http://dsbird.org.uk 
*/

#include <Wire.h>

#include <arduinoFFT.h> // Standard Arduino FFT library 
arduinoFFT FFT = arduinoFFT();// https://github.com/kosme/arduinoFFT, in IDE, Sketch, Include Library, Manage Library, then search for FFT

#if defined(__AVR__)
  #include <Adafruit_GFX.h>
  #include <Adafruit_SSD1306.h>
  Adafruit_SSD1306 display = Adafruit_SSD1306(128, 64, &Wire,-1);
  #define DISPLAY_INIT      if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) \
                                Serial.println(F("SSD1306 allocation failed")); \
                              else \
                                { display.display(); delay(1000); display.setTextColor(WHITE);}
  #define DISPLAY_CLEAR     display.clearDisplay();
  #define DISPLAY_FILLRECT(x, y, w, h) \
                            display.fillRect(x, y, w, h, WHITE);
  #define DISPLAY_DRAWLINE(x1, y1, x2, y2)  \
                            display.drawLine(x1, y1, x2, y2, WHITE); 
  #define DISPLAY_DRAWHLINE(x, y, l) \
                            display.drawFastHLine(x, y, l, WHITE);                               
#else 
  #include "SH1106Wire.h"   // legacy: #include "SH1106.h"
  SH1106Wire display(0x3c, SDA, SCL);
  #define DISPLAY_INIT      display.init(); \
                            display.setFont(ArialMT_Plain_10); \
                            display.flipScreenVertically(); // Adjust to suit or remove
  #define DISPLAY_CLEAR     display.clear();              
  #define DISPLAY_FILLRECT(x, y, w, h) \
                            display.fillRect(x, y, w, h);    
  #define DISPLAY_DRAWLINE(x1, y1, x2, y2)  \
                            display.drawLine(x1, y1, x2, y2);    
  #define DISPLAY_DRAWHLINE(x, y, l) \
                            display.drawHorizontalLine(x, y, l);                                                                         
#endif


#include <MD_MAX72xx.h>
#define HARDWARE_TYPE  MD_MAX72XX::FC16_HW   // Set display type  so that  MD_MAX72xx library treets it  properly
#define MAX_DEVICES  4   // Total number display modules
#define  CLK_PIN  52 //13  // Clock pin to communicate with display
#define DATA_PIN  51 //11  //  Data pin to communicate with display
#define CS_PIN    53 //10  // Control pin to  communicate with display
MD_MAX72XX  mx = MD_MAX72XX(HARDWARE_TYPE, CS_PIN, MAX_DEVICES);   // display object

static const byte MX_BAR[]={0b00000000, 0b10000000, 0b11000000, 0b11100000, 0b11110000, 0b11111000, 0b11111100, 0b11111110, 0b11111111};

/////////////////////////////////////////////////////////////////////////
#define modePin 5
uint8_t mode=2, modes=4;

/////////////////////////////////////////////////////////////////////////
#define SAMPLES 128              // Must be a power of 2
#define SAMPLING_FREQUENCY 16000 // Hz, must be 40000 or less due to ADC conversion time. Determines maximum frequency that can be analysed by the FFT Fmax=sampleF/2.
#define amplitude 100            // Depending on your audio source level, you may need to increase this value

#define SIMULATE_BEGIN  20 //in Hz
#define SIMULATE_END    8000 //in Hz
#define SIMULATE_STEP   50 // in Hz

double simulateFrequency = 0;
unsigned int sampling_period_us;
unsigned long microseconds;
byte peak[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
double vReal[SAMPLES];
double vImag[SAMPLES];
unsigned long newTime, oldTime;
static const int wid=2;

double abscissa = 0;

#define yres 64
#define xres 32
uint8_t data_avgs[xres];


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

#define FREQUENCY_BANDS 33 //(SAMPLES/2) //14
#define BARWIDTH 3 //10
const uint8_t BARS = (FREQUENCY_BANDS - 1);
double coutoffFrequencies[FREQUENCY_BANDS];
float reference = log10(8.0), refmed=4.0;
int oldHeight[FREQUENCY_BANDS];
int oldMax[FREQUENCY_BANDS];
double maxInFreq;

/////////////////////////////////////////////////////////////////////////
void setup() {
  Serial.begin(115200);
  //Wire.begin(5,4); // SDA, SCL

  DISPLAY_INIT 
  //display.setRotation(0);
  //display.invertDisplay(false);  
  display.setTextSize(1);
  display.setTextColor(WHITE);

  //sampling_period_us = (1.0 / SAMPLING_FREQ ) * pow(10.0, 6);
  sampling_period_us = round(1000000 * (1.0 / SAMPLING_FREQUENCY));
  abscissa = (( ((SAMPLES/2)-1) * 1.0 * SAMPLING_FREQUENCY) / SAMPLES);

  Serial.println("starting");
  Serial.print("SDA="); Serial.print(SDA,DEC); Serial.print(" SCK="); Serial.println(SCK,DEC);  

  // Calculate cuttoff frequencies,meake a logarithmic scale base basePOt
  double basePot = pow(SAMPLING_FREQUENCY / 2.0, 1.0 / FREQUENCY_BANDS);
  coutoffFrequencies[0] = basePot;

  Serial.println("coutoffFrequencies");
  for (int i = 1 ; i < FREQUENCY_BANDS; i++ ) {
    coutoffFrequencies[i] = basePot * coutoffFrequencies[i - 1];
    Serial.println((int)coutoffFrequencies[i]);
  }


    mx.begin();           // initialize display
    mx.control(MD_MAX72XX::INTENSITY, 4); // set LED intensity

  pinMode(modePin, INPUT_PULLUP);

}

void loop(){
  
  if (mode==0 || mode==2) readSimulate();
  else if (mode==1 || mode==3) readReal();
  
  if (mode==0 || mode==1) plotFrequency();
  else if (mode==2 || mode==3) plotFrequency2();

  if (digitalRead(modePin) == LOW) {
    mode++;
    if (mode>=modes) mode=0;
  }

}

void readReal() {
  for (int i = 0; i < SAMPLES; i++) {
    newTime = micros()-oldTime;
    oldTime = newTime;
    vReal[i] = analogRead(A0); // A conversion takes about 1uS on an ESP32
    vImag[i] = 0;
    while (micros() < (newTime + sampling_period_us)) {
       // do nothing to wait 
        }
  }

  FFT.Windowing(vReal, SAMPLES, FFT_WIN_TYP_HAMMING, FFT_FORWARD);
  FFT.Compute(vReal, vImag, SAMPLES, FFT_FORWARD);
  FFT.ComplexToMagnitude(vReal, vImag, SAMPLES);  
}

void readSimulate() {
   if (simulateFrequency==0) 
    simulateFrequency=SIMULATE_BEGIN;
   else {
    simulateFrequency+=SIMULATE_STEP;
    if (simulateFrequency>SIMULATE_END) 
      simulateFrequency=SIMULATE_BEGIN;
   }
   // Number of signal cycles that the sampling will read
   double cycles = (((SAMPLES-1) * simulateFrequency) / SAMPLING_FREQUENCY);
   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;
   }

  FFT.DCRemoval();
  FFT.Windowing(vReal, SAMPLES, FFT_WIN_TYP_HAMMING, FFT_FORWARD);
  FFT.Compute(vReal, vImag, SAMPLES, FFT_FORWARD);
  FFT.ComplexToMagnitude(vReal, vImag, SAMPLES);
    delay(100);
}

void plotFrequency() {
    DISPLAY_CLEAR
    display.print( (int)abscissa, DEC);display.print("Hz");

    for (uint8_t i = 1; i < (SAMPLES / 2); i++) {      
      //if (vReal[i]>1000) 
      {
        Serial.print( (int) ((i * 1.0 * SAMPLING_FREQUENCY) / SAMPLES), DEC );
        Serial.print("Hz ");
        int dsize = (int)vReal[i];
        Serial.print(dsize, DEC);
        dsize=dsize/amplitude;
        Serial.print(" ");
        Serial.println(dsize, DEC);
        if (dsize>50) dsize=50;
        DISPLAY_FILLRECT((wid+1)*i, 64-dsize, wid, 64);
      }
    }
    display.display();
}

void plotFrequency2() {
    DISPLAY_CLEAR
    double median[FREQUENCY_BANDS];
    double max[FREQUENCY_BANDS];
    int index = 0;
    double hzPerSample = (1.0 * SAMPLING_FREQUENCY) / SAMPLES; //
    double hz = 0;
    double maxinband = 0;
    double sum = 0;
    int count = 0;
    for (int i = 2; i < (SAMPLES / 2) ; i++) {
      count++;
      sum += vReal[i];
      if (vReal[i] >  max[index] ) {
        max[index] = vReal[i];
      }
      if (hz > coutoffFrequencies[index]) {
        median[index] = sum / count;
        sum = 0.0;
        count = 0;
        index++;
        max[index] = 0;
        median[index]  = 0;
      }
      hz += hzPerSample;
    }
    // calculate median and maximum per frequency band
    if ( sum > 0.0) {
      median[index] =  sum / count;
      if (median[index] > maxinband) {
        maxinband = median[index];
      }
    }

    int bar = 0;

    //for (int i = FREQUENCY_BANDS - 1; i >= 3; i--) {
    for (int i = 1; i <= BARS; i++) {
      int newHeight = 0;
      int newMax = 0;
      // calculate actual decibels
      if (median[i] > 0 && max[i] > 0 ) {
        newHeight = refmed * (log10(median[i] ) - reference);
        newMax = refmed * (log10(max[i] ) - reference);
      }

      // adjust minimum and maximum levels
      if (newHeight < 0 ||  newMax < 0) {
        newHeight = 1;
        newMax = 1;
      }
      if (newHeight >= SCREEN_HEIGHT - 2) {
        newHeight = SCREEN_HEIGHT - 3;
      }
      if (newMax >= SCREEN_HEIGHT - 2) {
        newMax = SCREEN_HEIGHT - 3;
      }

      int barX = bar * (BARWIDTH + 1);
      
      /*// remove old level median
      if (oldHeight[i] > newHeight) {
        DISPLAY_FILLRECT(barX, newHeight + 1, BARWIDTH, oldHeight[i]);
      }
      // remove old max level
      if ( oldMax[i] > newHeight) {
        for (int j = oldMax[i]; j > newHeight; j -= 2) {
          DISPLAY_DRAWHLINE(barX , j, BARWIDTH);
        }
      }
      // paint new max level
      for (int j = newMax; j > newHeight; j -= 2) {
        DISPLAY_DRAWHLINE(barX , j, BARWIDTH);
      }
      */
      // paint new level median
      DISPLAY_FILLRECT(barX , 1, BARWIDTH, newHeight*BARWIDTH );

      byte v = newHeight;
      if (v>8) v=8;
      mx.setColumn((32-i), MX_BAR[ v ] ); 
      
      //DISPLAY_FILLRECT(barX , 64-newHeight, BARWIDTH, 64);

      oldMax[i] = newMax;
      oldHeight[i] = newHeight;
      bar++;
    }
    //DISPLAY_DRAWHLINE(0 , SCREEN_HEIGHT - 1, SCREEN_WIDTH);
    display.setTextSize(0);
    display.setTextColor(WHITE);    
    display.setCursor(0,55);
    display.print((int)simulateFrequency,DEC);display.println(" Hz");

    display.display();
}


void original() {
   Serial.println("loop"); 
  DISPLAY_CLEAR
  //display.drawString(0,0,"0.1 0.2 0.5 1K  2K  4K  8K");
  
  for (int i = 0; i < SAMPLES; i++) {
    newTime = micros()-oldTime;
    oldTime = newTime;
    vReal[i] = analogRead(A0); // A conversion takes about 1uS on an ESP32
    //Serial.println(vReal[i]);
    vImag[i] = 0;
    while (micros() < (newTime + sampling_period_us)) {
       // do nothing to wait 
        }
  }
  FFT.Windowing(vReal, SAMPLES, FFT_WIN_TYP_HAMMING, FFT_FORWARD);
  FFT.Compute(vReal, vImag, SAMPLES, FFT_FORWARD);
  FFT.ComplexToMagnitude(vReal, vImag, SAMPLES);

  rearrangeTo32bands();

  /*
  for (int i = 2; i < (SAMPLES/2); i++){ // Don't use sample 0 and only first SAMPLES/2 are usable. Each array eleement represents a frequency and its value the amplitude.
    if (vReal[i] > 2000) { // Add a crude noise filter, 10 x amplitude or more
      if (i<=2 )             displayBand(0,(int)vReal[i]/amplitude); // 125Hz
      if (i >3   && i<=5 )   displayBand(1,(int)vReal[i]/amplitude); // 250Hz
      if (i >5   && i<=7 )   displayBand(2,(int)vReal[i]/amplitude); // 500Hz
      if (i >7   && i<=15 )  displayBand(3,(int)vReal[i]/amplitude); // 1000Hz
      if (i >15  && i<=30 )  displayBand(4,(int)vReal[i]/amplitude); // 2000Hz
      if (i >30  && i<=53 )  displayBand(5,(int)vReal[i]/amplitude); // 4000Hz
      if (i >53  && i<=200 ) displayBand(6,(int)vReal[i]/amplitude); // 8000Hz
      if (i >200           ) displayBand(7,(int)vReal[i]/amplitude); // 16000Hz      
    }
    //for (byte band = 0; band <= 6; band++) display.drawHorizontalLine(18*band,64-peak[band],14);
  }
  Serial.print("\n");
  if (millis()%4 == 0) {for (byte band = 0; band <= 6; band++) {if (peak[band] > 0) peak[band] -= 1;}} // Decay the peak
*/  
/*
  int dsize=0;
  for (int i = 2; i < (SAMPLES/2); i++){ // Don't use sample 0 and only first SAMPLES/2 are usable. Each array eleement represents a frequency and its value the amplitude.
    if (vReal[i] > 1000) { // Add a crude noise filter, 10 x amplitude or more
      dsize=(int)vReal[i]/amplitude;

      if (SAMPLES==512) {
            if (i<=2 )  displayBand1(0,dsize, wid); // 64 Hz
        else if (i >2 && i<=3 )  displayBand1(1,dsize, wid); // 128 Hz
        else if (i >3 && i<=4 )  displayBand1(2,dsize, wid); // 192 Hz
        else if (i >4 && i<=5 )  displayBand1(3,dsize, wid); // 256 Hz
        else if (i >5 && i<=6 )  displayBand1(4,dsize, wid); // 342 Hz
        else if (i >6 && i<=8 )  displayBand1(5,dsize, wid); // 428 Hz
        else if (i >8 && i<=10 )  displayBand1(6,dsize, wid); // 512 Hz
        else if (i >10 && i<=13 )  displayBand1(7,dsize, wid); // 640 Hz
        else if (i >13 && i<=15 )  displayBand1(8,dsize, wid); // 768 Hz
        else if (i >15 && i<=18 )  displayBand1(9,dsize, wid); // 896 Hz
        else if (i >18 && i<=20 )  displayBand1(10,dsize, wid); // 1024 Hz
        else if (i >20 && i<=24 )  displayBand1(11,dsize, wid); // 1195 Hz
        else if (i >24 && i<=27 )  displayBand1(12,dsize, wid); // 1365 Hz
        else if (i >27 && i<=30 )  displayBand1(13,dsize, wid); // 1536 Hz
        else if (i >30 && i<=34 )  displayBand1(14,dsize, wid); // 1707 Hz
        else if (i >34 && i<=37 )  displayBand1(15,dsize, wid); // 1877 Hz
        else if (i >37 && i<=40 )  displayBand1(16,dsize, wid); // 2048 Hz
        else if (i >40 && i<=47 )  displayBand1(17,dsize, wid); // 2389 Hz
        else if (i >47 && i<=54 )  displayBand1(18,dsize, wid); // 2731 Hz
        else if (i >54 && i<=61 )  displayBand1(19,dsize, wid); // 3072 Hz
        else if (i >61 && i<=67 )  displayBand1(20,dsize, wid); // 3413 Hz
        else if (i >67 && i<=74 )  displayBand1(21,dsize, wid); // 3755 Hz
        else if (i >74 && i<=81 )  displayBand1(22,dsize, wid); // 4096 Hz
        else if (i >81 && i<=94 )  displayBand1(23,dsize, wid); // 4779 Hz
        else if (i >94 && i<=108 )  displayBand1(24,dsize, wid); // 5461 Hz
        else if (i >108 && i<=121 )  displayBand1(25,dsize, wid); // 6144 Hz
        else if (i >121 && i<=200 )  displayBand1(26,dsize, wid); // 6827 Hz
        // else if (i > && i<= )  displayBand1(27,dsize, wid); // 7509 Hz
        // else if (i > && i<= )  displayBand1(28,dsize, wid); // 8192 Hz
        // else if (i > && i<= )  displayBand1(29,dsize, wid); // 10923 Hz
        // else if (i > && i<= )  displayBand1(30,dsize, wid); // 13653 Hz
        // else if (i > && i<= )  displayBand1(31,dsize, wid); // 16384 Hz
        
        //Serial.println(dsize,DEC); 
      }
    }
    //for (byte band = 0; band <= 7; band++) display.drawHorizontalLine((wid+1)*band,64-peak[band],wid);
  }


  for (int i = 0, x=0; i < (SAMPLES/2); i++, x++){
    if (vReal[i] > 2000 && x<=128) {            
      //if (i>=ri && i<=rf) 
      {
        int sz=(int)vReal[i]/amplitude;
        if (sz>50) sz=50;
        //display.fillRect(x, 64-sz, x, 64 );      
        DISPLAY_DRAWLINE(x, 0, x, sz ); 
        Serial.print(i,DEC); Serial.print(" "); Serial.println(sz,DEC);
      }
    }
    // if (i>=r) {
    //   r*=1.2;
    //   x+=2;
    //   Serial.print(r); Serial.print(" ");
    // }
  }
  Serial.print("\n");
*/


  display.display();
}

void displayBand(int band, int dsize){
  //Serial.print(dsize); Serial.print(" ");
  int dmax = 50;
  if (dsize > dmax) dsize = dmax;
  //if (band == 7) display.drawHorizontalLine(18*6,0, 14);
  //for (int s = 0; s <= dsize; s=s+2){display.drawHorizontalLine(18*band,64-s, 14);}
  DISPLAY_FILLRECT(18*band, 0, 14, dsize);
  if (dsize > peak[band]) {peak[band] = dsize;}
}

void displayBand1(int band, int dsize, int wid){
  //Serial.print(dsize); Serial.print(" ");
  int dmax = 50;
  if (dsize > dmax) dsize = dmax;
  //if (band == 7) display.drawHorizontalLine((wid+1)*6,0, wid);
  //for (int s = 0; s <= dsize; s=s+2){display.drawHorizontalLine(18*band,64-s, 14);}
  DISPLAY_FILLRECT((wid+1)*band, 64-dsize, wid, dsize+2);
  if (dsize > peak[band]) {peak[band] = dsize;}
}

void rearrangeTo32bands() {
    // ++ re-arrange FFT result to match with no. of columns on display ( xres )
    int step = (SAMPLES/2)/xres; 
    int c=0;
    for(int i=0; i<(SAMPLES/2); i+=step)  
    {
      data_avgs[c] = 0;
      for (int k=0 ; k< step ; k++) {
          if (vReal[i+k]>1000) {
            data_avgs[c] = data_avgs[c] + vReal[i+k];
          }
      }
      data_avgs[c] = data_avgs[c]/step; 
      c++;
    }
    // -- re-arrange FFT result to match with no. of columns on display ( xres )

    // ++ send to display according measured value 
    for(int i=0; i<xres; i++)
    {
      data_avgs[i] = constrain(data_avgs[i],0,64);            // set max & min values for buckets
      //data_avgs[i] = map(data_avgs[i], 0, 64, 0, yres);        // remap averaged values to yres
      uint8_t yvalue=data_avgs[i];

      //peaks[i] = peaks[i]-1;    // decay by one light
      //if (yvalue > peaks[i]) 
      //    peaks[i] = yvalue ;
      //yvalue = peaks[i];    
      //displayvalue=MY_ARRAY[yvalue];
      //displaycolumn=31-i;
      //mx.setColumn(displaycolumn, displayvalue);              // for left to right
      DISPLAY_FILLRECT((wid+1)*i, 64-yvalue, wid, yvalue);
    }
     // -- send to display according measured value          
}