#include <Arduino.h>
#include "driver/adc.h"

#include <Wire.h>
#include <U8g2lib.h>
U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/U8X8_PIN_NONE);
//U8G2_SH1106_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/U8X8_PIN_NONE);



#define pinADC 34



float sampleFrequency;

const int bufferSize = 4096;
uint8_t rawData[bufferSize];  // Buffer for ADC capture

int bufferIndx = 0;

void sampling() {

  bufferIndx = 0;

  unsigned long samplingTime = micros();
  while (bufferIndx < bufferSize) {
    if (samplingTime <= micros()) {
      samplingTime = micros() + 0;  //sampleperiod in micros
      rawData[bufferIndx++] = adc1_get_raw(ADC1_CHANNEL_6) >> 4;  // direkter Rohwert (0–4095)
    }
  }
}

void setup() {
  Wire.begin(21, 22);
  Serial.begin(115200);
  Serial.println("  ");
  pinMode(23, OUTPUT);

  u8g2.initDisplay();
  u8g2.setContrast(30);
  u8g2.setFlipMode(2);
  u8g2.setPowerSave(0);

  u8g2.clearBuffer();
  u8g2.drawRFrame(0, 0, 128, 64, 5);
  u8g2.setFont(u8g2_font_t0_22b_mf);
  u8g2.drawStr(20, 38, "  HELLO");
  u8g2.sendBuffer();

  adc1_config_width(ADC_WIDTH_BIT_12); // 0 - 4095 range
  adc1_config_channel_atten(ADC1_CHANNEL_6, ADC_ATTEN_DB_11); //0.0 – 3.9 V

  delay(2000);
}





void loop() {

  unsigned long elapse = micros();

  sampling();

  elapse = micros() - elapse;

  sampleFrequency = 1e6 * bufferSize / elapse;

  digitalWrite(23, HIGH);

  float frequency = findFrequency(rawData, bufferSize, sampleFrequency);
  Serial.print(String(frequency, 2) + "Hz\t" + pitch(frequency) + "\t" + String(deviation(frequency)) + "%\t");
  float test = 1e6 * bufferSize / elapse;
  Serial.println(test);

  static char strf[20];
  static char strq[99];

  u8g2.clearBuffer();
  u8g2.drawRFrame(0, 0, 128, 64, 5);
  u8g2.setFont(u8g2_font_t0_22b_mf);
  snprintf(strq, 99, "%s%s", dtostrf(frequency, 0, 2, strf), "Hz");
  u8g2.drawStr((128 - u8g2.getUTF8Width(strq)) / 2, 18, strq);
  snprintf(strq, 99, "%s", pitch(frequency));
  u8g2.drawStr((128 - u8g2.getUTF8Width(strq)) / 2, 38, strq);
  snprintf(strq, 99, "%s%s", dtostrf(deviation(frequency), 0, 2, strf), "%");
  u8g2.drawStr((128 - u8g2.getUTF8Width(strq)) / 2, 58, strq);
  u8g2.sendBuffer();

  digitalWrite(23, LOW);
}

float findFrequency(uint8_t* rawData, int bufferSize, float sampleFrequency) {

  // Calculate mean to remove DC offset
  long meanSum = 0;
  for (int k = 0; k < bufferSize; k++) {
    meanSum += rawData[k];
  }
  uint8_t mean = meanSum / bufferSize;

  // Autocorrelation
  long twoPreviousSum = 0;
  long previousSum = 0;
  long currentSum = 0;
  long threshold = 0;
  int pdState = 0;
  float period = 0;

  for (int i = 0; i < bufferSize && (pdState != 3); i++) {
    // Autocorrelation
    twoPreviousSum = previousSum;
    previousSum = currentSum;
    currentSum = 0;
    for (int k = 0; k < bufferSize - i; k++) {
      currentSum += (rawData[k] - mean) * (rawData[k + i] - mean);
    }
    // Peak detection
    switch (pdState) {
      case 0:  // Set threshold based on zero lag autocorrelation
        threshold = currentSum / 2;
        pdState = 1;
        break;
      case 1:  // Look for over threshold and increasing
        if ((currentSum > threshold) && (currentSum - previousSum) > 0)
          pdState = 2;
        break;
      case 2:  // Look for decreasing (past peak over threshold)
        if ((currentSum - previousSum) <= 0) {
          // quadratic interpolation
          float interpolationValue = 0.5 * (currentSum - twoPreviousSum) / (2 * previousSum - twoPreviousSum - currentSum);
          period = i - 1 + interpolationValue;
          pdState = 3;
        }
        break;
      default:
        pdState = 3;
        break;
    }
  }

  if (period <= 0) return 0;
  float frequency = sampleFrequency / period;
  if (threshold < 100 || frequency > 10000) return 0;
  return frequency;
}

char* pitch(float frequency) {

  static const char* pitch1[] = {
    "G#", "A", "A#", "B", "C", "C#", "D", "D#", "E", "F", "F#", "G"
  };
  static const char* pitch2[] = {
    "Ab", "A", "Bb", "B", "C", "Db", "D", "Eb", "E", "F", "Gb", "G"
  };

  if (frequency <= 0.0) return "no pitch";

  float n = 49 + 12 * log(frequency / 440.0) / log(2);

  int notenr;
  int octave;

  if (n < 0) {
    notenr = 11 + ((int)(n + 0.5) % 12);
    octave = ((int)(n + 0.5) - 4) / 12;
  } else {
    notenr = (int)(n + 0.5) % 12;
    octave = ((int)(n + 0.5) + 8) / 12;
  }
  static char MMM[99];

  switch (notenr) {
    case 1:
    case 3:
    case 4:
    case 6:
    case 8:
    case 9:
    case 11:
      snprintf(MMM, 99, "%s%d", pitch1[notenr], octave);
      return MMM;

    case 0:
    case 2:
    case 5:
    case 7:
    case 10:
      snprintf(MMM, 99, "%s%d/%s%d", pitch1[notenr], octave, pitch2[notenr], octave);
      return MMM;
    default:
      return "error";
  }
}

float deviation(float frequency) {

  if (frequency <= 0.0) return 0;

  float n = 49 + 12 * log(frequency / 440.0) / log(2);

  if (n >= 0) {
    return 100 * (n - (int)(n + 0.5));
  } else {
    return 100 * (n - (int)(n - 0.5));
  }
}