#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__)
  #define CLK_PIN   13  // Clock pin to communicate with display
  #define DATA_PIN  11  //  Data pin to communicate with display
  #define CS_PIN    10  // Control pin to  communicate with display                            
#else
  #define CLK_PIN   4
  #define DATA_PIN  6
  #define CS_PIN    7                                                                                                 
#endif

#include <MD_MAX72xx.h>
//#include <SPI.h>
//#define HARDWARE_TYPE  MD_MAX72XX::FC16_HW   // Set display type  so that  MD_MAX72xx library treets it  properly
 #define HARDWARE_TYPE MD_MAX72XX::PAROLA_HW
#define MAX_DEVICES  4   // Total number display modules
#define CHAR_SPACING 1
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=0, 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   100 // 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 8
#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(40.0), refmed=20.0;
int oldHeight[FREQUENCY_BANDS];
int oldMax[FREQUENCY_BANDS];
double maxInFreq;

int previousState = LOW;   // the previous reading from the input pin
unsigned long lastDebounceTime = 0;  // the last time the output pin was toggled
unsigned long debounceDelay = 50; 

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

    ADCSRA = 0b11100101;      // set ADC to free running mode and set pre-scalar to 32 (0xe5)
    ADMUX = 0b00000000;       // use pin A0 and external voltage reference

  //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);   
  pinMode(modePin, INPUT);

}

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

  modeChange();

}

void readReal() {
  for (int i = 0; i < SAMPLES; i++) {
    newTime = micros()-oldTime;
    oldTime = newTime;

    while(!(ADCSRA & 0x10));        // wait for ADC to complete current conversion ie ADIF bit set
      ADCSRA = 0b11110101 ;               // clear ADIF bit so that ADC can do next operation (0xf5)
    vReal[i]=ADC; 
    //Serial.println((int)vReal[i]);
    //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;
   }
   double cycles = (((SAMPLES-1) * simulateFrequency) / SAMPLING_FREQUENCY);
   for (uint16_t i = 0; i < SAMPLES; i++)
   {
       vReal[i] = int8_t((amplitude * (sin((i * (twoPi * cycles)) / SAMPLES))) / 2.0);
      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);
}

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

void plotFrequency2() {
    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);

      byte v = newHeight*yres/40;
      if (v>8) v=8;
      mx.setColumn((32-i), MX_BAR[ v ] ); 
      
     // Serial.print(newHeight,DEC);Serial.print(" ");
     // Serial.print(v,DEC);
     // Serial.print("\n");
      //DISPLAY_FILLRECT(barX , 64-newHeight, BARWIDTH, 64);

      oldMax[i] = newMax;
      oldHeight[i] = newHeight;
      bar++;
    }
}

void modeChange() {
  int reading = digitalRead(modePin);
  if (reading == HIGH && previousState == LOW && millis() - lastDebounceTime > debounceDelay) 
  {
    mode++;
    if (mode>=modes) mode=0;
    switch(mode) {
      case 0: printText(0,MAX_DEVICES-1,"MODE 0"); break;
      case 1: printText(0,MAX_DEVICES-1,"MODE 1"); break;
      case 2: printText(0,MAX_DEVICES-1,"MODE 2"); break;
      case 3: printText(0,MAX_DEVICES-1,"MODE 3"); break;
    }
    Serial.print("modeChange to ");Serial.println(mode,DEC);
    delay(1000);
    mx.clear();
    lastDebounceTime = millis();
  }
  previousState = reading;
}

void printText(uint8_t modStart, uint8_t modEnd, char *pMsg)
// Print the text string to the LED matrix modules specified.
// Message area is padded with blank columns after printing.
{
  uint8_t   state = 0;
  uint8_t	  curLen;
  uint16_t  showLen;
  uint8_t	  cBuf[8];
  int16_t   col = ((modEnd + 1) * COL_SIZE) - 1;

  mx.control(modStart, modEnd, MD_MAX72XX::UPDATE, MD_MAX72XX::OFF);

  do     // finite state machine to print the characters in the space available
  {
    switch(state)
    {
      case 0:	// Load the next character from the font table
        // if we reached end of message, reset the message pointer
        if (*pMsg == '\0')
        {
          showLen = col - (modEnd * COL_SIZE);  // padding characters
          state = 2;
          break;
        }

        // retrieve the next character form the font file
        showLen = mx.getChar(*pMsg++, sizeof(cBuf)/sizeof(cBuf[0]), cBuf);
        curLen = 0;
        state++;
        // !! deliberately fall through to next state to start displaying

      case 1:	// display the next part of the character
        mx.setColumn(col--, cBuf[curLen++]);

        // done with font character, now display the space between chars
        if (curLen == showLen)
        {
          showLen = CHAR_SPACING;
          state = 2;
        }
        break;

      case 2: // initialize state for displaying empty columns
        curLen = 0;
        state++;
        // fall through

      case 3:	// display inter-character spacing or end of message padding (blank columns)
        mx.setColumn(col--, 0);
        curLen++;
        if (curLen == showLen)
          state = 0;
        break;

      default:
        col = -1;   // this definitely ends the do loop
    }
  } while (col >= (modStart * COL_SIZE));

  mx.control(modStart, modEnd, MD_MAX72XX::UPDATE, MD_MAX72XX::ON);
}